Add matrix inverse computation in BussIK.

examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -129,7 +129,8 @@ 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_DLS_SVD:
             m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();

examples/ThirdPartyLibs/BussIK/Jacobian.cpp

@@ -320,7 +320,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse()
 
 }
 
-void Jacobian::CalcDeltaThetasDLS() 
+void Jacobian::CalcDeltaThetasDLSwithNullspace()
 {	
 	const MatrixRmn& J = ActiveJacobian();
 
@@ -336,6 +336,25 @@ void Jacobian::CalcDeltaThetasDLS()
 	// Use these two lines for the traditional DLS method
 	U.Solve( dS, &dT1 );
 	J.MultiplyTranspose( dT1, dTheta );
+    
+    VectorRn nullV(7);
+    nullV.SetZero();
+    nullV.Set(1, 2.0);
+    MatrixRmn I(U.GetNumRows(),U.GetNumColumns());
+    I.SetIdentity();
+    
+    MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns());
+    U.ComputeInverse(UInv);
+    MatrixRmn Res(U.GetNumRows(),U.GetNumColumns());
+    MatrixRmn::Multiply(U, UInv, Res);
+    for (int i = 0; i < Res.GetNumRows(); ++i)
+    {
+        for (int j = 0; j < Res.GetNumColumns(); ++j)
+        {
+            printf("i%d j%d: %f\n", i, j, Res.Get(i, j));
+        }
+    }
+    
 
 	// Scale back to not exceed maximum angle changes
 	double maxChange = dTheta.MaxAbs();
@@ -344,6 +363,30 @@ void Jacobian::CalcDeltaThetasDLS()
 	}
 }
 
+void Jacobian::CalcDeltaThetasDLS()
+{
+    const MatrixRmn& J = ActiveJacobian();
+    
+    MatrixRmn::MultiplyTranspose(J, J, U);		// U = J * (J^T)
+    U.AddToDiagonal( DampingLambdaSq );
+    
+    // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e.
+    // CalcdTClampedFromdS();
+    // VectorRn dTextra(3*m_nEffector);
+    // U.Solve( dT, &dTextra );
+    // J.MultiplyTranspose( dTextra, dTheta );
+    
+    // Use these two lines for the traditional DLS method
+    U.Solve( dS, &dT1 );
+    J.MultiplyTranspose( 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

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

examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp

@@ -513,6 +513,36 @@ void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const
 
 }
 
+void MatrixRmn::ComputeInverse( MatrixRmn& R) const
+{
+    assert ( this->NumRows==this->NumCols );
+    MatrixRmn U(this->NumRows, this->NumCols);
+    VectorRn w(this->NumRows);
+    MatrixRmn V(this->NumRows, this->NumCols);
+    
+    this->ComputeSVD(U, w, V);
+    
+    assert(this->DebugCheckSVD(U, w , V));
+    
+    double PseudoInverseThresholdFactor = 0.01;
+    double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs();
+    
+    MatrixRmn VD(this->NumRows, this->NumCols);
+    MatrixRmn D(this->NumRows, this->NumCols);
+    D.SetZero();
+    long diagLength = w.GetLength();
+    double* wPtr = w.GetPtr();
+    for ( long i = 0; i < diagLength; ++i ) {
+        double alpha = *(wPtr++);
+        if ( fabs(alpha)>pseudoInverseThreshold ) {
+            D.Set(i, i, 1.0/alpha);
+        }
+    }
+    
+    Multiply(V,D,VD);
+    MultiplyTranspose(VD,U,R);
+}
+
 // ************************************************ CalcBidiagonal **************************
 // Helper routine for SVD computation
 // U is a matrix to be bidiagonalized.

examples/ThirdPartyLibs/BussIK/MatrixRmn.h

@@ -129,6 +129,8 @@ public:
 	void ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const;
 	// Good for debugging SVD computations (I recommend this be used for any new application to check for bugs/instability).
 	bool DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const;
+    // Compute inverse of a matrix, the result is written in R
+    void ComputeInverse( MatrixRmn& R) const;
 
 	// Some useful routines for experts who understand the inner workings of these classes.
 	inline static double DotArray( long length, const double* ptrA, long strideA, const double* ptrB, long strideB );