AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-12 21:00:11 +00:00
Code Issues 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
Show Stats 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->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();

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,
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();

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

@ -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();

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

@ -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();

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

@ -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);

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

@ -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);