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

Merge pull request #852 from YunfeiBai/master

Browse Source 
Fix memory leak in IK.
This commit is contained in:
erwincoumans
2016-11-05 13:31:31 -07:00
committed by GitHub
parent c738e2269e 7bd585fa87
commit 0bf3a96365
1 changed files with 21 additions and 24 deletions
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; VectorRn m_nullSpaceVelocity;
b3AlignedObjectArray<Node*> m_ikNodes; b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
IKTrajectoryHelperInternalData() 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; 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); Jacobian ikJacobian(useAngularPart,numQ);
// Reset(m_ikTree,m_ikJacobian);
m_data->m_ikJacobian->Reset(); ikJacobian.Reset();
bool UseJacobianTargets1 = false; bool UseJacobianTargets1 = false;
if ( UseJacobianTargets1 ) { if ( UseJacobianTargets1 ) {
m_data->m_ikJacobian->SetJtargetActive(); ikJacobian.SetJtargetActive();
} }
else { else {
m_data->m_ikJacobian->SetJendActive(); ikJacobian.SetJendActive();
} }
VectorR3 targets; VectorR3 targets;
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]); 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 // Set one end effector world position from Bullet
VectorRn deltaS(3); VectorRn deltaS(3);
@@ -112,8 +109,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]); completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
} }
} }
m_data->m_ikJacobian->SetDeltaS(deltaC); ikJacobian.SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian); ikJacobian.SetJendTrans(completeJacobian);
} else } else
{ {
VectorRn deltaC(3); VectorRn deltaC(3);
@@ -126,53 +123,53 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]); completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
} }
} }
m_data->m_ikJacobian->SetDeltaS(deltaC); ikJacobian.SetDeltaS(deltaC);
m_data->m_ikJacobian->SetJendTrans(completeJacobian); ikJacobian.SetJendTrans(completeJacobian);
} }
} }
// Calculate the change in theta values // Calculate the change in theta values
switch (ikMethod) { switch (ikMethod) {
case IK2_JACOB_TRANS: case IK2_JACOB_TRANS:
m_data->m_ikJacobian->CalcDeltaThetasTranspose(); // Jacobian transpose method ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break; break;
case IK2_DLS: case IK2_DLS:
case IK2_VEL_DLS: case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION: case IK2_VEL_DLS_WITH_ORIENTATION:
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
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);
m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break; break;
case IK2_DLS_SVD: case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD(); ikJacobian.CalcDeltaThetasDLSwithSVD();
break; break;
case IK2_PURE_PSEUDO: case IK2_PURE_PSEUDO:
m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break; break;
case IK2_SDLS: case IK2_SDLS:
m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break; break;
default: default:
m_data->m_ikJacobian->ZeroDeltaThetas(); ikJacobian.ZeroDeltaThetas();
break; break;
} }
// Use for velocity IK, update theta dot // Use for velocity IK, update theta dot
//m_data->m_ikJacobian->UpdateThetaDot(); //ikJacobian.UpdateThetaDot();
// Use for position IK, incrementally update theta // Use for position IK, incrementally update theta
//m_data->m_ikJacobian->UpdateThetas(); //ikJacobian.UpdateThetas();
// Apply the change in the theta values // Apply the change in the theta values
//m_data->m_ikJacobian->UpdatedSClampValue(&targets); //ikJacobian.UpdatedSClampValue(&targets);
for (int i=0;i<numQ;i++) for (int i=0;i<numQ;i++)
{ {
// Use for velocity IK // 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 // Use for position IK
//q_new[i] = m_data->m_ikNodes[i]->GetTheta(); //q_new[i] = m_data->m_ikNodes[i]->GetTheta();
@@ -203,4 +200,4 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current,
} }
} }
return true; return true;
} }