AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Fix memory leak in IK.

Browse Source
This commit is contained in:
yunfeibai 2016-11-04 10:47:07 -07:00
parent a49d2b289e
commit 7bd585fa87
1 changed files with 21 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@ -18,7 +18,6 @@ struct IKTrajectoryHelperInternalData
VectorRn m_nullSpaceVelocity;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
IKTrajectoryHelperInternalData()
{
@ -48,23 +47,21 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
{
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);
// Reset(m_ikTree,m_ikJacobian);
Jacobian ikJacobian(useAngularPart,numQ);
m_data->m_ikJacobian->Reset();
ikJacobian.Reset();
bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) {
m_data->m_ikJacobian->SetJtargetActive();
ikJacobian.SetJtargetActive();
}
else {
m_data->m_ikJacobian->SetJendActive();
ikJacobian.SetJendActive();
}
VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
ikJacobian.ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
// Set one end effector world position from Bullet
VectorRn deltaS(3);
@ -112,8 +109,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
}
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
} else
{
VectorRn deltaC(3);
@ -126,53 +123,53 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
}
}
m_data->m_ikJacobian->SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian);
ikJacobian.SetDeltaS(deltaC);
ikJacobian.SetJendTrans(completeJacobian);
}
}
// Calculate the change in theta values
switch (ikMethod) {
case IK2_JACOB_TRANS:
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
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);
ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
ikJacobian.CalcDeltaThetasDLSwithSVD();
break;
case IK2_PURE_PSEUDO:
m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:
m_data->m_ikJacobian->ZeroDeltaThetas();
ikJacobian.ZeroDeltaThetas();
break;
}
// Use for velocity IK, update theta dot
//m_data->m_ikJacobian->UpdateThetaDot();
//ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta
//m_data->m_ikJacobian->UpdateThetas();
//ikJacobian.UpdateThetas();
// Apply the change in the theta values
//m_data->m_ikJacobian->UpdatedSClampValue(&targets);
//ikJacobian.UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++)
{
// Use for velocity IK
q_new[i] = m_data->m_ikJacobian->dTheta[i] + q_current[i];
q_new[i] = ikJacobian.dTheta[i] + q_current[i];
// Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta();
@ -203,4 +200,4 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current,
}
}
return true;
}
}