Add desired null space velocity computation.

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -96,6 +96,9 @@ struct b3RobotSimInverseKinematicArgs
 	double m_endEffectorTargetOrientation[4];
     int m_endEffectorLinkIndex;
 	int m_flags;
+    b3AlignedObjectArray<double> m_lowerLimits;
+    b3AlignedObjectArray<double> m_upperLimits;
+    b3AlignedObjectArray<double> m_jointRanges;
 
 	b3RobotSimInverseKinematicArgs()
 		:m_bodyUniqueId(-1),

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -15,7 +15,7 @@
 struct IKTrajectoryHelperInternalData
 {
     VectorR3 m_endEffectorTargetPosition;
-    
+    VectorRn m_nullSpaceVelocity;
     
     b3AlignedObjectArray<Node*> m_ikNodes;
     Jacobian* m_ikJacobian;
@@ -23,6 +23,7 @@ struct IKTrajectoryHelperInternalData
     IKTrajectoryHelperInternalData()
     {
         m_endEffectorTargetPosition.SetZero();
+        m_nullSpaceVelocity.SetZero();
     }
 };
 
@@ -129,8 +130,11 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
 		case IK2_DLS:
         case IK2_VEL_DLS:
 		case IK2_VEL_DLS_WITH_ORIENTATION:
-            //m_data->m_ikJacobian->CalcDeltaThetasDLS();			// Damped least squares method
+            m_data->m_ikJacobian->CalcDeltaThetasDLS();			// Damped least squares method
-            m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace();
+            break;
+        case IK2_VEL_DLS_WITH_NULLSPACE:
+            assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
+            m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
             break;
         case IK2_DLS_SVD:
             m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
@@ -165,3 +169,28 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
     }
     return true;
 }
+
+bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range)
+{
+    m_data->m_nullSpaceVelocity.SetLength(numQ);
+    m_data->m_nullSpaceVelocity.SetZero();
+    double stayCloseToZeroGain = 0.1;
+    double stayAwayFromLimitsGain = 10.0;
+    
+    // Stay close to zero
+    for (int i = 0; i < numQ; ++i)
+    {
+        m_data->m_nullSpaceVelocity[i] = -stayCloseToZeroGain * q_current[i];
+    }
+
+    // Stay away from joint limits
+    for (int i = 0; i < numQ; ++i) {
+        if (q_current[i] > upper_limit[i]) {
+            m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
+        }
+        if (q_current[i] < lower_limit[i]) {
+            m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
+        }
+    }
+    return true;
+}

examples/SharedMemory/IKTrajectoryHelper.h

@@ -10,6 +10,7 @@ enum IK2_Method
     IK2_DLS_SVD,
     IK2_VEL_DLS,
 	IK2_VEL_DLS_WITH_ORIENTATION,
+    IK2_VEL_DLS_WITH_NULLSPACE,
 };
 
 
@@ -28,5 +29,7 @@ public:
 		const double* q_old, int numQ, int endEffectorIndex,
 		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);
+    
 };
 #endif //IK_TRAJECTORY_HELPER_H

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -320,7 +320,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse()
 
 }
 
-void Jacobian::CalcDeltaThetasDLSwithNullspace()
+void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
 {	
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -337,11 +337,6 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace()
 	U.Solve( dS, &dT1 );
 	J.MultiplyTranspose( dT1, dTheta );
     
-    // Desired velocity
-    VectorRn desiredV(J.GetNumColumns());
-    desiredV.SetZero();
-    desiredV.Set(3, -0.2);
-    
     // Compute JInv in damped least square form
     MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns());
     U.ComputeInverse(UInv);

examples/ThirdPartyLibs/BussIK/Jacobian.h

@@ -70,7 +70,7 @@ public:
 	void CalcDeltaThetasDLS();
 	void CalcDeltaThetasDLSwithSVD();
 	void CalcDeltaThetasSDLS();
-    void CalcDeltaThetasDLSwithNullspace();
+    void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV);
 
 	void UpdateThetas();
     void UpdateThetaDot();