From 94c7bbe8e339ba0715bc69b5fe8800a73fb8aef8 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Thu, 29 Sep 2016 17:12:51 -0700
Subject: [PATCH] Add null space task for IK.

---
 examples/ThirdPartyLibs/BussIK/Jacobian.cpp  | 37 ++++++++++++--------
 examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp | 15 ++++++++
 examples/ThirdPartyLibs/BussIK/MatrixRmn.h   |  2 ++
 3 files changed, 39 insertions(+), 15 deletions(-)

diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
index 916d39765..238721a75 100644
--- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
@@ -337,25 +337,32 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace()
 	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();
+    // 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);
-    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));
-        }
-    }
+    assert(U.DebugCheckInverse(UInv));
+    MatrixRmn JInv(J.GetNumColumns(), J.GetNumRows());
+    MatrixRmn::TransposeMultiply(J, UInv, JInv);
+    
+    // Compute null space projection
+    MatrixRmn JInvJ(J.GetNumColumns(), J.GetNumColumns());
+    MatrixRmn::Multiply(JInv, J, JInvJ);
+    MatrixRmn P(J.GetNumColumns(), J.GetNumColumns());
+    P.SetIdentity();
+    P -= JInvJ;
+    
+    // Compute null space velocity
+    VectorRn nullV(J.GetNumColumns());
+    P.Multiply(desiredV, nullV);
+    
+    // Add null space velocity
+    dTheta += nullV;
     
-
 	// Scale back to not exceed maximum angle changes
 	double maxChange = dTheta.MaxAbs();
 	if ( maxChange>MaxAngleDLS ) {
diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
index 85e398628..15b94ac23 100644
--- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
+++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
@@ -973,6 +973,21 @@ bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const Matr
 	return ret;
 }
 
+bool MatrixRmn::DebugCheckInverse( const MatrixRmn& MInv ) const
+{
+    assert ( this->NumRows==this->NumCols );
+    assert ( MInv.NumRows==MInv.NumCols );
+    MatrixRmn I(this->NumRows, this->NumCols);
+    I.SetIdentity();
+    MatrixRmn MMInv(this->NumRows, this->NumCols);
+    Multiply(*this, MInv, MMInv);
+    I -= MMInv;
+    double error = I.FrobeniusNorm();
+    bool ret = ( fabs(error)<=1.0e-13 );
+    assert ( ret );
+    return ret;
+}
+
 bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
 {
 	// Special SVD test code
diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h
index 11625878a..491d6340d 100644
--- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h
+++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h
@@ -131,6 +131,8 @@ public:
 	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;
+    // Debug matrix inverse computation
+    bool DebugCheckInverse( const MatrixRmn& MInv ) 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 );