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

2
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -233,10 +233,12 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
for (int i = 0; i < 3; ++i)
{
state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i];
}
for (int i = 0; i < 4; ++i)
{
state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
}
}
}

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();
}

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -197,7 +197,9 @@ struct SendActualStateArgs
double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM];
double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
double m_linkState[7*MAX_NUM_LINKS];
double m_linkLocalInertialFrames[7*MAX_NUM_LINKS];
};
enum EnumSensorTypes

10
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -105,12 +105,18 @@ struct b3DebugLines
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
};
///b3LinkState provides extra information such as the Cartesian world coordinates of the link
///relative to the world reference frame. Orientation is a quaternion x,y,z,w
///b3LinkState provides extra information such as the Cartesian world coordinates
///center of mass (COM) of the link, relative to the world reference frame.
///Orientation is a quaternion x,y,z,w
///Note: to compute the URDF link frame (which equals the joint frame at joint position 0)
///use URDF link frame = link COM frame * inertiaFrame.inverse()
struct b3LinkState
{
double m_worldPosition[3];
double m_worldOrientation[4];
double m_localInertialPosition[3];
double m_localInertialOrientation[4];
};
//todo: discuss and decide about control mode and combinations