diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7553811bc..75578acb2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -480,6 +480,41 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) } return bodyId; } + +int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* numDegreeOfFreedomQ, + int* numDegreeOfFreedomU, + const double* rootLocalInertialFrame[], + const double* actualStateQ[], + const double* actualStateQdot[], + const double* jointReactionForces[]) { + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + const SendActualStateArgs &args = status->m_sendActualStateArgs; + if (bodyUniqueId) { + *bodyUniqueId = args.m_bodyUniqueId; + } + if (numDegreeOfFreedomQ) { + *numDegreeOfFreedomQ = args.m_numDegreeOfFreedomQ; + } + if (numDegreeOfFreedomU) { + *numDegreeOfFreedomU = args.m_numDegreeOfFreedomU; + } + if (rootLocalInertialFrame) { + *rootLocalInertialFrame = args.m_rootLocalInertialFrame; + } + if (actualStateQ) { + *actualStateQ = args.m_actualStateQ; + } + if (actualStateQdot) { + *actualStateQdot = args.m_actualStateQdot; + } + if (jointReactionForces) { + *jointReactionForces = args.m_jointReactionForces; + } + return true; +} + int b3CanSubmitCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -526,42 +561,54 @@ void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkInd cl->getJointInfo(bodyIndex, linkIndex,*info); } -int b3PickBody(struct SharedMemoryCommand *command, - double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, - double rayToWorldX, double rayToWorldY, double rayToWorldZ) +b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, + double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ) { - b3Assert(command); - b3Assert(command->m_type == CMD_PICK_BODY); - command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; - command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; - command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; - command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; - command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; - command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; - return 0; + PhysicsClient *cl = (PhysicsClient *)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_PICK_BODY; + command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; + command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; + command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; + command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; + command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; + command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; + return (b3SharedMemoryCommandHandle)command; } -int b3MovePickedBody(struct SharedMemoryCommand *command, - double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, - double rayToWorldX, double rayToWorldY, double rayToWorldZ) +b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX, + double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, + double rayToWorldZ) { - b3Assert(command); - b3Assert(command->m_type == CMD_MOVE_PICKED_BODY); - command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; - command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; - command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; - command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; - command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; - command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; - return 0; + PhysicsClient *cl = (PhysicsClient *)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_MOVE_PICKED_BODY; + command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; + command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; + command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; + command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; + command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; + command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; + return (b3SharedMemoryCommandHandle)command; } -int b3RemovePickingConstraint(b3SharedMemoryCommandHandle commandHandle) +b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient) { - struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command); - b3Assert(command->m_type == CMD_REMOVE_PICKING_CONSTRAINT_BODY); - return 0; + PhysicsClient *cl = (PhysicsClient *)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_REMOVE_PICKING_CONSTRAINT_BODY; + return (b3SharedMemoryCommandHandle)command; } b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 5f008c47e..c1590f562 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -36,7 +36,15 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); - +int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, + int* bodyUniqueId, + int* numDegreeOfFreedomQ, + int* numDegreeOfFreedomU, + const double* rootLocalInertialFrame[], + const double* actualStateQ[], + const double* actualStateQdot[], + const double* jointReactionForces[]); + int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, struct b3JointInfo* info); @@ -103,15 +111,15 @@ int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, in b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); -int b3PickBody(struct SharedMemoryCommand *command, - double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, - double rayToWorldX, double rayToWorldY, double rayToWorldZ); -int b3MovePickedBody(struct SharedMemoryCommand *command, - double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, - double rayToWorldX, double rayToWorldY, double rayToWorldZ); -int b3RemovePickingConstraint(struct SharedMemoryCommand *command); +b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, + double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, double rayToWorldZ); +b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX, + double rayFromWorldY, double rayFromWorldZ, + double rayToWorldX, double rayToWorldY, + double rayToWorldZ); +b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient); - #ifdef __cplusplus } #endif diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index aa43a7f52..2db5d663a 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -795,9 +795,11 @@ void PhysicsServerSharedMemory::createJointMotors(btMultiBody* mb) -bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, +bool PhysicsServerSharedMemory::loadUrdf(const char* fileName2, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr) { + const char* fileName="tmp/model/simulation/model_s_one_s15p1.urdf"; + btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) { diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index c7347ae00..3ae34e0a6 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -128,13 +128,12 @@ int main(int argc, char* argv[]) if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { -#if 0 - posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ; - dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU; - + b3GetStatusActualState(statusHandle, + 0, &posVarCount, &dofCount, + 0, 0, 0, 0); + b3Printf("posVarCount = %d\n",posVarCount); printf("dofCount = %d\n",dofCount); -#endif } } @@ -177,7 +176,6 @@ int main(int argc, char* argv[]) struct b3JointSensorState sensorState; b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); - b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&sensorState); b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[1],