Defining State dq for a Floating Base Robot in CasADi Optimization Problem

[Umair-Aslam-Projects]

Hello,

I am working on a CasADi optimization problem involving a floating base robot. I have a question regarding the correct approach to define the state dq.

The configuration q is defined as follows:
q=[x,y,z,quatx,quaty,quatz,quatw]

The velocity v is defined as:
v=[dx,dy,dz,wx,wy,wz]

I am trying to define the state dq using CasADi SX variables. Here is what I have tried:

# Configuration quaternion
quaternion = casadi.vertcat(quatx, quaty, quatz, quatw)

# Quaternion time derivative calculation
quat_dot = 0.5 * (casadi.SX(np.array([
    [wx, casadi.SX(0), wz, -wy],
    [wy, -wz, casadi.SX(0), wx],
    [wz, wy, -wx, casadi.SX(0)],
    [casadi.SX(0), -wx, -wy, -wz]
])) @ quaternion)

# State derivative
dq = casadi.vertcat(dxb, dyb, dzb, quat_dot[0], quat_dot[1], quat_dot[2], quat_dot[3])

Questions:

1. Is this the correct approach to define dq for my CasADi optimization problem?
2. Are there any improvements or corrections needed in the calculation of quat_dot?

Additional Context:

• All variables (dx, dy, dz, wx, wy, wz, quatx, quaty, quatz, quatw) are CasADi SX variables.
• I am using CasADi to formulate and solve the optimization problem.

Thank you for your assistance!

Best Regards,
Umair

[jorisv]

Hello @Umair-Aslam-Projects,

Usually we avoid to use the quaternion derivative in optimization problem:

• quaternion should be normalized to be useful
• a normalization constraint should be added in the problem
• the constraint can be violated at some point and the quaternion will be invalid

Instead, you can follow what is done in the chaper Optimization with proper integration in this notebook.

Instead of using q as the optimization variable, the problem use the acceleration vector (but the velocity vector could be used too) and add an integration constraint too link q and the acceleration/velocity vector.

I hope this resource can be useful for you.