Merge pull request #945 from YunfeiBai/master

Set joint damping in IK.

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -148,6 +148,7 @@ public:
         m_time+=dt;
         m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time));
         m_targetOri.setValue(0, 1.0, 0, 0);
+        m_targetPos.setValue(0.2*b3Cos( m_time), 0.2*b3Sin( m_time), 1.1);
         
         
         int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
@@ -195,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];
@@ -208,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;
@@ -236,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/IKTrajectoryHelper.cpp

@@ -16,6 +16,7 @@ struct IKTrajectoryHelperInternalData
 {
     VectorR3 m_endEffectorTargetPosition;
     VectorRn m_nullSpaceVelocity;
+    VectorRn m_dampingCoeff;
     
     b3AlignedObjectArray<Node*> m_ikNodes;
     
@@ -23,6 +24,7 @@ struct IKTrajectoryHelperInternalData
     {
         m_endEffectorTargetPosition.SetZero();
         m_nullSpaceVelocity.SetZero();
+        m_dampingCoeff.SetZero();
     }
 };
 
@@ -136,7 +138,9 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
 		case IK2_DLS:
         case IK2_VEL_DLS:
 		case IK2_VEL_DLS_WITH_ORIENTATION:
-            ikJacobian.CalcDeltaThetasDLS();			// Damped least squares method
+            //ikJacobian.CalcDeltaThetasDLS();			// Damped least squares method
+            assert(m_data->m_dampingCoeff.GetLength()==numQ);
+            ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
             break;
         case IK2_VEL_DLS_WITH_NULLSPACE:
         case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
@@ -201,3 +205,14 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current,
     }
     return true;
 }
+
+bool IKTrajectoryHelper::setDampingCoeff(int numQ, const double* coeff)
+{
+    m_data->m_dampingCoeff.SetLength(numQ);
+    m_data->m_dampingCoeff.SetZero();
+    for (int i = 0; i < numQ; ++i)
+    {
+        m_data->m_dampingCoeff[i] = coeff[i];
+    }
+    return true;
+}

examples/SharedMemory/IKTrajectoryHelper.h

@@ -31,6 +31,7 @@ public:
 		double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
     
     bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
+    bool setDampingCoeff(int numQ, const double* coeff);
     
 };
 #endif //IK_TRAJECTORY_HELPER_H

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

@@ -4011,12 +4011,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                 
                                 endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
                                 endEffectorOri.serializeDouble(endEffectorWorldOrientation);
-								double dampIK[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
+                                
+                                // Set joint damping coefficents. A small default
+                                // 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,
                                                        endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
                                                        &q_current[0],
                                                        numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
-									&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIK);
+									&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
                                 
                                 serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
                                 for (int i=0;i<numDofs;i++)

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

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -389,6 +389,26 @@ void Jacobian::CalcDeltaThetasDLS()
     }
 }
 
+void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
+{
+    const MatrixRmn& J = ActiveJacobian();
+    
+    U.SetSize(J.GetNumColumns(), J.GetNumColumns());
+    MatrixRmn::TransposeMultiply(J, J, U);
+    U.AddToDiagonal( dVec );
+    
+    dT1.SetLength(J.GetNumColumns());
+    J.MultiplyTranspose(dS, dT1);
+    U.Solve(dT1, &dTheta);
+    
+    // Scale back to not exceed maximum angle changes
+    double maxChange = dTheta.MaxAbs();
+    if ( maxChange>MaxAngleDLS ) {
+        dTheta *= MaxAngleDLS/maxChange;
+    }
+}
+
+
 void Jacobian::CalcDeltaThetasDLSwithSVD() 
 {	
 	const MatrixRmn& J = ActiveJacobian();

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -68,6 +68,7 @@ public:
 	void CalcDeltaThetasTranspose();
 	void CalcDeltaThetasPseudoinverse();
 	void CalcDeltaThetasDLS();
+    void CalcDeltaThetasDLS2(const VectorRn& dVec);
 	void CalcDeltaThetasDLSwithSVD();
 	void CalcDeltaThetasSDLS();
     void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV);
@@ -136,4 +137,4 @@ public:
 
 };
 
-#endif
+#endif

examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp

@@ -251,6 +251,18 @@ MatrixRmn& MatrixRmn::AddToDiagonal( double d )				// Adds d to each diagonal en
 	return *this;
 }
 
+// Add a vector to the entries on the diagonal
+MatrixRmn& MatrixRmn::AddToDiagonal( const VectorRn& dVec )				// Adds dVec to the diagonal entries
+{
+    long diagLen = Min( NumRows, NumCols );
+    double* dPtr = x;
+    for (int i = 0; i < diagLen && i < dVec.GetLength(); ++i) {
+        *dPtr += dVec[i];
+        dPtr += NumRows+1;
+    }
+    return *this;
+}
+
 // Multiply two MatrixRmn's
 MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst )
 {

examples/ThirdPartyLibs/BussIK/MatrixRmn.h

@@ -110,6 +110,7 @@ public:
 
 	// Miscellaneous operation
 	MatrixRmn& AddToDiagonal( double d );					// Adds d to each diagonal
+    MatrixRmn& AddToDiagonal( const VectorRn& dVec);
 
 	// Solving systems of linear equations
 	void Solve( const VectorRn& b, VectorRn* x ) const;	  // Solves the equation   (*this)*x = b;    Uses row operations.  Assumes *this is invertible.   

examples/pybullet/inverse_kinematics.py

@@ -22,6 +22,8 @@ ul=[.967,2	,2.96,2.29,2.96,2.09,3.05]
 jr=[5.8,4,5.8,4,5.8,4,6]
 #restposes for null space
 rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0]
+#joint damping coefficents
+jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1]
 
 for i in range (numJoints):
 	p.resetJointState(kukaId,i,rp[i])
@@ -64,7 +66,7 @@ while 1:
 				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
 		else:
 			if (useOrientation==1):
-				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn)
+				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd)
 			else:
 				jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos)
 	

examples/pybullet/pybullet.c

@@ -4430,9 +4430,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 		PyObject* upperLimitsObj = 0;
 		PyObject* jointRangesObj = 0;
 		PyObject* restPosesObj = 0;
+        PyObject* jointDampingObj = 0;
 
-	static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL };
+	static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL };
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist,  &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist,  &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, & jointDampingObj))
 	{
 		return NULL;
 	}
@@ -4452,18 +4453,21 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 		int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0;
 		int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0;
 		int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0;
+        int szJointDamping = jointDampingObj? PySequence_Size(jointDampingObj):0;
 
 		int numJoints = b3GetNumJoints(sm, bodyIndex);
 
 
 		int hasNullSpace = 0;
+        int hasJointDamping = 0;
 		double* lowerLimits = 0;
 		double* upperLimits = 0;
 		double* jointRanges = 0;
 		double* restPoses = 0;
+        double* jointDamping = 0;
 		
 		if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
-          (szJointRanges == numJoints) && (szRestPoses == numJoints)) 
+          (szJointRanges == numJoints) && (szRestPoses == numJoints))
 		{
 			int szInBytes = sizeof(double) * numJoints;
 			int i;
@@ -4480,11 +4484,23 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 				  pybullet_internalGetFloatFromSequence(upperLimitsObj, i);
 			  jointRanges[i] =
 				  pybullet_internalGetFloatFromSequence(jointRangesObj, i);
-			restPoses[i] =
+			  restPoses[i] =
 				pybullet_internalGetFloatFromSequence(restPosesObj, i);
 			}
 			hasNullSpace = 1;
 		}
+        
+        if (numJoints && (szJointDamping == numJoints))
+        {
+            int szInBytes = sizeof(double) * numJoints;
+            int i;
+            jointDamping = (double*)malloc(szInBytes);
+            for (i = 0; i < numJoints; i++)
+            {
+                jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i);
+            }
+            hasJointDamping = 1;
+        }
 
 		if (hasPos)
 		{
@@ -4514,6 +4530,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 					b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos);
 				}
 			}
+            
+            if (hasJointDamping)
+            {
+                b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
+            }
 
 			statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);