Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

How to use the geometric_jacobian correctly? #638

Closed
wly2014 opened this issue Sep 7, 2023 · 1 comment
Closed

How to use the geometric_jacobian correctly? #638

wly2014 opened this issue Sep 7, 2023 · 1 comment

Comments

@wly2014
Copy link

wly2014 commented Sep 7, 2023

When I use geometric_jacobian to calculate the jacobian of a simple robot, I found that the linear of result is not same to the result of MATLAB, but the angular is same. The robot is as follows. It has a fixed joint and a revolute joint.

<?xml version="1.0" encoding="utf-8"?>
<robot
  name="door">
  <link
    name="base_link">
    <inertial>
      <origin
        xyz="0.0 0.0 0.0"
        rpy="0 0 0" />
      <mass
        value="6000.0" />
      <inertia
        ixx="7.1711"
        ixy="1.6399E-05"
        ixz="1.1295"
        iyy="13.321"
        iyz="-0.00010257"
        izz="12.459" />
    </inertial>
    <visual>
      <origin
        xyz="0.0 0.0 0.0"
        rpy="0 0 0" />
      <geometry>
        <box size="0.1 0.1 0.1" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.37647 0.37647 0.37647 1" />
      </material>
    </visual>
  </link>
  <link
    name="door_base_link">
    <inertial>
      <origin
        xyz="0.0 0.0 0.0"
        rpy="0 0 0" />
      <mass
        value="2.8423" />
      <inertia
        ixx="0.011332"
        ixy="-0.00032807"
        ixz="-0.00025666"
        iyy="0.011319"
        iyz="-0.00020965"
        izz="0.0084278" />
    </inertial>
    <visual>
      <origin
        xyz="0.0 0 0"
        rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.05" length="0.5"/>
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
  </link>
  <joint
    name="door_base_joint"
    type="fixed">
    <origin
      xyz="0.4 0.6 0.0"
      rpy="0.0 0.0 0.0" />
    <parent
      link="base_link" />
    <child
      link="door_base_link" />
    <axis
      xyz="0 0 0" />
  </joint>
  <link
    name="door_link">
    <inertial>
      <origin
        xyz="0.0 -0.3 0.0"
        rpy="0 0 0" />
      <mass
        value="20" />
      <inertia
        ixx="7.85"
        ixy="-2.4416E-05"
        ixz="1.5843E-05"
        iyy="7.21"
        iyz="0.0012197"
        izz="0.65" />
    </inertial>
    <visual>
      <origin
        xyz="0.0 -0.3 0.0"
        rpy="0 0 0" />
      <geometry>
        <box size="0.02 0.6 1" />
      </geometry>
      <material
        name="">
        <color
          rgba="1 1 1 1" />
      </material>
    </visual>
  </link>
  <joint
    name="door_joint"
    type="revolute">
    <origin
      xyz="0.0 0.0 0.0"
      rpy="0.0 0.0 0.0" />
    <parent
      link="door_base_link" />
    <child
      link="door_link" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.12"
      upper="3.12"
      effort="0"
      velocity="0" />
  </joint>
</robot>

The robot is shown in MATLAB:

Snipaste_2023-09-07_10-40-35

model = parse_urdf(urdf_model_path)
   
    state_dynamics = MechanismState{Float64}(model)
    q = [0.0]
    set_configuration!(state_dynamics, q)

    from_ = RigidBodyDynamics.findbody(model,"world")
    to_ = RigidBodyDynamics.findbody(model,"door_link")
    path_ = RigidBodyDynamics.path(model, from_, to_)
    Jac = RigidBodyDynamics.geometric_jacobian(state_dynamics, path_)
    @show Jac

The result of geometric_jacobian is:

Jac = GeometricJacobian: body: "after_door_joint", base: "world", expressed in "world":
[0.0; 0.0; 1.0; 0.6; -0.4; 0.0;;]

The result of MATLAB is [0.0; 0.0; 1.0; 0.0; 0.0; 0.0].

As I known, the rotation of last joint will not affect the velocity of the frame in the last joint, so the linear should be zeros (the results of MATLAB)?

Thank you.

@wly2014
Copy link
Author

wly2014 commented Sep 20, 2023

I may find where I am wrong. In RigidBodyDynamics.jl, the result of geometric_jacobian is relative to the twist at the root frame, expressed in the root frame. And the result of MATLAB is relative to the twist at the end-effector, expressed in the root frame.
So, if I transform the result of geometric_jacobian as follows and I will get the same results.

where er is the vector from the end-effector frame to the root frame, expressed in the root frame.

@wly2014 wly2014 closed this as completed Jan 14, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant