Add desired null space velocity computation.

2016-09-29 22:45:31 -07:00
parent 94c7bbe8e3
commit 29f890ae10
5 changed files with 40 additions and 10 deletions
--- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+++ b/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);