- aditya mehrotra.

# 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])[0]
FRL = toTHETA(IN[3:6])[0]
BLL = toTHETA(IN[6:9])[0]
BRL = toTHETA(IN[9:])[0]
*#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.

#updates #omgrobots #chip #CMA #how_to_not_IMU #smart_but_cheap