From 6da931d0bd9f3028cef2f274d75629c0fddf5fe6 Mon Sep 17 00:00:00 2001
From: Jeffrey Bingham <bingjeff@gmail.com>
Date: Mon, 25 Sep 2017 23:11:35 -0700
Subject: [PATCH] Adjust jacobian.py to show mismatch between joint state and
 link state.

---
 examples/pybullet/examples/jacobian.py | 40 ++++++++++++++++++++++----
 examples/pybullet/pybullet.c           |  5 ++--
 2 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/examples/pybullet/examples/jacobian.py b/examples/pybullet/examples/jacobian.py
index 3a0dfd6e9..d0b4f68ce 100644
--- a/examples/pybullet/examples/jacobian.py
+++ b/examples/pybullet/examples/jacobian.py
@@ -25,6 +25,18 @@ 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):
+    num_joints = pb.getNumJoints(robot)
+    if velocity is None:
+        velocity = [0.0] * num_joints
+    if len(position) == num_joints and len(velocity) == num_joints:
+        for c in range(num_joints):
+            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):
@@ -45,12 +57,30 @@ if (numJoints!=7):
 setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId))
 p.stepSimulation()
 
-result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1)
-link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
+# 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)
-jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, pos, vel, vel)
 
-print "Link velocity of CoM from getLinkState:"
+# Now "reset" the simulation to have the same joint state and read the link state.
+setJointState(kukaId, pos, vel)
+result1 = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1)
+link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result1
+
+# 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_vt
-print "Link 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 "Link angular velocity of CoM from getLinkState:"
+print link_vr
+print "Link angular velocity of CoM from angularJacobian * q_dot:"
+print np.dot(np.array(jac_r), np.array(vel))
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index a6a51d23b..ea4917a88 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -7416,8 +7416,9 @@ static PyMethodDef SpamMethods[] = {
 	 " frame."},
 
 	{"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS,
-	 "Reset the state (position, velocity etc) for a joint on a body "
-	 "instantaneously, not through physics simulation."},
+	"resetJointState(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n"
+	"Reset the state (position, velocity etc) for a joint on a body "
+	"instantaneously, not through physics simulation."},
 	
 	{"changeDynamics", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS,
 	 "change dynamics information such as mass, lateral friction coefficient."},