- aditya mehrotra.

# 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[0] * 100.0/(2*math.pi), thetas[1] * 100.0/(2*math.pi), thetas[2] * 100.0/(2*math.pi)]
*""**"**calcualte the thetas from the positions**"**""*
def calculateTHETAS(cmds):
*return* [cmds[0] * (2*math.pi)/100.0, cmds[1] * (2*math.pi)/100.0, cmds[2] * (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[0]
t2 = thetas[1]
t3 = thetas[2]
*#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[0], CMD[1], CMD[2]] )
b = calculateTHETAS( [CMD[3], CMD[4], CMD[5]] )
c = calculateTHETAS( [CMD[6], CMD[7], CMD[8]] )
d = calculateTHETAS( [CMD[9], CMD[10], CMD[11]] )
L1 = forwardsKinematics( a )
L2 = forwardsKinematics( b )
L3 = forwardsKinematics( c )
L4 = forwardsKinematics( d )
CMD_NEW = L1+L2+L3+L4
THETA_1s = [a[0], b[0], c[0], d[0]]
*else*:
global CMD_NEW
global THETA_1s
a = inverseKinematics(CMD[0], CMD[1], CMD[2])
b = inverseKinematics(CMD[3], CMD[4], CMD[5])
c = inverseKinematics(CMD[6], CMD[7], CMD[8])
d = inverseKinematics(CMD[9], CMD[10], CMD[11])
CMD_NEW = CMD
THETA_1s = [a[0], b[0], c[0], d[0]]
*#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[0], CMD_NEW[1], CMD_NEW[2], dX, CMD_NEW[4], CMD_NEW[5], dX, CMD_NEW[7], CMD_NEW[8], dX, CMD_NEW[10], CMD_NEW[11]]
*if* (LEG==1):
global CMD_RETURN
CMD_RETURN = [dX, CMD_NEW[1], CMD_NEW[2], CMD_NEW[3], CMD_NEW[4], CMD_NEW[5], dX, CMD_NEW[7], CMD_NEW[8], dX, CMD_NEW[10], CMD_NEW[11]]
*if* (LEG==2):
global CMD_RETURN
CMD_RETURN = [dX, CMD_NEW[1], CMD_NEW[2], dX, CMD_NEW[4], CMD_NEW[5], CMD_NEW[6], CMD_NEW[7], CMD_NEW[8], dX, CMD_NEW[10], CMD_NEW[11]]
*if* (LEG==2):
global CMD_RETURN
CMD_RETURN = [dX, CMD_NEW[1], CMD_NEW[2], dX, CMD_NEW[4], CMD_NEW[5], dX, CMD_NEW[7], CMD_NEW[8], CMD_NEW[9], CMD_NEW[10], CMD_NEW[11]]
*#**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****.**