Torque Control for Gravity Compensation

So i am using the Odrive v3.6, and i am comunicating with it through Teensy 4.1,
Everything works just fine when commanding position set point and constant velocity,
My next step is to command PD+Gravity compensation through current/torque control,

My code is quite basic, i would like firstly to compensate for the attached link and i am doing this as follows,

         // Calculate Gravity Compensation
        float gravityTorque = rodMass*g*(rodLength)*cos(EncTurnToDeg);

rodMass and rodLength are constants in kg and m units, g is gravity constant,
EncTurnToDeg is encoder position reading read from odrive.GetPosition and i multiply that by 360 to convert turns to degress,

Max torque to compensate for the load is no more than 0.2 Nm depending on the center of mass calculated by the cosine of the angle,

The behavior of the motor is quite weird, it rotates back and forth quite randomly very fast and never settles down while the encoder position readings in degrees skyrockets,

The current is commanded with odrive.SetCurrent(0, -gravityTorque);

Note: I can command constant torque, but when the torque depends on an angle reading, it behaves odd,

I would appreciate any help,
Thank you

Check the documentation for the cos() function. It is likely using radians, not degrees.

1 Like

Good catch, the issue was in cogging, which was clearly noticeable at low torques,
Some anti-cogging calibration is what was needed,

For those who will read this in the future, this YT video explains the process of performing Anti-cogging calibration