Merge pull request #1344 from YunfeiBai/master

Fix some IK related issues.

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -185,7 +185,9 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current,
 {
     m_data->m_nullSpaceVelocity.SetLength(numQ);
     m_data->m_nullSpaceVelocity.SetZero();
-    double stayCloseToZeroGain = 0.1;
+	// TODO: Expose the coefficents of the null space term so that the user can choose to balance the null space task and the IK target task.
+	// Can also adaptively adjust the coefficients based on the residual of the null space velocity in the IK target task space.
+    double stayCloseToZeroGain = 0.001;
     double stayAwayFromLimitsGain = 10.0;
 
     // Stay close to zero

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -355,6 +355,12 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
     VectorRn nullV(J.GetNumColumns());
     P.Multiply(desiredV, nullV);
 	
+	// Compute residual
+	VectorRn residual(J.GetNumRows());
+	J.Multiply(nullV, residual);
+	// TODO: Use residual to set the null space term coefficient adaptively.
+	//printf("residual: %f\n", residual.Norm());
+    
     // Add null space velocity
     dTheta += nullV;
     

examples/pybullet/examples/inverse_kinematics.py

@@ -37,20 +37,21 @@ hasPrevPose = 0
 useNullSpace = 0
 
 useOrientation = 1
-useSimulation = 1
+#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
+#This can be used to test the IK result accuracy.
+useSimulation = 0
 useRealTimeSimulation = 1
 p.setRealTimeSimulation(useRealTimeSimulation)
 #trailDuration is duration (in seconds) after debug lines will be removed automatically
 #use 0 for no-removal
 trailDuration = 15
 	
-	
 while 1:
 	if (useRealTimeSimulation):
 		dt = datetime.now()
 		t = (dt.second/60.)*2.*math.pi
 	else:
-		t=t+0.1
+		t=t+0.001
 	
 	if (useSimulation and useRealTimeSimulation==0):
 		p.stepSimulation()

examples/pybullet/gym/pybullet_envs/bullet/kuka.py

@@ -34,7 +34,7 @@ class Kuka:
     #restposes for null space
     self.rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
     #joint damping coefficents
-    self.jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
+    self.jd=[0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001,0.00001]
     self.reset()
     
   def reset(self):

examples/pybullet/pybullet.c

@@ -6881,17 +6881,33 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 			hasNullSpace = 1;
 		}
 
-		if (numJoints && (szJointDamping == numJoints))
+		if (szJointDamping > 0)
 		{
-			int szInBytes = sizeof(double) * numJoints;
+			// We allow the number of joint damping values to be larger than
+			// the number of degrees of freedom (DOFs). On the server side, it does
+			// the check and only sets joint damping for all DOFs.
+			// We can use the number of DOFs here when that is exposed. Since the
+			// number of joints is larger than the number of DOFs (we assume the
+			// joint is either fixed joint or one DOF joint), it is safe to use
+			// number of joints here.
+			if (szJointDamping < numJoints)
+			{
+				PyErr_SetString(SpamError,
+								"calculateInverseKinematics the size of input joint damping values is smaller than the number of joints.");
+				return NULL;
+			}
+			else
+			{
+				int szInBytes = sizeof(double) * szJointDamping;
 				int i;
 				jointDamping = (double*)malloc(szInBytes);
-			for (i = 0; i < numJoints; i++)
+				for (i = 0; i < szJointDamping; i++)
 				{
 					jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i);
 				}
 				hasJointDamping = 1;
 			}
+		}
 
 		if (hasPos)
 		{
@@ -6929,6 +6945,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 			{
 				b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
 			}
+			free(jointDamping);
 
 			statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);