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;
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;

2
examples/SharedMemory/PhysicsClient.h
View File

@@ -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();

54
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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
}
}

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)
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

8
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -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;
}

56
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -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");

8
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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;
};
};

12
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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;