How to use Pinocchio + CasADi to obtain the relationship between the end-effector Jacobian and joint configuration J(q)?

[yilin404]

Description:

I am trying to compute the Jacobian matrix $J(q)$ of the end-effector of a robot using Pinocchio and CasADi, and I want to obtain the explicit functional relationship between the Jacobian and the joint configuration $q$.
So far, I have successfully loaded the robot model and obtained kinematic data for the robot at a given configuration. Specifically, I want to:

1. Use Pinocchio to compute the Jacobian matrix of the end-effector.Get the symbolic expression of $J(q)$ with respect to the joint angles $q$ using CasADi.
2. Use the symbolic expression for further optimization or calculations, such as minimizing the determinant of the Jacobian.

Here is what I have tried so far:

q = casadi.SX.sym("q", planner.reduced_robot.nq)  # 关节角度符号变量
cpin.framesForwardKinematics(planner.cmodel, planner.cdata, q)
J = cpin.getFrameJacobian(planner.cmodel, planner.cdata, planner.ee_link_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
print(J)
jacobian_func = casadi.Function("jacobian", [q], [J])

# 示例：计算给定关节角度下的雅可比矩阵
q_val = np.zeros(planner.reduced_robot.model.nq)  # 示例关节角度
print("Input joint angles:", q_val)
jacobian_val = jacobian_func(q_val)
print("Jacobian matrix:", jacobian_val)

But I found that the printed result always ends up being:

@1=0, 
[[@1, @1, @1, @1, @1, @1], 
 [@1, @1, @1, @1, @1, @1], 
 [@1, @1, @1, @1, @1, @1], 
 [@1, @1, @1, @1, @1, @1], 
 [@1, @1, @1, @1, @1, @1], 
 [@1, @1, @1, @1, @1, @1]]
Input joint angles: [0. 0. 0. 0. 0. 0.]
Jacobian matrix: 
[[0, 0, 0, 0, 0, 0], 
 [0, 0, 0, 0, 0, 0], 
 [0, 0, 0, 0, 0, 0], 
 [0, 0, 0, 0, 0, 0], 
 [0, 0, 0, 0, 0, 0], 
 [0, 0, 0, 0, 0, 0]]

[jcarpent]

Could you share a fully reproducible model with your robot model please?

[yilin404]

a1arm_description_.zip

Thank you for your response! I’ve attached the robot model I’m using. Below is the code I used to build my Pinocchio model:

### Setting up the robot model ###
self.robot = pin.RobotWrapper.BuildFromURDF(urdf_filename,
                                            [os.path.join(os.path.dirname(urdf_filename), "../..")])
self.reduced_robot = self.robot.buildReducedRobot(list_of_joints_to_lock=locked_joints,
                                                  reference_configuration=np.array([0.0] * self.robot.model.nq),)

# Creating Casadi models and data for symbolic computing
self.cmodel = cpin.Model(self.reduced_robot.model)
self.cdata = self.cmodel.createData()

[ryker779]

I think you just need to call cpin.computeJointJacobians() before calling cpin.getFrameJacobians() so that cdata.J is correctly populated.