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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -30,7 +30,6 @@ subject to the following restrictions:
#include <iostream>
using namespace std;
#include "Jacobian.h"
void Arrow(const VectorR3& tail, const VectorR3& head);
@@ -39,15 +38,15 @@ void Arrow(const VectorR3& tail, const VectorR3& head);
//extern VectorR3 target1[];
// Optimal damping values have to be determined in an ad hoc manner (Yuck!)
const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter)
const double Jacobian::DefaultDampingLambda = 0.6; // Optimal for the "Y" shape (any lower gives jitter)
//const double Jacobian::DefaultDampingLambda = 1.1; // Optimal for the DLS "double Y" shape (any lower gives jitter)
// const double Jacobian::DefaultDampingLambda = 0.7; // Optimal for the DLS "double Y" shape with distance clamping (lower gives jitter)
const double Jacobian::PseudoInverseThresholdFactor = 0.01;
const double Jacobian::MaxAngleJtranspose = 30.0*DegreesToRadians;
const double Jacobian::MaxAnglePseudoinverse = 5.0*DegreesToRadians;
const double Jacobian::MaxAngleDLS = 45.0*DegreesToRadians;
const double Jacobian::MaxAngleSDLS = 45.0*DegreesToRadians;
const double Jacobian::MaxAngleJtranspose = 30.0 * DegreesToRadians;
const double Jacobian::MaxAnglePseudoinverse = 5.0 * DegreesToRadians;
const double Jacobian::MaxAngleDLS = 45.0 * DegreesToRadians;
const double Jacobian::MaxAngleSDLS = 45.0 * DegreesToRadians;
const double Jacobian::BaseMaxTargetDist = 0.4;
Jacobian::Jacobian(Tree* tree)
@@ -55,86 +54,85 @@ Jacobian::Jacobian(Tree* tree)
m_tree = tree;
m_nEffector = tree->GetNumEffector();
nJoint = tree->GetNumJoint();
nRow = 3 * m_nEffector; // Include only the linear part
nRow = 3 * m_nEffector; // Include only the linear part
nCol = nJoint;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetZero();
SetJendActive();
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w .SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w.SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dPreTheta.SetLength(nCol);
// Used by Jacobian transpose method & DLS & SDLS
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
// Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow);
dSclamp.SetLength(m_nEffector);
errorArray.SetLength(m_nEffector);
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
Jacobian::Jacobian(bool useAngularJacobian,int nDof)
Jacobian::Jacobian(bool useAngularJacobian, int nDof)
{
m_tree = 0;
m_nEffector = 1;
if (useAngularJacobian)
{
nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
} else
nRow = 2 * 3 * m_nEffector; // Include both linear and angular part
}
else
{
nRow = 3 * m_nEffector; // Include only the linear part
nRow = 3 * m_nEffector; // Include only the linear part
}
nCol = nDof;
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetSize(nRow, nCol); // The Jocobian matrix
Jend.SetZero();
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetSize(nRow, nCol); // The Jacobian matrix based on target positions
Jtarget.SetZero();
SetJendActive();
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w .SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
U.SetSize(nRow, nRow); // The U matrix for SVD calculations
w.SetLength(Min(nRow, nCol));
V.SetSize(nCol, nCol); // The V matrix for SVD calculations
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dS.SetLength(nRow); // (Target positions) - (End effector positions)
dTheta.SetLength(nCol); // Changes in joint angles
dPreTheta.SetLength(nCol);
// Used by Jacobian transpose method & DLS & SDLS
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
dT1.SetLength(nRow); // Linearized change in end effector positions based on dTheta
// Used by the Selectively Damped Least Squares Method
//dT.SetLength(nRow);
dSclamp.SetLength(m_nEffector);
errorArray.SetLength(m_nEffector);
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Jnorms.SetSize(m_nEffector, nCol); // Holds the norms of the active J matrix
Reset();
}
void Jacobian::Reset()
void Jacobian::Reset()
{
// Used by Damped Least Squares Method
DampingLambda = DefaultDampingLambda;
DampingLambdaSq = Square(DampingLambda);
// DampingLambdaSDLS = 1.5*DefaultDampingLambda;
dSclamp.Fill(HUGE_VAL);
}
@@ -142,14 +140,16 @@ void Jacobian::Reset()
// Compute the J and K matrices (the Jacobians)
void Jacobian::ComputeJacobian(VectorR3* targets)
{
if (m_tree==0)
if (m_tree == 0)
return;
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
@@ -161,86 +161,92 @@ void Jacobian::ComputeJacobian(VectorR3* targets)
// Find all ancestors (they will usually all be joints)
// Set the corresponding entries in the Jacobians J, K.
Node* m = m_tree->GetParent(n);
while ( m ) {
while (m)
{
int j = m->GetJointNum();
assert ( 0 <=i && i<m_nEffector && 0<=j && j<nJoint );
if ( m->IsFrozen() ) {
assert(0 <= i && i < m_nEffector && 0 <= j && j < nJoint);
if (m->IsFrozen())
{
Jend.SetTriple(i, j, VectorR3::Zero);
Jtarget.SetTriple(i, j, VectorR3::Zero);
}
else {
temp = m->GetS(); // joint pos.
temp -= n->GetS(); // -(end effector pos. - joint pos.)
temp *= m->GetW(); // cross product with joint rotation axis
Jend.SetTriple(i, j, temp);
temp = m->GetS(); // joint pos.
temp -= targetPos; // -(target pos. - joint pos.)
temp *= m->GetW(); // cross product with joint rotation axis
Jtarget.SetTriple(i, j, temp);
else
{
temp = m->GetS(); // joint pos.
temp -= n->GetS(); // -(end effector pos. - joint pos.)
temp *= m->GetW(); // cross product with joint rotation axis
Jend.SetTriple(i, j, temp);
temp = m->GetS(); // joint pos.
temp -= targetPos; // -(target pos. - joint pos.)
temp *= m->GetW(); // cross product with joint rotation axis
Jtarget.SetTriple(i, j, temp);
}
m = m_tree->GetParent( m );
m = m_tree->GetParent(m);
}
}
n = m_tree->GetSuccessor( n );
n = m_tree->GetSuccessor(n);
}
}
void Jacobian::SetJendTrans(MatrixRmn& J)
{
Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
Jend.LoadAsSubmatrix(J);
Jend.SetSize(J.GetNumRows(), J.GetNumColumns());
Jend.LoadAsSubmatrix(J);
}
void Jacobian::SetDeltaS(VectorRn& S)
{
dS.Set(S);
dS.Set(S);
}
// The delta theta values have been computed in dTheta array
// Apply the delta theta values to the joints
// Nothing is done about joint limits for now.
void Jacobian::UpdateThetas()
void Jacobian::UpdateThetas()
{
// Traverse the tree to find all joints
// Update the joint angles
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsJoint() ) {
while (n)
{
if (n->IsJoint())
{
int i = n->GetJointNum();
n->AddToTheta( dTheta[i] );
n->AddToTheta(dTheta[i]);
}
n = m_tree->GetSuccessor( n );
n = m_tree->GetSuccessor(n);
}
// Update the positions and rotation axes of all joints/effectors
m_tree->Compute();
}
void Jacobian::UpdateThetaDot()
{
if (m_tree==0)
if (m_tree == 0)
return;
// Traverse the tree to find all joints
// Update the joint angles
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsJoint() ) {
int i = n->GetJointNum();
n->UpdateTheta( dTheta[i] );
}
n = m_tree->GetSuccessor( n );
}
// Update the positions and rotation axes of all joints/effectors
m_tree->Compute();
// Traverse the tree to find all joints
// Update the joint angles
Node* n = m_tree->GetRoot();
while (n)
{
if (n->IsJoint())
{
int i = n->GetJointNum();
n->UpdateTheta(dTheta[i]);
}
n = m_tree->GetSuccessor(n);
}
// Update the positions and rotation axes of all joints/effectors
m_tree->Compute();
}
void Jacobian::CalcDeltaThetas()
void Jacobian::CalcDeltaThetas()
{
switch (CurrentUpdateMode) {
switch (CurrentUpdateMode)
{
case JACOB_Undefined:
ZeroDeltaThetas();
break;
@@ -270,162 +276,165 @@ void Jacobian::CalcDeltaThetasTranspose()
{
const MatrixRmn& J = ActiveJacobian();
J.MultiplyTranspose( dS, dTheta );
J.MultiplyTranspose(dS, dTheta);
// Scale back the dTheta values greedily
J.Multiply ( dTheta, dT1 ); // dT = J * dTheta
double alpha = Dot(dS,dT1) / dT1.NormSq();
assert ( alpha>0.0 );
// Scale back the dTheta values greedily
J.Multiply(dTheta, dT1); // dT = J * dTheta
double alpha = Dot(dS, dT1) / dT1.NormSq();
assert(alpha > 0.0);
// Also scale back to be have max angle change less than MaxAngleJtranspose
double maxChange = dTheta.MaxAbs();
double beta = MaxAngleJtranspose/maxChange;
double beta = MaxAngleJtranspose / maxChange;
dTheta *= Min(alpha, beta);
}
void Jacobian::CalcDeltaThetasPseudoinverse()
{
void Jacobian::CalcDeltaThetasPseudoinverse()
{
MatrixRmn& J = const_cast<MatrixRmn&>(ActiveJacobian());
// Compute Singular Value Decomposition
// Compute Singular Value Decomposition
// This an inefficient way to do Pseudoinverse, but it is convenient since we need SVD anyway
J.ComputeSVD( U, w, V );
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w , V));
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the DLS solution.
// Delta target values are the dS values
// We multiply by Moore-Penrose pseudo-inverse of the J matrix
double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs();
double pseudoInverseThreshold = PseudoInverseThresholdFactor * w.MaxAbs();
long diagLength = w.GetLength();
double* wPtr = w.GetPtr();
dTheta.SetZero();
for ( long i=0; i<diagLength; i++ ) {
double dotProdCol = U.DotProductColumn( dS, i ); // Dot product with i-th column of U
for (long i = 0; i < diagLength; i++)
{
double dotProdCol = U.DotProductColumn(dS, i); // Dot product with i-th column of U
double alpha = *(wPtr++);
if ( fabs(alpha)>pseudoInverseThreshold ) {
alpha = 1.0/alpha;
MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol*alpha );
if (fabs(alpha) > pseudoInverseThreshold)
{
alpha = 1.0 / alpha;
MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
}
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAnglePseudoinverse ) {
dTheta *= MaxAnglePseudoinverse/maxChange;
if (maxChange > MaxAnglePseudoinverse)
{
dTheta *= MaxAnglePseudoinverse / maxChange;
}
}
void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV)
{
{
const MatrixRmn& J = ActiveJacobian();
MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T)
U.AddToDiagonal( DampingLambdaSq );
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 );
// 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);
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);
// Compute residual
VectorRn residual(J.GetNumRows());
J.Multiply(nullV, residual);
// TODO: Use residual to set the null space term coefficient adaptively.
//printf("residual: %f\n", residual.Norm());
// Add null space velocity
dTheta += nullV;
// Add null space velocity
dTheta += nullV;
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleDLS ) {
dTheta *= MaxAngleDLS/maxChange;
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
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;
}
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::CalcDeltaThetasDLS2(const VectorRn& dVec)
{
const MatrixRmn& J = ActiveJacobian();
U.SetSize(J.GetNumColumns(), J.GetNumColumns());
MatrixRmn::TransposeMultiply(J, J, U);
U.AddToDiagonal( dVec );
dT1.SetLength(J.GetNumColumns());
J.MultiplyTranspose(dS, dT1);
U.Solve(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();
// Compute Singular Value Decomposition
U.SetSize(J.GetNumColumns(), J.GetNumColumns());
MatrixRmn::TransposeMultiply(J, J, U);
U.AddToDiagonal(dVec);
dT1.SetLength(J.GetNumColumns());
J.MultiplyTranspose(dS, dT1);
U.Solve(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();
// Compute Singular Value Decomposition
// This an inefficient way to do DLS, but it is convenient since we need SVD anyway
J.ComputeSVD( U, w, V );
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w , V));
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the DLS solution.
// Delta target values are the dS values
@@ -433,31 +442,32 @@ void Jacobian::CalcDeltaThetasDLSwithSVD()
long diagLength = w.GetLength();
double* wPtr = w.GetPtr();
dTheta.SetZero();
for ( long i=0; i<diagLength; i++ ) {
double dotProdCol = U.DotProductColumn( dS, i ); // Dot product with i-th column of U
for (long i = 0; i < diagLength; i++)
{
double dotProdCol = U.DotProductColumn(dS, i); // Dot product with i-th column of U
double alpha = *(wPtr++);
alpha = alpha/(Square(alpha)+DampingLambdaSq);
MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol*alpha );
alpha = alpha / (Square(alpha) + DampingLambdaSq);
MatrixRmn::AddArrayScale(V.GetNumRows(), V.GetColumnPtr(i), 1, dTheta.GetPtr(), 1, dotProdCol * alpha);
}
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleDLS ) {
dTheta *= MaxAngleDLS/maxChange;
if (maxChange > MaxAngleDLS)
{
dTheta *= MaxAngleDLS / maxChange;
}
}
void Jacobian::CalcDeltaThetasSDLS()
{
void Jacobian::CalcDeltaThetasSDLS()
{
const MatrixRmn& J = ActiveJacobian();
// Compute Singular Value Decomposition
// Compute Singular Value Decomposition
J.ComputeSVD( U, w, V );
J.ComputeSVD(U, w, V);
// Next line for debugging only
assert(J.DebugCheckSVD(U, w , V));
assert(J.DebugCheckSVD(U, w, V));
// Calculate response vector dTheta that is the SDLS solution.
// Delta target values are the dS values
@@ -469,9 +479,10 @@ void Jacobian::CalcDeltaThetasSDLS()
// Calculate the norms of the 3-vectors in the Jacobian
long i;
const double *jx = J.GetPtr();
double *jnx = Jnorms.GetPtr();
for ( i=nCols*numEndEffectors; i>0; i-- ) {
const double* jx = J.GetPtr();
double* jnx = Jnorms.GetPtr();
for (i = nCols * numEndEffectors; i > 0; i--)
{
double accumSq = Square(*(jx++));
accumSq += Square(*(jx++));
accumSq += Square(*(jx++));
@@ -482,27 +493,29 @@ void Jacobian::CalcDeltaThetasSDLS()
CalcdTClampedFromdS();
// Loop over each singular vector
for ( i=0; i<nRows; i++ ) {
for (i = 0; i < nRows; i++)
{
double wiInv = w[i];
if ( NearZero(wiInv,1.0e-10) ) {
if (NearZero(wiInv, 1.0e-10))
{
continue;
}
wiInv = 1.0/wiInv;
wiInv = 1.0 / wiInv;
double N = 0.0; // N is the quasi-1-norm of the i-th column of U
double alpha = 0.0; // alpha is the dot product of dT and the i-th column of U
double N = 0.0; // N is the quasi-1-norm of the i-th column of U
double alpha = 0.0; // alpha is the dot product of dT and the i-th column of U
const double *dTx = dT1.GetPtr();
const double *ux = U.GetColumnPtr(i);
const double* dTx = dT1.GetPtr();
const double* ux = U.GetColumnPtr(i);
long j;
for ( j=numEndEffectors; j>0; j-- ) {
for (j = numEndEffectors; j > 0; j--)
{
double tmp;
alpha += (*ux)*(*(dTx++));
tmp = Square( *(ux++) );
alpha += (*ux)*(*(dTx++));
alpha += (*ux) * (*(dTx++));
tmp = Square(*(ux++));
alpha += (*ux) * (*(dTx++));
tmp += Square(*(ux++));
alpha += (*ux)*(*(dTx++));
alpha += (*ux) * (*(dTx++));
tmp += Square(*(ux++));
N += sqrt(tmp);
}
@@ -510,29 +523,32 @@ void Jacobian::CalcDeltaThetasSDLS()
// M is the quasi-1-norm of the response to angles changing according to the i-th column of V
// Then is multiplied by the wiInv value.
double M = 0.0;
double *vx = V.GetColumnPtr(i);
double* vx = V.GetColumnPtr(i);
jnx = Jnorms.GetPtr();
for ( j=nCols; j>0; j-- ) {
double accum=0.0;
for ( long k=numEndEffectors; k>0; k-- ) {
for (j = nCols; j > 0; j--)
{
double accum = 0.0;
for (long k = numEndEffectors; k > 0; k--)
{
accum += *(jnx++);
}
M += fabs((*(vx++)))*accum;
M += fabs((*(vx++))) * accum;
}
M *= fabs(wiInv);
double gamma = MaxAngleSDLS;
if ( N<M ) {
gamma *= N/M; // Scale back maximum permissable joint angle
if (N < M)
{
gamma *= N / M; // Scale back maximum permissable joint angle
}
// Calculate the dTheta from pure pseudoinverse considerations
double scale = alpha*wiInv; // This times i-th column of V is the psuedoinverse response
dPreTheta.LoadScaled( V.GetColumnPtr(i), scale );
double scale = alpha * wiInv; // This times i-th column of V is the psuedoinverse response
dPreTheta.LoadScaled(V.GetColumnPtr(i), scale);
// Now rescale the dTheta values.
double max = dPreTheta.MaxAbs();
double rescale = (gamma)/(gamma+max);
dTheta.AddScaled(dPreTheta,rescale);
double rescale = (gamma) / (gamma + max);
dTheta.AddScaled(dPreTheta, rescale);
/*if ( gamma<max) {
dTheta.AddScaled( dPreTheta, gamma/max );
}
@@ -543,28 +559,32 @@ void Jacobian::CalcDeltaThetasSDLS()
// Scale back to not exceed maximum angle changes
double maxChange = dTheta.MaxAbs();
if ( maxChange>MaxAngleSDLS ) {
dTheta *= MaxAngleSDLS/(MaxAngleSDLS+maxChange);
if (maxChange > MaxAngleSDLS)
{
dTheta *= MaxAngleSDLS / (MaxAngleSDLS + maxChange);
//dTheta *= MaxAngleSDLS/maxChange;
}
}
void Jacobian::CalcdTClampedFromdS()
void Jacobian::CalcdTClampedFromdS()
{
long len = dS.GetLength();
long j = 0;
for ( long i=0; i<len; i+=3, j++ ) {
double normSq = Square(dS[i])+Square(dS[i+1])+Square(dS[i+2]);
if ( normSq>Square(dSclamp[j]) ) {
double factor = dSclamp[j]/sqrt(normSq);
dT1[i] = dS[i]*factor;
dT1[i+1] = dS[i+1]*factor;
dT1[i+2] = dS[i+2]*factor;
for (long i = 0; i < len; i += 3, j++)
{
double normSq = Square(dS[i]) + Square(dS[i + 1]) + Square(dS[i + 2]);
if (normSq > Square(dSclamp[j]))
{
double factor = dSclamp[j] / sqrt(normSq);
dT1[i] = dS[i] * factor;
dT1[i + 1] = dS[i + 1] * factor;
dT1[i + 2] = dS[i + 2] * factor;
}
else {
else
{
dT1[i] = dS[i];
dT1[i+1] = dS[i+1];
dT1[i+2] = dS[i+2];
dT1[i + 1] = dS[i + 1];
dT1[i + 2] = dS[i + 2];
}
}
}
@@ -576,8 +596,10 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
temp = targetPos;
@@ -586,7 +608,7 @@ double Jacobian::UpdateErrorArray(VectorR3* targets)
errorArray[i] = err;
totalError += err;
}
n = m_tree->GetSuccessor( n );
n = m_tree->GetSuccessor(n);
}
return totalError;
}
@@ -596,8 +618,10 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
// Traverse tree to find all end effectors
VectorR3 temp;
Node* n = m_tree->GetRoot();
while ( n ) {
if ( n->IsEffector() ) {
while (n)
{
if (n->IsEffector())
{
int i = n->GetEffectorNum();
const VectorR3& targetPos = targets[i];
@@ -605,40 +629,44 @@ void Jacobian::UpdatedSClampValue(VectorR3* targets)
// While we are at it, also update the clamping values in dSclamp;
temp = targetPos;
temp -= n->GetS();
double normSi = sqrt(Square(dS[i])+Square(dS[i+1])+Square(dS[i+2]));
double changedDist = temp.Norm()-normSi;
if ( changedDist>0.0 ) {
double normSi = sqrt(Square(dS[i]) + Square(dS[i + 1]) + Square(dS[i + 2]));
double changedDist = temp.Norm() - normSi;
if (changedDist > 0.0)
{
dSclamp[i] = BaseMaxTargetDist + changedDist;
}
else {
else
{
dSclamp[i] = BaseMaxTargetDist;
}
}
n = m_tree->GetSuccessor( n );
n = m_tree->GetSuccessor(n);
}
}
void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 )
void Jacobian::CompareErrors(const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2)
{
const VectorRn& e1 = j1.errorArray;
const VectorRn& e2 = j2.errorArray;
double ret1 = 0.0;
double ret2 = 0.0;
int len = e1.GetLength();
for ( long i=0; i<len; i++ ) {
for (long i = 0; i < len; i++)
{
double v1 = e1[i];
double v2 = e2[i];
if ( v1<v2 ) {
ret1 += v1/v2;
if (v1 < v2)
{
ret1 += v1 / v2;
ret2 += 1.0;
}
else if ( v1 != 0.0 ) {
else if (v1 != 0.0)
{
ret1 += 1.0;
ret2 += v2/v1;
ret2 += v2 / v1;
}
else {
else
{
ret1 += 0.0;
ret2 += 0.0;
}
@@ -647,22 +675,26 @@ void Jacobian::CompareErrors( const Jacobian& j1, const Jacobian& j2, double* we
*weightedDist2 = ret2;
}
void Jacobian::CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies )
void Jacobian::CountErrors(const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies)
{
const VectorRn& e1 = j1.errorArray;
const VectorRn& e2 = j2.errorArray;
int b1=0, b2=0, tie=0;
int b1 = 0, b2 = 0, tie = 0;
int len = e1.GetLength();
for ( long i=0; i<len; i++ ) {
for (long i = 0; i < len; i++)
{
double v1 = e1[i];
double v2 = e2[i];
if ( v1<v2 ) {
if (v1 < v2)
{
b1++;
}
else if ( v2<v1 ) {
else if (v2 < v1)
{
b2++;
}
else {
else
{
tie++;
}
}
@@ -756,7 +788,3 @@ void Jacobian::CalcDeltaThetasSDLSrev2()
dTheta *= MaxAngleSDLS/maxChange;
}
} */