Fix the translation jacobian.

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6667,6 +6667,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 }
                                 // Only calculate if the localPosition is non-zero.
                                 if (btInverseDynamics::maxAbs(localPosition) > 0.0) {
+                                    // Write the localPosition into world coordinates.
+                                    btInverseDynamics::mat33 world_rotation_body;
+                                    tree->getBodyTransform(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &world_rotation_body);
+                                    localPosition = world_rotation_body * localPosition;
+                                	// Correct the translational jacobian.
                                     btInverseDynamics::mat33 skewCrossProduct;
                                     btInverseDynamics::skew(localPosition, &skewCrossProduct);
                                     btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs);

examples/pybullet/examples/jacobian.py

@@ -1,7 +1,7 @@
 import sys
 sys.path.append('/Users/bingjeff/Documents/projects/walkman/bullet3/bin/')
 import pybullet as p
-import numpy as np
+
 
 def getJointStates(robot):
 	joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
@@ -25,22 +25,17 @@ def setJointPosition(robot, position, kp=1.0, kv=0.3):
 			  "Expected torque vector of "
 			  "length {}, got {}".format(num_joints, len(torque)))
 
-def setJointState(robot, position, velocity=None):
+def multiplyJacobian(jacobian, vector):
-    num_joints = pb.getNumJoints(robot)
+	result = [0.0, 0.0, 0.0]
-    if velocity is None:
+	for c in range(len(vector)):
-        velocity = [0.0] * num_joints
+		for r in range(3):
-    if len(position) == num_joints and len(velocity) == num_joints:
+			result[r] += jacobian[r][c] * vector[c]
-        for c in range(num_joints):
+	return result
-            pb.resetJointState(robot, c, position[c], velocity[c])
-    else:
-        print("Not setting state. "
-              "Expected vectors of length {}, "
-              "got pos={}, vel={}".format(num_joints, len(position), len(velocity)))
 
 
 clid = p.connect(p.SHARED_MEMORY)
 if (clid<0):
-	p.connect(p.GUI)
+	p.connect(p.DIRECT)
 time_step = 0.001
 gravity_constant = -9.81
 p.resetSimulation()
@@ -53,34 +48,24 @@ kukaEndEffectorIndex = 6
 numJoints = p.getNumJoints(kukaId)
 if (numJoints!=7):
 	exit()
-
+# Set a joint target for the position control and step the sim.
 setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId))
 p.stepSimulation()
-
+# Get the joint and link state directly from Bullet.
-# Get the value of the link and joint state after a simulation step.
-result0 = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1)
 pos, vel, torq = getJointStates(kukaId)
-
+result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1)
-# Now "reset" the simulation to have the same joint state and read the link state.
+link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
-setJointState(kukaId, pos, vel)
+# Get the Jacobians for the CoM of the end-effector link.
-result1 = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1)
+# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
-link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result1
+# The localPosition is always defined in terms of the link frame coordinates.
-
-# Show that the link origin is different for supposedly the same joint state.
-print "Link origin - stepSimulation:"
-print result0[0]
-print "Link origin - resetJointState:"
-print result1[0]
-
-# Using the "reset" system the body velocities (should) match.
 zero_vec = [0.0] * numJoints
 jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, pos, zero_vec, zero_vec)
 
-print "Link linear velocity of CoM from getLinkState:"
+print ("Link linear velocity of CoM from getLinkState:")
-print link_vt
+print (link_vt)
-print "Link linear velocity of CoM from linearJacobian * q_dot:"
+print ("Link linear velocity of CoM from linearJacobian * q_dot:")
-print np.dot(np.array(jac_t), np.array(vel))
+print (multiplyJacobian(jac_t, vel))
-print "Link angular velocity of CoM from getLinkState:"
+print ("Link angular velocity of CoM from getLinkState:")
-print link_vr
+print (link_vr)
-print "Link angular velocity of CoM from angularJacobian * q_dot:"
+print ("Link angular velocity of CoM from angularJacobian * q_dot:")
-print np.dot(np.array(jac_r), np.array(vel))
+print (multiplyJacobian(jac_r, vel))

examples/pybullet/pybullet.c

@@ -7411,6 +7411,12 @@ static PyMethodDef SpamMethods[] = {
 	 "Get the state (position, velocity etc) for multiple joints on a body."},
 
 	{"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS,
+	"position_linkcom_world, world_rotation_linkcom, "
+	"position_linkcom_frame, frame_rotation_linkcom, "
+	"position_frame_world, world_rotation_frame, "
+	"linearVelocity_linkcom_world, angularVelocity_linkcom_world = "
+	"getLinkState(objectUniqueId, linkIndex, computeLinkVelocity=0, "
+	"computeForwardKinematics=0, physicsClientId=0)\n"
 	 "Provides extra information such as the Cartesian world coordinates"
 	 " center of mass (COM) of the link, relative to the world reference"
 	 " frame."},
@@ -7564,7 +7570,7 @@ static PyMethodDef SpamMethods[] = {
 	 "Args:\n"
 	 "  bodyIndex - a scalar defining the unique object id.\n"
 	 "  linkIndex - a scalar identifying the link containing the local point.\n"
-	 "  localPosition - a list of [x, y, z] of the coordinates of the local point.\n"
+	 "  localPosition - a list of [x, y, z] of the coordinates defined in the link frame.\n"
 	 "  objPositions - a list of the joint positions.\n"
 	 "  objVelocities - a list of the joint velocities.\n"
 	 "  objAccelerations - a list of the joint accelerations.\n"