Questions about Solving Acceleration aba in JointModelFreeFlyer and Coordinate System Definitions

[qiyuanchn]

Hello, I've run into several issues while using JointModelFreeFlyer to solve for the acceleration aba, and I'm seeking some clarifications.

1. When solving for the acceleration aba in JointModelFreeFlyer, in which coordinate system are the quantities q, v, tau, and a defined?
2. If these quantities are defined in the body - fixed coordinate system, why isn't there any coordinate system transformation involved in the examples/casadi/quadrotor - ocp.py file?
3. In the program, what does dq = v * dt + a * dt**2 mean? Is it used to represent the change in distance or the change in velocity?

I'd appreciate it if you could provide relevant explanations and guidance. Thank you!

[jcarpent]

1. When solving for the acceleration aba in JointModelFreeFlyer, in which coordinate system are the quantities q, v, tau, and a defined?

q is the configuration vector. The translation is represented in the world frame.
The other quantities v, a and tau are expressed in the body frame of the FreeFlyer.

2. If these quantities are defined in the body - fixed coordinate system, why isn't there any coordinate system transformation involved in the examples/casadi/quadrotor - ocp.py file?

I don't understand your question.

3. In the program, what does dq = v * dt + a * dt**2 mean? Is it used to represent the change in distance or the change in velocity?

dq is the variation of the configuration vector due to the change in velocity and acceleration.
The, because we work on Manifold (SE(3)), we need to use a dedicated integrator which is encoded with pin.integrate.

[qiyuanchn]

Okay, I understand now. Thank you for your reply!
In the state_integrate function of quadrotor - ocp.py, there are statements like q = casadi.SX.sym("dq", model.nq) and dq = casadi.SX.sym("q", model.nv). Is there a spelling mistake here?

[qiyuanchn]

For the third question, I'm still puzzled about why it is dq = v * dt + a * dt**2, rather than dq = v * dt + 0.5 * dt ** 2 or dq = v * dt. I'm looking forward to your explanation. Thanks.

[jcarpent]

The two variants might work.
The dq = v * dt + a * dt ** 2 version comes from the fact that v_n = v + a * dt and dq_n = v_n * dt = v * dt + a * dt **2.
Hopes it clarify your question.

[qiyuanchn]

Thanks for your reply. I understand it.

[qiyuanchn]

Hello! Since there are still some issues with my program, I have a few more questions.

1. Is the translation vector you mentioned equivalent to the global position in ROS (obtained through the libgazebo_ros_p3d.so plugin for pose information)?

2. Due to the existing program, I need to transform linear velocity between the world frame and the body frame. Currently, I am using the following code:

   q_pin = ca.vertcat(px, py, pz, qx, qy, qz, qw)
   M = cpin.XYZQUATToSE3(q)
   R = M.rotation
   RT = ca.transpose(R)
   v_linear_world = ca.mtimes(R, v_linear_body[0:3])

   However, the results seem inconsistent with those obtained by manually rotating the quaternion.

  def quat_mult(q1,q2):
      ans = ca.vertcat(q2[0,:] * q1[0,:] - q2[1,:] * q1[1,:] - q2[2,:] * q1[2,:] - q2[3,:] * q1[3,:],
             q2[0,:] * q1[1,:] + q2[1,:] * q1[0,:] - q2[2,:] * q1[3,:] + q2[3,:] * q1[2,:],
             q2[0,:] * q1[2,:] + q2[2,:] * q1[0,:] + q2[1,:] * q1[3,:] - q2[3,:] * q1[1,:],
             q2[0,:] * q1[3,:] - q2[1,:] * q1[2,:] + q2[2,:] * q1[1,:] + q2[3,:] * q1[0,:])
      return ans

  def rotate_quat(q1,v1):
      ans = quat_mult(quat_mult(q1, ca.vertcat(0, v1)), ca.vertcat(q1[0,:],-q1[1,:], -q1[2,:], -q1[3,:]))
      return ca.vertcat(ans[1,:], ans[2,:], ans[3,:]) 
  q = ca.vertcat(qw, qx, qy, qz)
  v_linear_body = ca.vertcat(vx, vy, vz)
  v_linear_world = rotate_quat(q, v_linear_body)

Hope for your reply. Thank you!