diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index c4ee51b0e..ebad64f95 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/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(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 6740733a5..916d39765 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/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(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index e02a3bee4..549d362c3 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -70,7 +70,7 @@ public: void CalcDeltaThetasDLS(); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); - + void CalcDeltaThetasDLSwithNullspace(); void UpdateThetas(); void UpdateThetaDot(); diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index 365d90f6c..85e398628 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp +++ b/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. diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 4ce0c515e..11625878a 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/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 );