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

Test four IK modes: with and without orientation constraint, with and without null space task.

Browse Source
This commit is contained in:
yunfeibai
2016-09-30 00:15:51 -07:00
parent fd3cb061cb
commit ee00696ae9
3 changed files with 15 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -75,16 +75,19 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
// Set one end effector world orientation from Bullet
VectorRn deltaR(3);
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
if (useAngularPart)
{
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
}
{