0

I am currently trying to calculate the jacobian of a kuka arm using the "rigid_body_tree.cc" file for the equation: Tau = J^T*F, where Tau is the 7 joint torques of the kuka arm, F is the cartesian forces and torques at the end-effector, and J^T is the jacobian transposed.

There exists a function in drake called transformPointsJacobian which takes in a cache, points, from_body_or_frame_ind, to_body_or_frame_ind, and in_terms_of_qdot.

The function first calculates the geometric Jacobian which outputs a 6x7 matrix (kuka has 7 joints)

Then, it takes that matrix and uses it to determine a 3x7 jacobian which is calculated below:

J.template block<kSpaceDimension, 1>(row_start, *it) = Jv.col(col);

J.template block<kSpaceDimension, 1>(row_start, *it).noalias() += Jomega.col(col).cross(points_base.col(i));

This shrinks down the 6x7 geometric jacobian into a 3x7 jacobian where the first 3 rows were calculated by Jv + Jw*Transformation.

This code definitely works, but I don't seem to understand why this step works. Also, since I will need the torques in the cartesian end-effector space, I will need the full 6x7 jacobian.

In order to get the last 3 rows of the jacobian, how can I use the output of the geometric jacobian so that it will be valid in the equation, Tau = J^T*F?

Thanks!

Yuki
  • 139
  • 6

1 Answers1

0

Please consider switching to the supported class MultibodyPlant rather than the soon-to-be-deprecated RigidBodyTree in the Drake attic. The Jacobian documentation is much better there -- the group of methods is here. An example (with lots of documentation) is here; that one produces the 6x7.

Is there a reason you need to use the old code?

Sherm
  • 643
  • 4
  • 6
  • Unfortunately, I have a deadline that I want to meet and I may not have time to change all my code to the newest version. – Yuki Nov 20 '19 at 06:15
  • I looked inside multibody_tree.cc's CalcJacobianAngularVelocity. Inside this function, it calls CalcJacobianAngularAndOrTranslationalVelocityInWorld() At the end of this function, the final linear component of the jacobian is calculated by v + w*T : Hv_PFpi_W = (Hv_PB_W + Hw_PB_W.colwise().cross(p_BoFp_W)) * Nplus; Looking above a few lines, I found that the rotational section of the jacobian is calculated by : Js_w_PB_W = Hw_PB_W * Nplus; Ignoring Nplus, it seems that the rotational component is equal to the final jacobian's rotational component (). – Yuki Nov 20 '19 at 19:14
  • However, when I tried implementing this in rigidbodytree by changing transformpointjacobian to return a 6x7 matrix, it didn't work. The arm moves in a random direction. – Yuki Nov 20 '19 at 19:16
  • We also had trouble working with RigidBodyTree which is one of the reasons we replaced it with better-documented code. You might try asking one of the original authors for help -- you could check git blame. I think Twan Koolen may be the author of the code you are looking at. – Sherm Nov 21 '19 at 16:14