chip updates: trying out CMA finally [updates]
So now that we've gotten past all the wrong power supply and the gearbox issues and etc, I hope it's time to try CMA out once and for all. We're going to try it on like one of the legs just so we can test its functionality. We're going to do this before we get anywhere close to walking. And we'll use the FRL for now. Then we can test the other three legs after it's working on that. Quick test to see if everything is working...
The first thing we need to do is like: https://www.adim.io/post/chip-updates-trying-cma-for-the-first-time-with-the-cha-cha-updates says, we need to find the zero position of the CoG so we can keep it in the triangle. We came up with a set of CMDS for this and we're going to try it out now. DEFAULTS.CMA_CENTER
Seems like from the looks of it, these foot positions are not at the 0.0 in the x-direction they need to be to balance the platform weight. Let's see if we can get them there, increase the knee command. From 28.8 to 30.8 to account for SLOP (which seems to be about a few rotations). And let's give the back two-more rotations than the front 32.8! Actually maybe 31.8...
So at this point, two positive rotations of the knee would put the weight on the front legs, and two negative rotations of the knee would put the weight on the back legs. Then all we need to do to move any leg around is to use CMA and move the other feet as this leg is moving. It might be unnatural but it also might be effective.
The idea is to use this feed-forward CMA technique for control and then use the IMU after for feedback if we need it and when we get to that. For now, let's just do CMA. We're going to need to write a CMA class with a CMA object and everything later to make this thing work but for now let's do everything in test mode on one leg. We have to think of a good way to do this as a test...
''' This is a CMA class that gives us the direction the feet should move given a desired leg movement ''' from DEFAULTS import DEFAULTS import math from LEG_MODEL import MODEL as legModel class CMA(): #____________________________________________________________________________________________________________________________________________________________________ # THESE METHODS ARE CLASS METHODS #____________________________________________________________________________________________________________________________________________________________________ '''constructor for the class creates a CMA calculator object''' def __init__(self, CENTER, SHIFT_MAG): self.CENTER = CENTER self.SHIFT = SHIFT_MAG #____________________________________________________________________________________________________________________________________________________________________ # THESE METHODS ARE HELPER METHODS #____________________________________________________________________________________________________________________________________________________________________ """Bounds the value input between lower and upper, known limits of the system, think of them as virtual hardstops""" def clip(value, lower, upper): if (value>=upper): return upper if (value<=lower): return lower return value """actual inverse kinematic model/formula""" def inverseKinematics(xD, yD, zD): l1 = legModel.L3 l2 = legModel.L6 rSquared = xD*xD+yD*yD #setup limits for rsquared and z rSquared = clip(rSquared, (l1-l2)*(l1-l2), (l1+l2)*(l1+l2)) zD = clip(zD, -0.1, 0.1) print(rSquared) print(zD) phi = math.atan2(yD, xD) kneeTheta = math.acos((rSquared-l1*l1-l2*l2)/(-2.0*l1*l2)) alpha = math.acos((l2*l2-l1*l1-rSquared)/(-2.0*l1*math.sqrt(rSquared))) shoulderTheta = phi-alpha hingeTheta = math.asin(zD/yD) return [shoulderTheta, hingeTheta, kneeTheta] """calcualte the commands to send""" def calculateCMDS(thetas): return [thetas * 100.0/(2*math.pi), thetas * 100.0/(2*math.pi), thetas * 100.0/(2*math.pi)] """calcualte the thetas from the positions""" def calculateTHETAS(cmds): return [cmds * (2*math.pi)/100.0, cmds * (2*math.pi)/100.0, cmds * (2*math.pi)/100.0] """actual inverse kinematic model/formula""" def forwardsKinematics(thetas): #define some needed variables l1 = legModel.L3 l2 = legModel.L6 t1 = thetas t2 = thetas t3 = thetas #caluclate X and Y xC = l1*math.cos(t1)-l2*math.cos(t3-t1) yC = l1*math.sin(t1)+l2*math.sin(t3-t1) #calculate Z zC = yC*math.sin(t2) #return return [xC, yC, zC]
That's what we have so far... now it's time to get some real code for CMA itself written. REMEMBER THIS IS NOT A GENERIC CLASS THIS IS SPECIFIC TO THIS ROBOT. Here's what we wrote:
'''feedforward method of CMA that actually returns the command we should send CMD - is the command we want to send LEG - is the leg we want to MOVE (SINGULAR FOR NOW), 0, 1, 2, or 3 for FLL FRL BLL BRL in that order RAW is if the CMD is RAW or not true for raw and false for not raw RETURNS COMMAND IN FORMAT YOU INPUTTED IT IN (RAW OR XYZ) ''' def ff(self, CMD, RAW, LEG): #first convert a raw command to xyz CMD_NEW =  THETA_1s =  if (RAW): global CMD_NEW global THETA_1s a = calculateTHETAS( [CMD, CMD, CMD] ) b = calculateTHETAS( [CMD, CMD, CMD] ) c = calculateTHETAS( [CMD, CMD, CMD] ) d = calculateTHETAS( [CMD, CMD, CMD] ) L1 = forwardsKinematics( a ) L2 = forwardsKinematics( b ) L3 = forwardsKinematics( c ) L4 = forwardsKinematics( d ) CMD_NEW = L1+L2+L3+L4 THETA_1s = [a, b, c, d] else: global CMD_NEW global THETA_1s a = inverseKinematics(CMD, CMD, CMD) b = inverseKinematics(CMD, CMD, CMD) c = inverseKinematics(CMD, CMD, CMD) d = inverseKinematics(CMD, CMD, CMD) CMD_NEW = CMD THETA_1s = [a, b, c, d] #okay so now we have the commands as XYZ commands and we have the leg THETA_1s #now let's remember these aren't the current they're the DESIRED and all we're doing is OFFSETTING THE X_D from the center #FOR NON MOVING LEGS so first we calculate the offsets THETA_M = THETA_1s[LEG] dCOG = 5.0/4.0*cos(THETA_M-math.pi/4.0)/100.0 dX = dCOG/3.0+0.005 #this is how much in the x-direction each leg needs to move from the CENTER POSITION plus a 0.005 safety factor #now we need to send the feet to the x_CMD in question CMD_RETURN =  if (LEG==0): global CMD_RETURN CMD_RETURN = [CMD_NEW, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW] if (LEG==1): global CMD_RETURN CMD_RETURN = [dX, CMD_NEW, CMD_NEW, CMD_NEW, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW] if (LEG==2): global CMD_RETURN CMD_RETURN = [dX, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW, CMD_NEW, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW] if (LEG==2): global CMD_RETURN CMD_RETURN = [dX, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW, dX, CMD_NEW, CMD_NEW, CMD_NEW, CMD_NEW, CMD_NEW] #return the value if(RAW): return doIK(CMD_RETURN) return CMD_RETURN
Remember this is chip-specific, we're going to try it out after lunch as a separate test first and then on the platform. WE WANT TO REVIEW IT FIRST. Actually, we're going to JUST MAKE THIS METHOD RETUN IN CMDS not in xyz because then we can directly publish it (after we test of course). Here's the main main main part of the code.
THETA_M = THETA_1s[LEG] dCOG = -5.0/4.0*math.cos(THETA_M-math.pi/4.0)/100.0 #in the opposite direction! because reverse coord frame dX = dCOG/3.0+math.copysign(0.005, dCOG) #this is how much in the x-direction each leg needs to move from the CENTER POSITION plus a 0.005 safety factor
Now we're just going to fix it so it only returns commands we can publish. So that's in direct CMD frame. So I think we're ready to start our first CMA thing. Remember, for CMA to work, we need to have the robot in the 0-position in between at the start of using it. For now we're testing so it doesn't matter.
if(MODE==CONTROL_MODE.TEST): #then publish stand/sit commands TEST_POS = [0.0, 0.45, 0.0, 0.1, 0.25, 0.0, 0.0, 0.45, 0.0, 0.0, 0.45, 0.0] if(LB==1.0): publish_CMDS(cmaCalc.ff(TEST_POS, False, 1)) if(RB==1.0): publish_CMDS(DEFAULTS.CMA_CENTER)
So we'll try lifting the leg with CMA involved. And nowww... the rear left beatbox at the knee is acting up and we need to fix it. Yay... that one's been not great for a while and now we need to fix it we're going to test all the boxes while we're doing that.
So why are we doing this CMA thing? What we're trying to do is say, hey, I'm going to give it a foot position, and it'll automatically adjust its other feet positions to keep the platform balanced no matter what I do with the other leg. That's all we're trying to do. Just trying to make sure the platform doesn't fall over. The only other thing we just added in CMA is the diagonally opposite leg of the one we're picking up gets dropped down by 0.1 in y-height so the platform leans in the opposite direction of how it would fall. But for now, we have a gearbox to fix...
So I don't know what's going on with this gearbox there is a lot of slipping happening here. We opened it, oiled it, tightened it up, and hopefully now there won't be much more slip. We're going to put the whole thing back together and we'll test out more CMA stuff either tomorrow or later today. But so far, everything not going terribly. We assumed we'd have mechanical problems.
And after all that nonsense, here's what happens. The gearbox is worse than it was before which means there's something very wrong with it. So what we're going to do, this is the sound of like the motor not connecting to the gearbox properly that's where the slip is. So what we're going to do tomorrow, is take apart the whole leg again, take off all the fabric, and fix it completely once and for all. I don't know how yet but we will. That's tomorrow morning's plan. At least we got a lot of code written today.
ONE FINAL OTHER NOTE: FOR CMA CLASS, THE NEXT TIME WE LOG ON, WE NEED TO REVERSE THE DIRECTION IF THE LEG IS A BACK LEG BECAUSE WE WANT IT TO DO THE OPPOSITE IN THAT CASE. RIGHT? WE ALSO WANT TO THINK ABOUT IF WE'RE DOING CMA DO WE WANT TO RETURN A TRAJECTORY WHERE IT MOVES FIRST AND THEN LIFTS THE LEG OR DO WE WANT TO JUST DO ALL OF IT AT ONCE, WE NEED TO TEST FOR THAT AFTER WE FIX THE GEARBOX.