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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
erwincoumans
2016-09-30 08:09:15 -07:00
parent 032ffe427e c1657c6d5a
commit 591a55d913
13 changed files with 292 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -195,7 +195,7 @@ public:
ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION;
ikargs.m_flags |= /*B3_HAS_IK_TARGET_ORIENTATION +*/ B3_HAS_NULL_SPACE_VELOCITY;
ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
@@ -203,6 +203,41 @@ public:
ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
ikargs.m_endEffectorLinkIndex = 6;
// Settings based on default KUKA arm setting
ikargs.m_lowerLimits.resize(numJoints);
ikargs.m_upperLimits.resize(numJoints);
ikargs.m_jointRanges.resize(numJoints);
ikargs.m_restPoses.resize(numJoints);
ikargs.m_lowerLimits[0] = -2.32;
ikargs.m_lowerLimits[1] = -1.6;
ikargs.m_lowerLimits[2] = -2.32;
ikargs.m_lowerLimits[3] = -1.6;
ikargs.m_lowerLimits[4] = -2.32;
ikargs.m_lowerLimits[5] = -1.6;
ikargs.m_lowerLimits[6] = -2.4;
ikargs.m_upperLimits[0] = 2.32;
ikargs.m_upperLimits[1] = 1.6;
ikargs.m_upperLimits[2] = 2.32;
ikargs.m_upperLimits[3] = 1.6;
ikargs.m_upperLimits[4] = 2.32;
ikargs.m_upperLimits[5] = 1.6;
ikargs.m_upperLimits[6] = 2.4;
ikargs.m_jointRanges[0] = 5.8;
ikargs.m_jointRanges[1] = 4;
ikargs.m_jointRanges[2] = 5.8;
ikargs.m_jointRanges[3] = 4;
ikargs.m_jointRanges[4] = 5.8;
ikargs.m_jointRanges[5] = 4;
ikargs.m_jointRanges[6] = 6;
ikargs.m_restPoses[0] = 0;
ikargs.m_restPoses[1] = 0;
ikargs.m_restPoses[2] = 0;
ikargs.m_restPoses[3] = SIMD_HALF_PI;
ikargs.m_restPoses[4] = 0;
ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
ikargs.m_restPoses[6] = 0;
ikargs.m_numDegreeOfFreedom = numJoints;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{
//copy the IK result to the desired state of the motor/actuator
@@ -210,10 +245,10 @@ public:
{
b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
t.m_maxTorqueValue = 100;
t.m_kp= 1;
t.m_maxTorqueValue = 100.0;
t.m_kp= 1.0;
t.m_targetVelocity = 0;
t.m_kp = 0.5;
t.m_kd = 1.0;
m_robotSim.setJointMotorControl(m_kukaIndex,i,t);
}

10
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -485,10 +485,16 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId);
if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
{
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
} else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
{
b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
} else
} else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
{
b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
} else
{
b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
}

6
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -85,6 +85,7 @@ struct b3JointMotorArgs
enum b3InverseKinematicsFlags
{
B3_HAS_IK_TARGET_ORIENTATION=1,
B3_HAS_NULL_SPACE_VELOCITY=2,
};
struct b3RobotSimInverseKinematicArgs
@@ -96,6 +97,11 @@ struct b3RobotSimInverseKinematicArgs
double m_endEffectorTargetOrientation[4];
int m_endEffectorLinkIndex;
int m_flags;
int m_numDegreeOfFreedom;
b3AlignedObjectArray<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges;
b3AlignedObjectArray<double> m_restPoses;
b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1),

56
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -15,7 +15,7 @@
struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
VectorRn m_nullSpaceVelocity;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
@@ -23,6 +23,7 @@ struct IKTrajectoryHelperInternalData
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
}
};
@@ -45,7 +46,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])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false;
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ);
@@ -74,16 +75,19 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
// Set one end effector world orientation from Bullet
VectorRn deltaR(3);
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
if (useAngularPart)
{
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
}
{
@@ -131,6 +135,11 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_VEL_DLS_WITH_ORIENTATION:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
break;
@@ -164,3 +173,28 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
}
return true;
}
bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
{
m_data->m_nullSpaceVelocity.SetLength(numQ);
m_data->m_nullSpaceVelocity.SetZero();
double stayCloseToZeroGain = 0.1;
double stayAwayFromLimitsGain = 10.0;
// Stay close to zero
for (int i = 0; i < numQ; ++i)
{
m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i]-q_current[i]);
}
// Stay away from joint limits
for (int i = 0; i < numQ; ++i) {
if (q_current[i] > upper_limit[i]) {
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
}
if (q_current[i] < lower_limit[i]) {
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
}
}
return true;
}

4
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -10,6 +10,8 @@ enum IK2_Method
IK2_DLS_SVD,
IK2_VEL_DLS,
IK2_VEL_DLS_WITH_ORIENTATION,
IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
};
@@ -28,5 +30,7 @@ public:
const double* q_old, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
};
#endif //IK_TRAJECTORY_HELPER_H

47
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1391,6 +1391,53 @@ void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemory
}
void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
}
}
void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
}
}
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -133,6 +133,8 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]);
void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

38
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2665,7 +2665,43 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> q_new;
q_new.resize(numDofs);
int ikMethod= (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)? IK2_VEL_DLS_WITH_ORIENTATION : IK2_VEL_DLS;
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
ikMethod = IK2_VEL_DLS;
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
rest_pose.resize(numDofs);
for (int i = 0; i < numDofs; ++i)
{
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
}
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;

7
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -395,7 +395,8 @@ enum EnumCalculateInverseKinematicsFlags
{
IK_HAS_TARGET_POSITION=1,
IK_HAS_TARGET_ORIENTATION=2,
//IK_HAS_CURRENT_JOINT_POSITIONS=4,//not used yet
IK_HAS_NULL_SPACE_VELOCITY=4,
//IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet
};
struct CalculateInverseKinematicsArgs
@@ -405,6 +406,10 @@ struct CalculateInverseKinematicsArgs
double m_targetPosition[3];
double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
int m_endEffectorLinkIndex;
double m_lowerLimit[MAX_DEGREE_OF_FREEDOM];
double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
double m_jointRange[MAX_DEGREE_OF_FREEDOM];
double m_restPose[MAX_DEGREE_OF_FREEDOM];
};
struct CalculateInverseKinematicsResultArgs

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

@@ -320,7 +320,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse()
}
void Jacobian::CalcDeltaThetasDLS()
void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
{
const MatrixRmn& J = ActiveJacobian();
@@ -337,6 +337,27 @@ void Jacobian::CalcDeltaThetasDLS()
U.Solve( dS, &dT1 );
J.MultiplyTranspose( dT1, dTheta );
// Compute JInv in damped least square form
MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns());
U.ComputeInverse(UInv);
assert(U.DebugCheckInverse(UInv));
MatrixRmn JInv(J.GetNumColumns(), J.GetNumRows());
MatrixRmn::TransposeMultiply(J, UInv, JInv);
// Compute null space projection
MatrixRmn JInvJ(J.GetNumColumns(), J.GetNumColumns());
MatrixRmn::Multiply(JInv, J, JInvJ);
MatrixRmn P(J.GetNumColumns(), J.GetNumColumns());
P.SetIdentity();
P -= JInvJ;
// Compute null space velocity
VectorRn nullV(J.GetNumColumns());
P.Multiply(desiredV, nullV);
// Add null space velocity
dTheta += nullV;
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleDLS ) {
@@ -344,6 +365,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();

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

@@ -70,7 +70,7 @@ public:
void CalcDeltaThetasDLS();
void CalcDeltaThetasDLSwithSVD();
void CalcDeltaThetasSDLS();
void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV);
void UpdateThetas();
void UpdateThetaDot();

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

@@ -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.
@@ -943,6 +973,21 @@ bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const Matr
return ret;
}
bool MatrixRmn::DebugCheckInverse( const MatrixRmn& MInv ) const
{
assert ( this->NumRows==this->NumCols );
assert ( MInv.NumRows==MInv.NumCols );
MatrixRmn I(this->NumRows, this->NumCols);
I.SetIdentity();
MatrixRmn MMInv(this->NumRows, this->NumCols);
Multiply(*this, MInv, MMInv);
I -= MMInv;
double error = I.FrobeniusNorm();
bool ret = ( fabs(error)<=1.0e-13 );
assert ( ret );
return ret;
}
bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const
{
// Special SVD test code

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

@@ -129,6 +129,10 @@ 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;
// Debug matrix inverse computation
bool DebugCheckInverse( const MatrixRmn& MInv ) 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 );