From 41a1362bc551320b333a65825a4c59e03346a5af Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 24 Oct 2017 21:06:44 -0700 Subject: [PATCH] perform IK in local body-fixed frame For now, Jacobian, mass matrix and inverse dynamics return results in local coordinates of the tree. --- .../btMultiBodyTreeCreator.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 6 +++ .../PhysicsServerCommandProcessor.cpp | 43 ++++++++++++++++--- 3 files changed, 45 insertions(+), 8 deletions(-) diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index ef3ebf6b8..e432cf438 100644 --- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp +++ b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp @@ -27,7 +27,9 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const } else { link.joint_type = FLOATING; } - btTransform transform(btmb->getBaseWorldTransform()); + btTransform transform=(btmb->getBaseWorldTransform()); + //compute inverse dynamics in body-fixed frame + transform.setIdentity(); link.parent_r_parent_body_ref(0) = transform.getOrigin()[0]; link.parent_r_parent_body_ref(1) = transform.getOrigin()[1]; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b27d7b6f2..847f42f8f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3638,6 +3638,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; + + + command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6c4461d4e..d7ea993ba 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7769,15 +7769,44 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); btVector3DoubleData endEffectorWorldPosition; - btVector3DoubleData endEffectorWorldOrientation; + btQuaternionDoubleData endEffectorWorldOrientation; - btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation(); - btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); + btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); + btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); + btTransform endEffectorWorld; + endEffectorWorld.setOrigin(endEffectorPosWorldOrg); + endEffectorWorld.setRotation(endEffectorOriWorldOrg); + + btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + + btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; + + btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); + + btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); - endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); - endEffectorOri.serializeDouble(endEffectorWorldOrientation); + endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); + endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); + btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); + btTransform targetWorld; + targetWorld.setOrigin(targetPosWorld); + targetWorld.setRotation(targetOrnWorld); + btTransform targetBaseCoord; + targetBaseCoord = tr.inverse()*targetWorld; + + btVector3DoubleData targetPosBaseCoord; + btQuaternionDoubleData targetOrnBaseCoord; + targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); + targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); + // Set joint damping coefficents. A small default // damping constant is added to prevent singularity // with pseudo inverse. The user can set joint damping @@ -7796,7 +7825,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); 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, + ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, &q_current[0], numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,