Add shared memory API and RobotSim API for setting joint damping in IK.

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -196,7 +196,8 @@ public:
             ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
             ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
 			
-			//ikargs.m_flags |= /*B3_HAS_IK_TARGET_ORIENTATION +*/ B3_HAS_NULL_SPACE_VELOCITY;
+			//ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION;
+            ikargs.m_flags |= B3_HAS_JOINT_DAMPING;
 
 			ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
 			ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
@@ -209,6 +210,7 @@ public:
             ikargs.m_upperLimits.resize(numJoints);
             ikargs.m_jointRanges.resize(numJoints);
             ikargs.m_restPoses.resize(numJoints);
+            ikargs.m_jointDamping.resize(numJoints,0.5);
             ikargs.m_lowerLimits[0] = -2.32;
             ikargs.m_lowerLimits[1] = -1.6;
             ikargs.m_lowerLimits[2] = -2.32;
@@ -237,6 +239,7 @@ public:
             ikargs.m_restPoses[4] = 0;
             ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
             ikargs.m_restPoses[6] = 0;
+            ikargs.m_jointDamping[0] = 10.0;
             ikargs.m_numDegreeOfFreedom = numJoints;
             
 			if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -516,7 +516,11 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin
 	{
 		b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
 	}
-	
+    
+    if (args.m_flags & B3_HAS_JOINT_DAMPING)
+    {
+        b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
+    }
 
     b3SharedMemoryStatusHandle statusHandle;
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -86,6 +86,7 @@ enum b3InverseKinematicsFlags
 {
 	B3_HAS_IK_TARGET_ORIENTATION=1,
     B3_HAS_NULL_SPACE_VELOCITY=2,
+    B3_HAS_JOINT_DAMPING=4,
 };
 
 struct b3RobotSimInverseKinematicArgs
@@ -102,6 +103,7 @@ struct b3RobotSimInverseKinematicArgs
     b3AlignedObjectArray<double> m_upperLimits;
     b3AlignedObjectArray<double> m_jointRanges;
     b3AlignedObjectArray<double> m_restPoses;
+    b3AlignedObjectArray<double> m_jointDamping;
 
 	b3RobotSimInverseKinematicArgs()
 		:m_bodyUniqueId(-1),

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2341,6 +2341,19 @@ void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHan
 
 }
 
+void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
+    command->m_updateFlags |= IK_HAS_JOINT_DAMPING;
+    
+    for (int i = 0; i < numDof; ++i)
+    {
+        command->m_calculateInverseKinematicsArguments.m_jointDamping[i] = jointDampingCoeff[i];
+    }
+}
+
 int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
 	int* bodyUniqueId,
 	int* dofCount,

examples/SharedMemory/PhysicsClientC_API.h

@@ -236,6 +236,7 @@ void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHand
 void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
 void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
 void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
+void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
 int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
 	int* bodyUniqueId,
 	int* dofCount,

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4012,8 +4012,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
                                 endEffectorOri.serializeDouble(endEffectorWorldOrientation);
                                 
-                                double jointDampCoeff[7] = {20.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+                                // Set joint damping coefficents. A small default
-                                ikHelperPtr->setDampingCoeff(numDofs, jointDampCoeff);
+                                // damping constant is added to prevent singularity
+                                // with pseudo inverse. The user can set joint damping
+                                // coefficients differently for each joint. The larger
+                                // the damping coefficient is, the less we rely on
+                                // this joint to achieve the IK target.
+                                btAlignedObjectArray<double> joint_damping;
+                                joint_damping.resize(numDofs,0.5);
+                                if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
+                                {
+                                    for (int i = 0; i < numDofs; ++i)
+                                    {
+                                        joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
+                                    }
+                                }
+                                ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
                                 
 								double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
                                 ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,

examples/SharedMemory/SharedMemoryCommands.h

@@ -512,7 +512,8 @@ enum EnumCalculateInverseKinematicsFlags
     IK_HAS_TARGET_POSITION=1,
 	IK_HAS_TARGET_ORIENTATION=2,
     IK_HAS_NULL_SPACE_VELOCITY=4,
-    //IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet
+    IK_HAS_JOINT_DAMPING=8,
+    //IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet
 };
 
 struct CalculateInverseKinematicsArgs
@@ -526,6 +527,7 @@ struct CalculateInverseKinematicsArgs
     double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
     double m_jointRange[MAX_DEGREE_OF_FREEDOM];
     double m_restPose[MAX_DEGREE_OF_FREEDOM];
+    double m_jointDamping[MAX_DEGREE_OF_FREEDOM];
 };
 
 struct CalculateInverseKinematicsResultArgs