Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Remove a temporary global static work matrix in the BussIK, since it conflicts with multithreaded applications.

Browse Source 
Instead, let the user pass it in explicitly.
This commit is contained in:
Erwin Coumans
2020-01-11 12:43:27 -08:00
parent 83bdef8254
commit 2336dfcb9e
6 changed files with 25 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/InverseKinematics/InverseKinematicsExample.cpp
View File

@@ -100,7 +100,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
jacob->SetJendActive(); jacob->SetJendActive();
} }
jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
MatrixRmn AugMat;
// Calculate the change in theta values // Calculate the change in theta values
switch (ikMethod) switch (ikMethod)
{ {
@@ -108,7 +108,7 @@ void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method
break; break;
case IK_DLS: case IK_DLS:
jacob->CalcDeltaThetasDLS(); // Damped least squares method jacob->CalcDeltaThetasDLS(AugMat); // Damped least squares method
break; break;
case IK_DLS_SVD: case IK_DLS_SVD:
jacob->CalcDeltaThetasDLSwithSVD(); jacob->CalcDeltaThetasDLSwithSVD();

10
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -44,6 +44,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ, int endEffectorIndex, 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]) 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; 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); Jacobian ikJacobian(useAngularPart, numQ, 1);
@@ -142,12 +143,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_VEL_DLS: case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method //ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ); assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff); ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
break; break;
case IK2_VEL_DLS_WITH_NULLSPACE: case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ); assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
break; break;
case IK2_DLS_SVD: case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD(); ikJacobian.CalcDeltaThetasDLSwithSVD();
@@ -193,6 +194,7 @@ bool IKTrajectoryHelper::computeIK2(
const double* q_current, int numQ, const double* q_current, int numQ,
double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6]) 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; 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); Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors);
@@ -250,12 +252,12 @@ bool IKTrajectoryHelper::computeIK2(
case IK2_VEL_DLS: case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method //ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength() == numQ); assert(m_data->m_dampingCoeff.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff); ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
break; break;
case IK2_VEL_DLS_WITH_NULLSPACE: case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength() == numQ); assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
break; break;
case IK2_DLS_SVD: case IK2_DLS_SVD:
ikJacobian.CalcDeltaThetasDLSwithSVD(); ikJacobian.CalcDeltaThetasDLSwithSVD();

16
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -243,7 +243,7 @@ void Jacobian::UpdateThetaDot()
m_tree->Compute(); m_tree->Compute();
} }
void Jacobian::CalcDeltaThetas() void Jacobian::CalcDeltaThetas(MatrixRmn& AugMat)
{ {
switch (CurrentUpdateMode) switch (CurrentUpdateMode)
{ {
@@ -257,7 +257,7 @@ void Jacobian::CalcDeltaThetas()
CalcDeltaThetasPseudoinverse(); CalcDeltaThetasPseudoinverse();
break; break;
case JACOB_DLS: case JACOB_DLS:
CalcDeltaThetasDLS(); CalcDeltaThetasDLS(AugMat);
break; break;
case JACOB_SDLS: case JACOB_SDLS:
CalcDeltaThetasSDLS(); 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(); const MatrixRmn& J = ActiveJacobian();
@@ -341,7 +341,7 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
// J.MultiplyTranspose( dTextra, dTheta ); // J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method // Use these two lines for the traditional DLS method
U.Solve(dS, &dT1); U.Solve(dS, &dT1, AugMat);
J.MultiplyTranspose(dT1, dTheta); J.MultiplyTranspose(dT1, dTheta);
// Compute JInv in damped least square form // 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(); const MatrixRmn& J = ActiveJacobian();
@@ -393,7 +393,7 @@ void Jacobian::CalcDeltaThetasDLS()
// J.MultiplyTranspose( dTextra, dTheta ); // J.MultiplyTranspose( dTextra, dTheta );
// Use these two lines for the traditional DLS method // Use these two lines for the traditional DLS method
U.Solve(dS, &dT1); U.Solve(dS, &dT1, AugMat);
J.MultiplyTranspose(dT1, dTheta); J.MultiplyTranspose(dT1, dTheta);
// Scale back to not exceed maximum angle changes // 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(); const MatrixRmn& J = ActiveJacobian();
@@ -414,7 +414,7 @@ void Jacobian::CalcDeltaThetasDLS2(const VectorRn& dVec)
dT1.SetLength(J.GetNumColumns()); dT1.SetLength(J.GetNumColumns());
J.MultiplyTranspose(dS, dT1); J.MultiplyTranspose(dS, dT1);
U.Solve(dT1, &dTheta); U.Solve(dT1, &dTheta, AugMat);
// Scale back to not exceed maximum angle changes // Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs(); double maxChange = dTheta.MaxAbs();

8
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -66,15 +66,15 @@ public:
void SetJendTrans(MatrixRmn& J); void SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S); 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 ZeroDeltaThetas();
void CalcDeltaThetasTranspose(); void CalcDeltaThetasTranspose();
void CalcDeltaThetasPseudoinverse(); void CalcDeltaThetasPseudoinverse();
void CalcDeltaThetasDLS(); void CalcDeltaThetasDLS(MatrixRmn& AugMat);
void CalcDeltaThetasDLS2(const VectorRn& dVec); void CalcDeltaThetasDLS2(const VectorRn& dVec, MatrixRmn& AugMat);
void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS(); void CalcDeltaThetasSDLS();
void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV); void CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV, MatrixRmn& AugMat);
void UpdateThetas(); void UpdateThetas();
void UpdateThetaDot(); void UpdateThetaDot();

7
examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
View File

@@ -30,7 +30,6 @@ subject to the following restrictions:
#include "MatrixRmn.h" #include "MatrixRmn.h"
MatrixRmn MatrixRmn::WorkMatrix; // Temporary work matrix
// Fill the diagonal entries with the value d. The rest of the matrix is unchanged. // Fill the diagonal entries with the value d. The rest of the matrix is unchanged.
void MatrixRmn::SetDiagonalEntries(double d) void MatrixRmn::SetDiagonalEntries(double d)
@@ -354,12 +353,14 @@ MatrixRmn& MatrixRmn::MultiplyTranspose(const MatrixRmn& A, const MatrixRmn& B,
// Solves the equation (*this)*xVec = b; // Solves the equation (*this)*xVec = b;
// Uses row operations. Assumes *this is square and invertible. // Uses row operations. Assumes *this is square and invertible.
// No error checking for divide by zero or instability (except with asserts) // 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()); assert(NumRows == NumCols && NumCols == xVec->GetLength() && NumRows == b.GetLength());
// Copy this matrix and b into an Augmented Matrix // Copy this matrix and b into an Augmented Matrix
MatrixRmn& AugMat = GetWorkMatrix(NumRows, NumCols + 1);
AugMat.SetSize(NumRows, NumCols + 1);
AugMat.LoadAsSubmatrix(*this); AugMat.LoadAsSubmatrix(*this);
AugMat.SetColumn(NumRows, b); AugMat.SetColumn(NumRows, b);

9
examples/ThirdPartyLibs/BussIK/MatrixRmn.h
View File

@@ -117,7 +117,7 @@ public:
MatrixRmn& AddToDiagonal(const VectorRn& dVec); MatrixRmn& AddToDiagonal(const VectorRn& dVec);
// Solving systems of linear equations // 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 and Reduced Row Echelon Form routines
// Row echelon form here allows non-negative entries (instead of 1's) in the positions of lead variables. // 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 double* x; // Array of vector entries - stored in column order
long AllocSize; // Allocated size of the x array 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 // Internal helper routines for SVD calculations
static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag); static void CalcBidiagonal(MatrixRmn& U, MatrixRmn& V, VectorRn& w, VectorRn& superDiag);