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];
}
}

1
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -31,6 +31,7 @@ public:
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
bool setDampingCoeff(int numQ, const double* coeff);
};
#endif //IK_TRAJECTORY_HELPER_H

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -4011,12 +4011,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
double dampIK[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
double jointDampCoeff[7] = {20.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
ikHelperPtr->setDampingCoeff(numDofs, jointDampCoeff);
double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIK);
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)