AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #606 from erwincoumans/master

Browse Source 
expose multibody link world transform in the shared memory API
This commit is contained in:
erwincoumans 2016-04-23 17:53:45 -07:00
commit 190622ed9e
5 changed files with 40 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -222,6 +222,25 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl
}
}
void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex>=0);
if (bodyIndex>=0)
{
for (int i = 0; i < 7; ++i)
{
state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
}
for (int i = 0; i < 4; ++i)
{
state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
}
}
}
b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -130,6 +130,7 @@ int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, in
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,

11
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1349,9 +1349,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//}
}
}
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();
serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x();
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_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -31,6 +31,7 @@
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
struct TmpFloat3
{
@ -196,6 +197,7 @@ struct SendActualStateArgs
double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM];
double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
double m_linkState[7*MAX_NUM_LINKS];
};
enum EnumSensorTypes

8
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -105,6 +105,14 @@ 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
struct b3LinkState
{
double m_worldPosition[3];
double m_worldOrientation[4];
};
//todo: discuss and decide about control mode and combinations
enum {
// POSITION_CONTROL=0,