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

Fix the link transform in world frame in IK.

Browse Source
This commit is contained in:
yunfeibai
2016-10-12 09:57:14 -07:00
parent 7cb8756c28
commit 9ab28ed466
1 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1811,16 +1811,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
@@ -2709,12 +2709,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
} }
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation; btVector3DoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin(); btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation(); btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);