inaccurate jacobian calculated for a simple 2 link robot

[TJBetter]

Hello everyone,

I'm trying to design a lab for student to verify the jacobian of a 2 link planar robot. But it looks the jacobian I got from pinocchio is wrong. Did I do anythign wrong here? I appreciate your time reading this post and help me out. Thank you.

import pinocchio as pin
import numpy as np

# Path to your URDF file
urdf_filename = 'urdf_assem.urdf'

model = pin.buildModelFromUrdf(urdf_filename)
data = model.createData()

# Define the joint configuration (in radians)
joint_configuration = np.array([np.deg2rad(30), np.deg2rad(30)]) 

# Compute the Jacobian for the end-effector (link2) in the global frame
# The end-effector frame index can be obtained from the model, assuming 'link2' is the name of the end effector
end_effector_frame_id = model.getFrameId('link2')
pin.computeJointJacobians(model, data, joint_configuration)
pin.updateFramePlacements(model, data)
jacobian = pin.getFrameJacobian(model, data, end_effector_frame_id, pin.ReferenceFrame.WORLD)

# Since we're interested in the planar case, we can select the relevant rows from the Jacobian (ignoring the rotational components for simplicity)
jacobian_reduced = jacobian[:2, -2:]  # Selecting only the x and y components for the last two joints

print("Jacobian for the end-effector (link2) in the global frame:")
print(jacobian_reduced)

The output from above script has a weird zero first column:

Jacobian for the end-effector (link2) in the global frame:
[[ 0.         0.5      ]
 [ 0.        -0.8660254]

However, by deriving it by hand,

The urdf file is:

<?xml version="1.0" ?>

<robot name="2dof" xmlns:xacro="http://ros.org/wiki/xacro">
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.1"/>
      </geometry>
      <material name="White">
       <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="1"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
        <inertia ixx="0.0217284832211" ixy="0.1" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
    </inertial>
  </link>

  <link name="link1">
    <visual>
      <origin xyz="0.5 0 0"/>
      <geometry>
        <box size="1 0.05 0.05"/>
      </geometry>
      <material name="Red">
       <color rgba="0.9 0.9 0.9 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="2"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
      <inertia ixx="0.0217284832211" ixy="0.2" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
    </inertial>
  </link>

  <link name="link2">
    <visual>
      <origin xyz="0.25 0 0"/>
      <geometry>
        <box size="1 0.05 0.05"/>
      </geometry>
      <material name="Blue">
       <color rgba="0.8 0.8 0.8 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="3.42"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
      <inertia ixx="0.0217284832211" ixy="0.3" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
    </inertial>
  </link>

<!-- Joints -->

  <joint name="joint1" type="revolute">
    <axis xyz="0 0 1" />
    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5" />
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="base_link"/>
    <child link="link1"/>
  </joint>

  <joint name="joint2" type="revolute">
    <axis xyz="0 0 1" />
    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5" />
    <origin xyz="1 0 0" rpy="0 0 0" />
    <parent link="link1"/>
    <child link="link2"/>
  </joint>
</robot>

[jorisv]

Hello @TJBetter,

I think your URDF model is not the same than in your document.

• joint1 rotate around base_link
• then, link1 x, y position is always 0
• joint2 is at 1 meter from link1 in the X axis
• then, link2 position will be on circle of radius 1 around the base_link

In your document, you're interested at a point at 1 meter from link2 in the X axis. You need to add a new frame.
You can do it by adding a new link attach to link2 with a fixed joint:

  <link name="end_effector" />

  <joint name="joint3" type="fixed">
    <origin xyz="1 0 0" rpy="0 0 0" />
    <parent link="link2"/>
    <child link="end_effector"/>
  </joint>

Then, you need to use pin.ReferenceFrame.LOCAL_WORLD_ALIGNED and the end_effector frame id to compute the frame jacobian and you will end with the same result.

[TJBetter]

@jorisv Thank you so much for this info. I should have checked the urdf file more closely.

It works and if anyone is intereted, the modified code is:

import pinocchio as pin
import numpy as np

# Path to your URDF file
urdf_filename = 'urdf_assem.urdf'

model = pin.buildModelFromUrdf(urdf_filename)
data = model.createData()

# Define the joint configuration (in radians)
# Note: Pinocchio's convention is to have the configuration as [root_joint_position (3D), root_joint_orientation (quaternion or 3 angles), joint1, joint2, ...]
# For a planar robot defined with revolute joints around the x-axis, and assuming no translation or rotation at the base, the configuration vector is straightforward.
joint_configuration = np.array([np.deg2rad(30), np.deg2rad(30)])  # Including 6 DOF for the free-flyer base, followed by our joint angles

# Compute the Jacobian for the end-effector (link2) in the global frame
# The end-effector frame index can be obtained from the model, assuming 'link2' is the name of the end effector
end_effector_frame_id = model.getFrameId('end_effector')
pin.computeJointJacobians(model, data, joint_configuration)
pin.updateFramePlacements(model, data)
jacobian = pin.getFrameJacobian(model, data, end_effector_frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)

# Since we're interested in the planar case, we can select the relevant rows from the Jacobian (ignoring the rotational components for simplicity)
jacobian_reduced = jacobian[:2, -2:]  # Selecting only the x and y components for the last two joints

print("Jacobian for the end-effector (end_effector) in the global frame:")
print(jacobian_reduced)

URDF file is:

<?xml version="1.0" ?>

<robot name="2dof" xmlns:xacro="http://ros.org/wiki/xacro">
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <cylinder length="0.2" radius="0.1"/>
      </geometry>
      <material name="White">
       <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="1"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
        <inertia ixx="0.0217284832211" ixy="0.1" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
    </inertial>
  </link>

  <link name="link1">
    <visual>
      <origin xyz="0.5 0 0"/>
      <geometry>
        <box size="1 0.05 0.05"/>
      </geometry>
      <material name="Red">
       <color rgba="0.9 0.9 0.9 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="2"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
      <inertia ixx="0.0217284832211" ixy="0.2" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
    </inertial>
  </link>

  <link name="link2">
    <visual>
      <origin xyz="0.25 0 0"/>
      <geometry>
        <box size="1 0.05 0.05"/>
      </geometry>
      <material name="Blue">
       <color rgba="0.8 0.8 0.8 1.0"/>
      </material>
    </visual>
    <inertial>
      <mass value="3.42"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.121825"/>
      <inertia ixx="0.0217284832211" ixy="0.3" ixz="0.0" iyy="0.0217284832211" iyz="0.0" izz="0.00961875"/>
    </inertial>
  </link>

   <link name="end_effector" />

<!-- Joints -->
  <joint name="joint1" type="revolute">
    <axis xyz="0 0 1" />
    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5" />
    <origin xyz="0 0 0" rpy="0 0 0" />
    <parent link="base_link"/>
    <child link="link1"/>
  </joint>

  <joint name="joint2" type="revolute">
    <axis xyz="0 0 1" />
    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5" />
    <origin xyz="1 0 0" rpy="0 0 0" />
    <parent link="link1"/>
    <child link="link2"/>
  </joint>

  <joint name="joint3" type="fixed">
    <origin xyz="1 0 0" rpy="0 0 0" />
    <parent link="link2"/>
    <child link="end_effector"/>
  </joint>
</robot>