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

expose the local inertial frame for each link in the shared memory API

Browse Source 
struct b3LinkState
{
    double m_worldPosition[3];//this is the inertial frame
    double m_worldOrientation[4];

    double m_localInertialPosition[3];//this is the local frame from inertial to link frame
    double m_localInertialOrientation[4];
};

  const btTransform link_frame_world =
               inertial_frame_world * m_local_inertial_frame->inverse();
This commit is contained in:
Erwin Coumans
2016-04-29 14:45:15 -07:00
parent 1635a9d594
commit ab4299f517
4 changed files with 34 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -82,6 +82,7 @@ struct InteralBodyData
int m_testData;
btTransform m_rootLocalInertialFrame;
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
InteralBodyData()
:m_multiBody(0),
@@ -763,11 +764,18 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i=0;i<mb->getNumLinks();i++)
{
//disable serialization of the collision objects
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
int urdfLinkIndex = creation.m_mb2urdfLink[i];
btScalar mass;
btVector3 localInertiaDiagonal(0,0,0);
btTransform localInertialFrame;
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
m_data->m_strings.push_back(linkName);
util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str());
@@ -1349,8 +1357,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//}
}
}
btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ();
@@ -1358,6 +1370,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w();
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+2] = linkLocalInertialOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w();
}