Adjust the IK setup to address the inverse kinematics issues mentioned in #1249.

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

@@ -354,6 +354,12 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
     // Compute null space velocity
     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()