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

Expose IK solver options including DLS and SDLS.

Browse Source
This commit is contained in:
yunfeibai
2017-10-19 14:00:53 -07:00
parent 1393666a83
commit dda1b05f4a
10 changed files with 64 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -47,7 +47,8 @@ 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 || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE
|| ikMethod==IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
@@ -136,8 +137,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
@@ -154,6 +155,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
case IK2_VEL_SDLS:
case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default: