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

2
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -485,7 +485,7 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId);
if (args.m_flags & (B3_HAS_IK_TARGET_ORIENTATION + B3_HAS_NULL_SPACE_VELOCITY)) if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
{ {
b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]); b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]);
} else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)

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 // Set one end effector world orientation from Bullet
VectorRn deltaR(3); VectorRn deltaR(3);
btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]); if (useAngularPart)
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]); 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]);
}
} }
{ {

4
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2666,7 +2666,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> q_new; btAlignedObjectArray<double> q_new;
q_new.resize(numDofs); q_new.resize(numDofs);
int ikMethod = 0; int ikMethod = 0;
if (clientCmd.m_updateFlags& (IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY)) if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{ {
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
} }
@@ -2683,7 +2683,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
ikMethod = IK2_VEL_DLS; ikMethod = IK2_VEL_DLS;
} }
if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{ {
btAlignedObjectArray<double> lower_limit; btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit; btAlignedObjectArray<double> upper_limit;