From 2d5d89d99989c258a02ed3174ad23b0a24615deb Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 27 Oct 2015 15:46:13 -0700 Subject: [PATCH] Allow to request the state of a rigid body (position, orientation, lin/ang velocity) through shared memory API PhysicsClientC_API: b3RequestActualStateCommandInit requires a body unique Id as second argument --- examples/SharedMemory/PhysicsClientC_API.cpp | 9 ++- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../SharedMemory/PhysicsClientExample.cpp | 7 +- .../PhysicsClientSharedMemory.cpp | 5 ++ examples/SharedMemory/PhysicsServer.cpp | 65 +++++++++++++++---- examples/SharedMemory/SharedMemoryCommands.h | 5 ++ test/SharedMemory/test.c | 4 +- 7 files changed, 78 insertions(+), 19 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 639191b14..7553811bc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index df456c938..5f008c47e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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, diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 76e533914..4308e054d 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -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; }; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 255bb2bb5..ba1b3127e 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -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); diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 2ecc95c67..63bc65275 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -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) + + if (body && body->m_multiBody) { - mb = body->m_multiBody; - } - if (mb) - { - + 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; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 49893f2be..6016a3ba1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; }; }; diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index ed32209d9..d3939583f 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -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) {