Modify damped least square IK formulation. Test setting joint damping coefficients for IK.

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);
@@ -136,4 +137,4 @@ public:
 
 };
 
-#endif
+#endif

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.