diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index e11103dcf..84edd5198 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -485,7 +485,7 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin 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]); } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 8ae7e64b1..f671c5951 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -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]); + } } { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9f3160895..edda7b3be 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2666,7 +2666,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray q_new; q_new.resize(numDofs); 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; } @@ -2683,7 +2683,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm ikMethod = IK2_VEL_DLS; } - if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) { btAlignedObjectArray lower_limit; btAlignedObjectArray upper_limit;