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

Allow to request the state of a rigid body (position, orientation, lin/ang velocity) through shared memory API

Browse Source 
PhysicsClientC_API: b3RequestActualStateCommandInit requires a body unique Id as second argument
This commit is contained in:
erwin coumans
2015-10-27 15:46:13 -07:00
parent 9cc2b1ec12
commit 2d5d89d999
7 changed files with 78 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -191,7 +191,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
}
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient)
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -199,7 +199,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = 0;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
@@ -467,6 +467,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
break;
}
case CMD_RIGID_BODY_CREATION_COMPLETED:
{
bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId;
break;
}
default:
{
b3Assert(0);

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -100,7 +100,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3PickBody(struct SharedMemoryCommand *command,

7
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -229,8 +229,11 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
case CMD_REQUEST_ACTUAL_STATE:
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
if (m_selectedBody>=0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
}
break;
};

5
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -450,6 +450,11 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_RIGID_BODY_CREATION_COMPLETED:
{
break;
}
case CMD_DEBUG_LINES_OVERFLOW_FAILED: {
b3Warning("Error receiving debug lines");
m_data->m_debugLinesFrom.resize(0);

65
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -79,12 +79,14 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
struct InteralBodyData
{
btMultiBody* m_multiBody;
btRigidBody* m_rigidBody;
int m_testData;
btTransform m_rootLocalInertialFrame;
InteralBodyData()
:m_multiBody(0),
m_rigidBody(0),
m_testData(0)
{
m_rootLocalInertialFrame.setIdentity();
@@ -1089,14 +1091,10 @@ void PhysicsServerSharedMemory::processClientCommands()
}
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
btMultiBody* mb = 0;
if (body)
{
mb = body->m_multiBody;
}
if (mb)
{
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
@@ -1192,10 +1190,48 @@ void PhysicsServerSharedMemory::processClientCommands()
} else
{
if (body && body->m_rigidBody)
{
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
b3Warning("Request state but no multibody available");
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
btTransform tr = rb->getWorldTransform();
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
m_data->submitServerStatus(serverCmd);
} else
{
b3Warning("Request state but no multibody or rigid body available");
SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
}
}
break;
@@ -1417,10 +1453,15 @@ void PhysicsServerSharedMemory::processClientCommands()
bool isDynamic = (mass>0);
worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RIGID_BODY_CREATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
int bodyUniqueId = m_data->allocHandle();
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
bodyHandle->m_rootLocalInertialFrame.setIdentity();
bodyHandle->m_rigidBody = rb;
m_data->submitServerStatus(serverCmd);
break;

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -245,6 +245,10 @@ struct SharedMemoryCommand
};
};
struct RigidBodyCreateArgs
{
int m_bodyUniqueId;
};
struct SharedMemoryStatus
{
@@ -258,6 +262,7 @@ struct SharedMemoryStatus
struct BulletDataStreamArgs m_dataStreamArguments;
struct SendActualStateArgs m_sendActualStateArgs;
struct SendDebugLinesArgs m_sendDebugLinesArgs;
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
};
};

4
test/SharedMemory/test.c
View File

@@ -121,7 +121,7 @@ int main(int argc, char* argv[])
{
int statusType;
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
@@ -157,7 +157,7 @@ int main(int argc, char* argv[])
}
{
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm));
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
if (sensorJointIndexLeft>=0)
{