Open matheecs opened 11 months ago
When I use JointModelSphericalZYX as part of floting joint, the result C*v+g is not equal to nonLinearEffects (nle).
JointModelSphericalZYX
$$C(q,v)v+g(q) = nle$$
import pinocchio as pin import numpy as np np.set_printoptions(precision=8, suppress=True, linewidth=250) # create a model with a JointModelSphericalZYX urdf_file = "mybot.urdf" jc = pin.JointModelComposite() jc = jc.addJoint(pin.JointModelTranslation()) jc = jc.addJoint(pin.JointModelSphericalZYX()) model = pin.buildModelFromUrdf( urdf_file, jc ) data = model.createData() q_home = np.zeros(model.nq) v_home = np.array([0.1] * model.nv) pin.computeCoriolisMatrix(model, data, q_home, v_home) pin.nonLinearEffects(model, data, q_home, v_home) pin.computeGeneralizedGravity(model, data, q_home) # check C(q,v)v+g(q) == nle if not np.allclose(data.nle, data.C@v_home + data.g): print(f"nle = {data.nle}") print(f"Cv+g= {data.C@v_home + data.g}")
Output:
nle = [-0.00993994 -0.00706209 56.54352842 -0.00136424 -0.52168281 -0.00489886 -0.25487205 -0.25479257] Cv+g= [-0.00628687 -0.0028713 56.54406614 -0.0005245 -0.52431665 -0.00165649 -0.25502346 -0.25493177]
The results are inconsistent.
Model file (mybot.urdf):
mybot.urdf
<?xml version="1.0" encoding="utf-8"?> <robot name="mybot"> <link name="body"> <inertial> <origin xyz="0.00037 0 0.01792" rpy="0 0 0" /> <mass value="4.935" /> <inertia ixx="0.036517" ixy="-3.49E-05" ixz="1.11E-05" iyy="0.0058391" iyz="6.41E-06" izz="0.039681" /> </inertial> </link> <link name="left_link"> <inertial> <origin xyz="0.064144 0 -0.047389" rpy="0 0 0" /> <mass value="0.394" /> <inertia ixx="0.000169" ixy="-1E-09" ixz="0.000195" iyy="0.0019044" iyz="0" izz="0.0018657" /> </inertial> </link> <joint name="left_joint" type="revolute"> <origin xyz="0 0.1 -0.5" rpy="0 0 0" /> <parent link="body" /> <child link="left_link" /> <axis xyz="0 1 0" /> <limit lower="-0.7" upper="0.7" effort="200" velocity="23" /> </joint> <joint name="left_front_joint" type="fixed"> <parent link="left_link" /> <child link="lf_contact" /> <origin xyz=" 0.10 0 -0.06" /> </joint> <joint name="left_back_joint" type="fixed"> <parent link="left_link" /> <child link="lb_contact" /> <origin xyz="-0.03 0 -0.06" /> </joint> <link name="lf_contact"> <inertial> <mass value="0.01" /> <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" /> </inertial> </link> <link name="lb_contact"> <inertial> <mass value="0.01" /> <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" /> </inertial> </link> <link name="right_link"> <inertial> <origin xyz="0.064144 0 -0.047389" rpy="0 0 0" /> <mass value="0.394" /> <inertia ixx="0.000169" ixy="1E-09" ixz="0.000195" iyy="0.0019044" iyz="0" izz="0.0018657" /> </inertial> </link> <joint name="right_joint" type="revolute"> <origin xyz="0 -0.1 -0.5" rpy="0 0 0" /> <parent link="body" /> <child link="right_link" /> <axis xyz="0 1 0" /> <limit lower="-0.7" upper="0.7" effort="200" velocity="23" /> </joint> <joint name="right_front_joint" type="fixed"> <parent link="right_link" /> <child link="rf_contact" /> <origin xyz=" 0.10 0 -0.06" /> </joint> <joint name="right_back_joint" type="fixed"> <parent link="right_link" /> <child link="rb_contact" /> <origin xyz="-0.03 0 -0.06" /> </joint> <link name="rf_contact"> <inertial> <mass value="0.01" /> <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" /> </inertial> </link> <link name="rb_contact"> <inertial> <mass value="0.01" /> <inertia ixx="1.6e-06" ixy="-0.0" ixz="-0.0" iyy="1.6e-06" iyz="-0.0" izz="1.6e-06" /> </inertial> </link> </robot>
Thanks for reporting this issue @matheecs. It is coming from the fact we do not fully support JointModelComposite. We will try to fix it in the following weeks.
Best, Justin
C(q,v)v+g(q) ≠ nonLinearEffects
When I use
JointModelSphericalZYX
as part of floting joint, the result C*v+g is not equal to nonLinearEffects (nle).Expected behavior
$$C(q,v)v+g(q) = nle$$
Code
Output:
The results are inconsistent.
Additional context
Model file (
mybot.urdf
):System