diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index 9fea9ce35..a5333555d 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -100,7 +100,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod) jacob->SetJendActive(); } jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors - + MatrixRmn AugMat; // Calculate the change in theta values switch (ikMethod) { @@ -108,7 +108,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod) jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method break; case IK_DLS: - jacob->CalcDeltaThetasDLS(); // Damped least squares method + jacob->CalcDeltaThetasDLS(AugMat); // Damped least squares method break; case IK_DLS_SVD: jacob->CalcDeltaThetasDLSwithSVD(); diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 472a24beb..fb002cc3a 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -44,6 +44,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], const double* q_current, int numQ, int endEffectorIndex, double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]) { + MatrixRmn AugMat; bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false; Jacobian ikJacobian(useAngularPart, numQ, 1); @@ -142,12 +143,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], case IK2_VEL_DLS: //ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method assert(m_data->m_dampingCoeff.GetLength() == numQ); - ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff); + ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat); break; case IK2_VEL_DLS_WITH_NULLSPACE: case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: assert(m_data->m_nullSpaceVelocity.GetLength() == numQ); - ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); + ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat); break; case IK2_DLS_SVD: ikJacobian.CalcDeltaThetasDLSwithSVD(); @@ -193,6 +194,7 @@ bool IKTrajectoryHelper::computeIK2( const double* q_current, int numQ, double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6]) { + MatrixRmn AugMat; bool useAngularPart = false;//for now (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false; Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors); @@ -250,12 +252,12 @@ bool IKTrajectoryHelper::computeIK2( case IK2_VEL_DLS: //ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method assert(m_data->m_dampingCoeff.GetLength() == numQ); - ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff); + ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat); break; case IK2_VEL_DLS_WITH_NULLSPACE: case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: assert(m_data->m_nullSpaceVelocity.GetLength() == numQ); - ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); + ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat); break; case IK2_DLS_SVD: ikJacobian.CalcDeltaThetasDLSwithSVD(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index f91a5e397..4a728a072 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -243,7 +243,7 @@ void Jacobian::UpdateThetaDot() m_tree->Compute(); } -void Jacobian::CalcDeltaThetas() +void Jacobian::CalcDeltaThetas(MatrixRmn& AugMat) { switch (CurrentUpdateMode) { @@ -257,7 +257,7 @@ void Jacobian::CalcDeltaThetas() CalcDeltaThetasPseudoinverse(); break; case JACOB_DLS: - CalcDeltaThetasDLS(); + CalcDeltaThetasDLS(AugMat); break; case JACOB_SDLS: CalcDeltaThetasSDLS(); @@ -327,7 +327,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse() } } -void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) +void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat) { const MatrixRmn& J = ActiveJacobian(); @@ -341,7 +341,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) // J.MultiplyTranspose( dTextra, dTheta ); // Use these two lines for the traditional DLS method - U.Solve(dS, &dT1); + U.Solve(dS, &dT1, AugMat); J.MultiplyTranspose(dT1, dTheta); // Compute JInv in damped least square form @@ -379,7 +379,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) } } -void Jacobian::CalcDeltaThetasDLS() +void Jacobian::CalcDeltaThetasDLS(MatrixRmn& AugMat) { const MatrixRmn& J = ActiveJacobian(); @@ -393,7 +393,7 @@ void Jacobian::CalcDeltaThetasDLS() // J.MultiplyTranspose( dTextra, dTheta ); // Use these two lines for the traditional DLS method - U.Solve(dS, &dT1); + U.Solve(dS, &dT1, AugMat); J.MultiplyTranspose(dT1, dTheta); // Scale back to not exceed maximum angle changes @@ -404,7 +404,7 @@ void Jacobian::CalcDeltaThetasDLS() } } -void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec) +void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat) { const MatrixRmn& J = ActiveJacobian(); @@ -414,7 +414,7 @@ void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec) dT1.SetLength(J.GetNumColumns()); J.MultiplyTranspose(dS, dT1); - U.Solve(dT1, &dTheta); + U.Solve(dT1, &dTheta, AugMat); // Scale back to not exceed maximum angle changes double maxChange = dTheta.MaxAbs(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index d7dc81bcd..915e56aa3 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -66,15 +66,15 @@ public: void SetJendTrans(MatrixRmn& J); void SetDeltaS(VectorRn& S); - void CalcDeltaThetas(); // Use this only if the Current Mode has been set. + void CalcDeltaThetas(MatrixRmn& AugMat); // Use this only if the Current Mode has been set. void ZeroDeltaThetas(); void CalcDeltaThetasTranspose(); void CalcDeltaThetasPseudoinverse(); - void CalcDeltaThetasDLS(); - void CalcDeltaThetasDLS2(const VectorRn& dVec); + void CalcDeltaThetasDLS(MatrixRmn& AugMat); + void CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); - void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV); + void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat); void UpdateThetas(); void UpdateThetaDot(); diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index 4bf1e8771..c1c20cc83 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp @@ -30,7 +30,6 @@ subject to the following restrictions: #include "MatrixRmn.h" -MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix // Fill the diagonal entries with the value d. The rest of the matrix is unchanged. void MatrixRmn::SetDiagonalEntries(double d) @@ -354,12 +353,14 @@ MatrixRmn& MatrixRmn::MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B, // Solves the equation (*this)*xVec = b; // Uses row operations. Assumes *this is square and invertible. // No error checking for divide by zero or instability (except with asserts) -void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec) const +void MatrixRmn::Solve(const VectorRn& b, VectorRn* xVec, MatrixRmn& AugMat) const { + B3_PROFILE("MatrixRmn::Solve"); assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength()); // Copy this matrix and b into an Augmented Matrix - MatrixRmn& AugMat = GetWorkMatrix(NumRows, NumCols + 1); + + AugMat.SetSize(NumRows, NumCols + 1); AugMat.LoadAsSubmatrix(*this); AugMat.SetColumn(NumRows, b); diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 86e42678b..343e79b90 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -117,7 +117,7 @@ public: 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. + void Solve(const VectorRn& b, VectorRn* x, MatrixRmn& AugMat) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible. // Row Echelon Form and Reduced Row Echelon Form routines // Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables. @@ -150,13 +150,6 @@ private: double* x; // Array of vector entries - stored in column order long AllocSize; // Allocated size of the x array - static MatrixRmn WorkMatrix; // Temporary work matrix - static MatrixRmn& GetWorkMatrix() { return WorkMatrix; } - static MatrixRmn& GetWorkMatrix(long numRows, long numCols) - { - WorkMatrix.SetSize(numRows, numCols); - return WorkMatrix; - } // Internal helper routines for SVD calculations static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag);