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

Modify damped least square IK formulation. Test setting joint damping coefficients for IK.

Browse Source
This commit is contained in:
yunfeibai
2017-01-31 22:58:37 -08:00
parent 2fa3e267fc
commit 0022d0dafb
8 changed files with 59 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -16,6 +16,7 @@ struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
VectorRn m_nullSpaceVelocity;
VectorRn m_dampingCoeff;
b3AlignedObjectArray<Node*> m_ikNodes;
@@ -23,6 +24,7 @@ struct IKTrajectoryHelperInternalData
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
m_dampingCoeff.SetZero();
}
};
@@ -136,7 +138,9 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
@@ -201,3 +205,13 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current,
}
return true;
}
bool IKTrajectoryHelper::setDampingCoeff(int numQ, const double* coeff)
{
m_data->m_dampingCoeff.SetLength(numQ);
m_data->m_dampingCoeff.SetZero();
for (int i = 0; i < numQ; ++i)
{
m_data->m_dampingCoeff[i] = coeff[i];
}
}