From e05825f639b219a566dc075abc37b4c961beadcc Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 24 Sep 2015 22:42:22 -0700 Subject: [PATCH] improvements to the shared memory physics API: support picking in C API etc. --- examples/SharedMemory/PhysicsClient.cpp | 2 + examples/SharedMemory/PhysicsClient.h | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 54 +++++++++++++++++- examples/SharedMemory/PhysicsClientC_API.h | 14 ++++- .../SharedMemory/PhysicsClientExample.cpp | 8 +-- examples/SharedMemory/PhysicsServer.cpp | 56 ++++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 8 ++- examples/SharedMemory/SharedMemoryPublic.h | 12 +++- 8 files changed, 140 insertions(+), 16 deletions(-) diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index 4fe6067d7..0a5e41d18 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -231,6 +231,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3JointInfo info; info.m_flags = 0; + info.m_jointIndex = link; info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; @@ -278,6 +279,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3JointInfo info; info.m_flags = 0; + info.m_jointIndex = link; info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 1b9f8dd52..185a4d0e7 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -21,7 +21,7 @@ public: virtual bool isConnected() const; - // return true if there is a status, and fill in 'serverStatus' + // return non-null if there is a status, nullptr otherwise virtual const struct SharedMemoryStatus* processServerStatus(); virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand(); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ad8cb6aea..d1fa6a5f0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -139,11 +139,11 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy return (b3SharedMemoryCommandHandle) command; } -int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); - command->m_sendDesiredStateCommandArgument.m_desiredStateQ[dofIndex] = value; + command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value; return 0; } @@ -200,6 +200,19 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl return (b3SharedMemoryCommandHandle) command; } +void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3JointInfo info; + b3GetJointInfo(physClient, jointIndex, &info); + state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex]; + state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex]; + for (int ii(0); ii < 6; ++ii) { + state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; + } +} + b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; @@ -379,7 +392,43 @@ void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3Jo cl->getJointInfo(linkIndex,*info); } +int b3PickBody(struct SharedMemoryCommand *command, + 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; +} +int b3MovePickedBody(struct SharedMemoryCommand *command, + 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; +} + +int b3RemovePickingConstraint(b3SharedMemoryCommandHandle commandHandle) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REMOVE_PICKING_CONSTRAINT_BODY); + return 0; +} b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) { @@ -409,4 +458,3 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l } } - diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 201c92f1d..19ddc2832 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -63,11 +63,11 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD -int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); //Only use when controlMode is CONTROL_MODE_VELOCITY -int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); @@ -85,11 +85,19 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient); -int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int dofIndex, int enable); +int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable); int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient); +void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, 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); #ifdef __cplusplus diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 9e2cfb7a5..adb8a6acb 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -5,7 +5,7 @@ #include "SharedMemoryCommon.h" #include "../CommonInterfaces/CommonParameterInterface.h" - +#include "PhysicsClientC_API.h" #include "PhysicsClient.h" //#include "SharedMemoryCommands.h" @@ -154,8 +154,6 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr) { cl->enqueueCommand(buttonId); } - - } void PhysicsClientExample::enqueueCommand(int commandId) @@ -177,9 +175,9 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) double startPosX = 0; double startPosY = 0; double startPosZ = 0; - int ret = b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); + b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); - ret = b3SubmitClientCommand(m_physicsClientHandle, commandHandle); + b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 7d64dbfc9..1aabe081d 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1067,8 +1067,20 @@ void PhysicsServerSharedMemory::processClientCommands() { b3Printf("Server Init Pose not implemented yet"); } - ///@todo: implement this - m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); + int body_unique_id = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; + if (m_data->m_dynamicsWorld->getNumMultibodies()>body_unique_id) + { + btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(body_unique_id); + mb->setBasePos(btVector3( + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[2])); + mb->setWorldToBaseRot(btQuaternion( + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[3], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[4], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[5], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[6])); + } SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(serverCmd); @@ -1136,6 +1148,46 @@ void PhysicsServerSharedMemory::processClientCommands() break; } + case CMD_PICK_BODY: + { + pickBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + SharedMemoryStatus &serverCmd = + m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED, + clientCmd.m_sequenceNumber, timeStamp); + m_data->submitServerStatus(serverCmd); + break; + } + case CMD_MOVE_PICKED_BODY: + { + movePickedBody(btVector3(clientCmd.m_pickBodyArguments.m_rayFromWorld[0], + clientCmd.m_pickBodyArguments.m_rayFromWorld[1], + clientCmd.m_pickBodyArguments.m_rayFromWorld[2]), + btVector3(clientCmd.m_pickBodyArguments.m_rayToWorld[0], + clientCmd.m_pickBodyArguments.m_rayToWorld[1], + clientCmd.m_pickBodyArguments.m_rayToWorld[2])); + + SharedMemoryStatus &serverCmd = + m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED, + clientCmd.m_sequenceNumber, timeStamp); + m_data->submitServerStatus(serverCmd); + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + removePickingConstraint(); + + SharedMemoryStatus &serverCmd = + m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED, + clientCmd.m_sequenceNumber, timeStamp); + m_data->submitServerStatus(serverCmd); + break; + } default: { b3Error("Unknown command encountered"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 9c8d2cb1d..40343d08c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -25,7 +25,6 @@ #endif - #define SHARED_MEMORY_SERVER_TEST_C #define MAX_DEGREE_OF_FREEDOM 256 #define MAX_NUM_SENSORS 256 @@ -92,6 +91,12 @@ struct SendDebugLinesArgs int m_numRemainingDebugLines; }; +struct PickBodyArgs +{ + double m_rayFromWorld[3]; + double m_rayToWorld[3]; +}; + ///Controlling a robot involves sending the desired state to its joint motor controllers. ///The control mode determines the state variables used for motor control. @@ -225,6 +230,7 @@ struct SharedMemoryCommand struct CreateSensorArgs m_createSensorArguments; struct CreateBoxShapeArgs m_createBoxShapeArguments; struct RequestDebugLinesArgs m_requestDebugLinesArguments; + struct PickBodyArgs m_pickBodyArguments; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index c73670b70..fcdd9ff73 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -20,6 +20,9 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_DEBUG_LINES, CMD_STEP_FORWARD_SIMULATION, CMD_RESET_SIMULATION, + CMD_PICK_BODY, + CMD_MOVE_PICKED_BODY, + CMD_REMOVE_PICKING_CONSTRAINT_BODY, CMD_MAX_CLIENT_COMMANDS }; @@ -61,10 +64,17 @@ struct b3JointInfo int m_jointType; int m_qIndex; int m_uIndex; - /// + int m_jointIndex; int m_flags; }; +struct b3JointSensorState +{ + double m_jointPosition; + double m_jointVelocity; + double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */ +}; + struct b3DebugLines { int m_numDebugLines;