How does MATLAB calculate the geometric jacobian for a manipulator?

I am going through this Robot Rigid Body Tree tutorial. I have defined UR5 robot and calculated the transformation matrix using the getTransfrom and verified with the transformation matrix I got by calculating on paper.

I also calculated the Jacobian using the definition for a revolute joint as given here. When I used the geometric Jacobain function, it gives me a different Jacobian. I went thru the definition and MATLAB has interchanged the linear velocity Jacobian and angular velocity Jacobian, however, the values should remain the same. But it is not so! How does MATLAB calculate the Jacobian for a robotic rigid body tree structure?

Answer 1

[MathWorks Robotics and Autonomous Systems Team]

Unfortunately, we don't see your hand-calculated solution, so it's a little bit difficult for us to pinpoint the actual problem you encountered. But to your concern: Mark Spong and Seth Hutchinson are correct and their results are consistent with the output of the geometricJacobian function. Let's illustrate this with a simple example.

First, create a simple rigid body tree model in RST that kind of looks like the one shown in your reference. It has two revolute joints (joint1 and joint2)

robot = robotics.RigidBodyTree('DataFormat', 'column'); % robot configuration as column vector 

% add first body (fixed, with an offset from ICS)
body1 = robotics.RigidBody('body1');
body1.Joint = robotics.Joint('joint1', 'revolute');
T = trvec2tform([-0.5, 0 0.2])*eul2tform([pi/2 0 0]);
body1.Joint.setFixedTransform(T);
robot.addBody(body1, robot.BaseName);

% add second body (revolute joint)
body2 = robotics.RigidBody('body2');
body2.Joint = robotics.Joint('joint2', 'revolute');
body2.Joint.setFixedTransform([0.3 -pi/3 0.1 0], 'mdh');
robot.addBody(body2, 'body1');

Inspect the robot and pay attention to the joint axes

robot.show

Call geometricJacobian method

Jac = robot.geometricJacobian([0; 0], 'body2')

Jac =

         0   -0.8660
         0    0.0000
    1.0000    0.5000
   -0.3000    0.0000
   -0.0866    0.0000
         0    0.0000

Does this answer make sense? Remember that a Jacobian relates the joint velocities to task space velocities.

1) upper 3 in the first column Joint1 rotates around body1 frame's z-axis, and body1's z-axis happen to align with the base. And in RST, the Jacobian computed is always w.r.t. the base frame (the black one in figure). So [0 0 1] means that qdot1 directly contributes to wz of the end effector.

2) lower 3 in the second column In this particular robot model, no matter how joint 2 rotates, the linear velocity of rigid body 2 (i.e. the end effector) is always zero, so you see [0 0 0]'.

3) lower3 of the first column. And this is the fun part. We can use the upper half of Spong's equation,

Here, z_(i-1) is the joint axis of joint1, [0, 0, 1]. And the [O_n - O_(i-1)] part (the line segment that connects body1 frame and body2 frame in plot) is [0.3, 0.086, 0.05] w.r.t. body1 frame (and thus [-0.0866, 0.3, 0.05] in base frame) by construction.

   cross([0 0 1]', [-0.0866, 0.3, 0.05]')

   ans =

   -0.3000
   -0.0866
         0

So, did you forget to change the coordinates?