Fix jacobian computation

examples/pybullet/examples/jacobian.py

@@ -8,6 +8,15 @@ def getJointStates(robot):
 	joint_torques = [state[3] for state in joint_states]
 	return joint_positions, joint_velocities, joint_torques
 
+def getMotorJointStates(robot):
+	joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
+	joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
+	joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
+	joint_positions = [state[0] for state in joint_states]
+	joint_velocities = [state[1] for state in joint_states]
+	joint_torques = [state[3] for state in joint_states]
+	return joint_positions, joint_velocities, joint_torques
+
 def setJointPosition(robot, position, kp=1.0, kv=0.3):
 	num_joints = p.getNumJoints(robot)
 	zero_vec = [0.0] * num_joints
@@ -20,47 +29,60 @@ def setJointPosition(robot, position, kp=1.0, kv=0.3):
 			  "Expected torque vector of "
 			  "length {}, got {}".format(num_joints, len(torque)))
 
-def multiplyJacobian(jacobian, vector):
+def multiplyJacobian(robot, jacobian, vector):
 	result = [0.0, 0.0, 0.0]
+	i = 0
 	for c in range(len(vector)):
+		if p.getJointInfo(robot, c)[3] > -1:
 			for r in range(3):
-			result[r] += jacobian[r][c] * vector[c]
+				result[r] += jacobian[r][i] * vector[c]
+			i += 1
 	return result
 
 
 clid = p.connect(p.SHARED_MEMORY)
 if (clid<0):
 	p.connect(p.DIRECT)
+
 time_step = 0.001
 gravity_constant = -9.81
 p.resetSimulation()
 p.setTimeStep(time_step)
 p.setGravity(0.0, 0.0, gravity_constant)
+
 p.loadURDF("plane.urdf",[0,0,-0.3])
-kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+
+kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf", useFixedBase=True)
+#kukaId = p.loadURDF("TwoJointRobot_w_fixedJoints.urdf",[0,0,0])
+#kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
+#kukaId = p.loadURDF("kuka_lwr/kuka.urdf",[0,0,0])
+#kukaId = p.loadURDF("humanoid/nao.urdf",[0,0,0])
 p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1])
-kukaEndEffectorIndex = 6
 numJoints = p.getNumJoints(kukaId)
-if (numJoints!=7):
+kukaEndEffectorIndex = numJoints - 1
-	exit()
+
 # Set a joint target for the position control and step the sim.
-setJointPosition(kukaId, [0.1] * p.getNumJoints(kukaId))
+setJointPosition(kukaId, [0.1] * numJoints)
 p.stepSimulation()
+
 # Get the joint and link state directly from Bullet.
 pos, vel, torq = getJointStates(kukaId)
+mpos, mvel, mtorq = getMotorJointStates(kukaId)
+
 result = p.getLinkState(kukaId, kukaEndEffectorIndex, computeLinkVelocity=1, computeForwardKinematics=1)
 link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
 # Get the Jacobians for the CoM of the end-effector link.
 # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
 # The localPosition is always defined in terms of the link frame coordinates.
-zero_vec = [0.0] * numJoints
+
-jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, pos, zero_vec, zero_vec)
+zero_vec = [0.0] * len(mpos)
+jac_t, jac_r = p.calculateJacobian(kukaId, kukaEndEffectorIndex, com_trn, mpos, zero_vec, zero_vec)
 
 print ("Link linear velocity of CoM from getLinkState:")
 print (link_vt)
 print ("Link linear velocity of CoM from linearJacobian * q_dot:")
-print (multiplyJacobian(jac_t, vel))
+print (multiplyJacobian(kukaId, jac_t, vel))
 print ("Link angular velocity of CoM from getLinkState:")
 print (link_vr)
 print ("Link angular velocity of CoM from angularJacobian * q_dot:")
-print (multiplyJacobian(jac_r, vel))
+print (multiplyJacobian(kukaId, jac_r, vel))

examples/pybullet/pybullet.c

@@ -8326,10 +8326,48 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
 			int szObVel = PySequence_Size(objVelocities);
 			int szObAcc = PySequence_Size(objAccelerations);
 			int numJoints = b3GetNumJoints(sm, bodyUniqueId);
-			if (numJoints && (szLoPos == 3) && (szObPos == numJoints) && 
+
-				(szObVel == numJoints) && (szObAcc == numJoints))
+			int j=0;
+			int dofCountOrg = 0;
+			for (j=0;j<numJoints;j++)
 			{
-				int byteSizeJoints = sizeof(double) * numJoints;
+				struct b3JointInfo info;
+				b3GetJointInfo(sm, bodyUniqueId, j, &info);
+				switch (info.m_jointType)
+				{
+				case eRevoluteType:
+					{
+						dofCountOrg+=1;
+						break;
+					}
+				case ePrismaticType:
+					{
+						dofCountOrg+=1;
+						break;
+					}
+				case eSphericalType:
+					{
+						PyErr_SetString(SpamError,
+								"Spherirical joints are not supported in the pybullet binding");
+						return NULL;
+					}
+				case ePlanarType:
+					{
+						PyErr_SetString(SpamError,
+								"Planar joints are not supported in the pybullet binding");
+						return NULL;
+					}
+					default:
+					{
+						//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
+					}
+				}
+			}
+
+			if (dofCountOrg && (szLoPos == 3) && (szObPos == dofCountOrg) &&
+				(szObVel == dofCountOrg) && (szObAcc == dofCountOrg))
+			{
+				int byteSizeJoints = sizeof(double) * dofCountOrg;
 				int byteSizeVec3 =  sizeof(double) * 3;
 				int i;
 				PyObject* pyResultList = PyTuple_New(2);
@@ -8341,7 +8379,7 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
 				double* angularJacobian = NULL;
 
 				pybullet_internalSetVectord(localPosition, localPoint);
-				for (i = 0; i < numJoints; i++)
+				for (i = 0; i < dofCountOrg; i++)
 				{
 					jointPositions[i] =
 						pybullet_internalGetFloatFromSequence(objPositions, i);
@@ -8423,11 +8461,11 @@ static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyOb
 			else
 			{
 				PyErr_SetString(SpamError,
-								"calculateJacobian [numJoints] needs to be "
+								"calculateJacobian [numDof] needs to be "
 								"positive, [local position] needs to be of "
 								"size 3 and [joint positions], "
 								"[joint velocities], [joint accelerations] "
-								"need to match the number of joints.");
+								"need to match the number of DoF.");
 				return NULL;
 			}
 		}