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

improvements to the shared memory physics API:

Browse Source 
support picking in C API etc.
This commit is contained in:
=
2015-09-24 22:42:22 -07:00
parent 066ff5f2e9
commit e05825f639
8 changed files with 140 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -231,6 +231,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
{ {
b3JointInfo info; b3JointInfo info;
info.m_flags = 0; info.m_flags = 0;
info.m_jointIndex = link;
info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1;
@@ -278,6 +279,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
{ {
b3JointInfo info; b3JointInfo info;
info.m_flags = 0; info.m_flags = 0;
info.m_jointIndex = link;
info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1; info.m_qIndex = (0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1; info.m_uIndex = (0 < mb->m_links[link].m_dofCount) ? uOffset : -1;

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -21,7 +21,7 @@ public:
virtual bool isConnected() const; 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 const struct SharedMemoryStatus* processServerStatus();
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand(); virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand();

54
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -139,11 +139,11 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
return (b3SharedMemoryCommandHandle) command; 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; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
return 0; return 0;
} }
@@ -200,6 +200,19 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
return (b3SharedMemoryCommandHandle) command; 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) b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
@@ -379,7 +392,43 @@ void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3Jo
cl->getJointInfo(linkIndex,*info); 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) b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{ {
@@ -409,4 +458,3 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
} }
} }

14
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -63,11 +63,11 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD ///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 b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
//Only use when controlMode is CONTROL_MODE_VELOCITY //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); int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE, ///Only use if when controlMode is CONTROL_MODE_TORQUE,
int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
@@ -85,11 +85,19 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle,
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient); 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); int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient); 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 #ifdef __cplusplus

8
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -5,7 +5,7 @@
#include "SharedMemoryCommon.h" #include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsClientC_API.h"
#include "PhysicsClient.h" #include "PhysicsClient.h"
//#include "SharedMemoryCommands.h" //#include "SharedMemoryCommands.h"
@@ -154,8 +154,6 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
{ {
cl->enqueueCommand(buttonId); cl->enqueueCommand(buttonId);
} }
} }
void PhysicsClientExample::enqueueCommand(int commandId) void PhysicsClientExample::enqueueCommand(int commandId)
@@ -177,9 +175,9 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
double startPosX = 0; double startPosX = 0;
double startPosY = 0; double startPosY = 0;
double startPosZ = 0; double startPosZ = 0;
int ret = b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
ret = b3SubmitClientCommand(m_physicsClientHandle, commandHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break; break;
} }

56
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -1067,8 +1067,20 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
b3Printf("Server Init Pose not implemented yet"); b3Printf("Server Init Pose not implemented yet");
} }
///@todo: implement this int body_unique_id = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); 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); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd); m_data->submitServerStatus(serverCmd);
@@ -1136,6 +1148,46 @@ void PhysicsServerSharedMemory::processClientCommands()
break; 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: default:
{ {
b3Error("Unknown command encountered"); b3Error("Unknown command encountered");

8
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -25,7 +25,6 @@
#endif #endif
#define SHARED_MEMORY_SERVER_TEST_C #define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 256 #define MAX_DEGREE_OF_FREEDOM 256
#define MAX_NUM_SENSORS 256 #define MAX_NUM_SENSORS 256
@@ -92,6 +91,12 @@ struct SendDebugLinesArgs
int m_numRemainingDebugLines; 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. ///Controlling a robot involves sending the desired state to its joint motor controllers.
///The control mode determines the state variables used for motor control. ///The control mode determines the state variables used for motor control.
@@ -225,6 +230,7 @@ struct SharedMemoryCommand
struct CreateSensorArgs m_createSensorArguments; struct CreateSensorArgs m_createSensorArguments;
struct CreateBoxShapeArgs m_createBoxShapeArguments; struct CreateBoxShapeArgs m_createBoxShapeArguments;
struct RequestDebugLinesArgs m_requestDebugLinesArguments; struct RequestDebugLinesArgs m_requestDebugLinesArguments;
struct PickBodyArgs m_pickBodyArguments;
}; };
}; };

12
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -20,6 +20,9 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_DEBUG_LINES, CMD_REQUEST_DEBUG_LINES,
CMD_STEP_FORWARD_SIMULATION, CMD_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION, CMD_RESET_SIMULATION,
CMD_PICK_BODY,
CMD_MOVE_PICKED_BODY,
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
CMD_MAX_CLIENT_COMMANDS CMD_MAX_CLIENT_COMMANDS
}; };
@@ -61,10 +64,17 @@ struct b3JointInfo
int m_jointType; int m_jointType;
int m_qIndex; int m_qIndex;
int m_uIndex; int m_uIndex;
/// int m_jointIndex;
int m_flags; 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 struct b3DebugLines
{ {
int m_numDebugLines; int m_numDebugLines;