Modify damped least square IK formulation. Test setting joint damping coefficients for 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,7 @@ 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 +*/ B3_HAS_NULL_SPACE_VELOCITY;
 
 			ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
 			ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[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,13 @@ 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];
+    }
+}

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/PhysicsServerCommandProcessor.cpp

@@ -4011,12 +4011,16 @@ 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 };
+                                
+                                double jointDampCoeff[7] = {20.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+                                ikHelperPtr->setDampingCoeff(numDofs, jointDampCoeff);
+                                
+								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/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);

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.