chip updates: trying CMA for the first time, with the cha-cha [updates]
Last time we talked about chip's next steps we talked about this: https://www.adim.io/post/chip-updates-next-steps-but-not-actually-steps-updates the next step in our development is to use the technique we developed (CMA) to (a) estimate the position of the CoG and allow the robot to move one leg around without falling over by moving the other three to keep the CoG within bounds.
What we don't want to do is do this as a feed-back controller where we send the command and it gets altered in another node like the previous approach with the IMU. What we want is for CMA to be more of a predictive control algorithm, we figure out where we want to move the leg, we run CMA on it, and we then shove that into the trajectory file.
''' IN - is the input vector to be altered with CMA leg_in - is the leg/legs being moved (to not alter) do_IK - is a boolean that tells the method if the command entered was a raw CMD or if we need to do IK NOTE: method only accepts raw commands or XYZ foot positions ''' def CMA(float IN, String leg_in, bool raw): #if the command is in XYZ format we need to do IK first if(not raw): global IN IN = do_IK(IN) #then we get the shoulder theta of all four legs FLL = toTHETA(IN[0:3]) FRL = toTHETA(IN[3:6]) BLL = toTHETA(IN[6:9]) BRL = toTHETA(IN[9:]) #now we get to actual CMA num_legs = len(leg_in) #number of moving legs #calculate total motion of the CoG CMA_OUT = 5/4*cos(FLL)+5/4*cos(FRL)+5/4*cos(BLL)+5/4*cos(BRL) #calculate how much each leg must move to ensure stability dx = -CMA_OUT/num_legs #we need to reverse the coord frames ''' THEN WE ONLY ADJUST THE LEGS X-POS FOR THOSE WHO ARE NOT IN THE LEG_IN VECTOR ''' return new_CMD
So CoG Motion Analysis isn't going to be exactly like the above but it'll be pretty close. We also need to determine a "start" position that's stable and things like that, we need to figure out how to implement all of this, and it might be worth writing a CMA class that has a default "center position" where the weight is exactly balanced on the front and back of the robot. That's the true center of CMA because if we take that as center, then we can derive for a dTheta what is dX of the CoG and then we can determine what the dX of each foot must be. That's the idea here!
So of course, we need to start by finding that center position, equal weight on the front and back legs. That'll happen when the estimated CoG is halfway between the feet. We can also determine the set of commands where this occurs. If we ignore the y-position of the robot we can do this for any theta. Right? But we're going to choose one for simplicity.
f(theta) = (5 cm) * cos(theta) + (28 cm)
Now if we take theta = pi/4...
f(pi/4)=(5 cm)* cos(pi/4)+(28 cm) = (5)*sqrt(2)/2+28 = 31.5355339059 cm
Now... we've already set a very specific width between the feet of the robot at the moment and that's 22in = 55.88~56cm=0.56m.
Now... what's next? Let's draw some things.
So if the length of the whole robot is 63.5cm, which means the half-way from the front of the robot is at 31.75 cm. Which means the CoG at theta=45 deg is 0.25 in infront of the center which means we need to shift all the feet 0.25 in froward from their zero positions to center the CoG in the foot square. That's <0.01 meters which means if we say 0.0 for the x-position in the foot coordinate frame, that's roughly a good center for CMA at 45deg.
So what we need to do is figure out what y-gives us x=0.0 at 45 deg so we can use this as the center for CMA. We'll need the LEG model for this.
So now we've calculated the CENTER for CMA. Here it is in raw commands:
[12.5, 0.0, 28.8, 12.5, 0.0, 28.8, 12.5, 0.0, 28.8, 12.5, 0.0, 28.8]
That's theta_1 = 45deg, theta_2 = 0deg, and theta_3 ~= 103deg or something. That's the center of CMA that allows us to have equal weight on the front and back feet in RAW CMDS. We're going to call this the equal point. So now we can come up with a new CoG motion model that shifts the model by a phase shift of 45deg.
df(theta)=(5 cm)/4* cos(theta - 45 deg)
^ that's for each leg. That's important because now we can use the position we calculated above as the center of the platform and we can say how far the CoG has shifted from where it currently is at the center of the platform.
So the next steps are to set this set of commands as the default position of the platform after a stand-up or a sit-down, and then we can test if this theory works.
if this theory works, what we can then do is create a function that for a given desired starting theta, it calculates the CMA center position.
But that's for later and also very not necessary. But here's the deal, this is what we're going to try next.