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

another step closer to useable shared memory C API

Browse Source 
(force/torque sensor needs new API)
in a nutshell, users of shared memory physics API should not
directly poke into shared memory, not fill 'SharedMemorCommand'
nor read SharedMemoryStatus directly. The C-API declares 'handles' for those,
to avoid it from happening.
This commit is contained in:
=
2015-09-16 23:09:10 -07:00
parent 4041748f55
commit 003a42478b
16 changed files with 671 additions and 489 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -20,7 +20,6 @@ SET(App_ExampleBrowser_SRCS
../SharedMemory/PhysicsClientC_API.cpp ../SharedMemory/PhysicsClientC_API.cpp
../SharedMemory/PhysicsServerExample.cpp ../SharedMemory/PhysicsServerExample.cpp
../SharedMemory/PhysicsClientExample.cpp ../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/RobotControlExample.cpp
../SharedMemory/PosixSharedMemory.cpp ../SharedMemory/PosixSharedMemory.cpp
../SharedMemory/Win32SharedMemory.cpp ../SharedMemory/Win32SharedMemory.cpp
../BasicDemo/BasicExample.cpp ../BasicDemo/BasicExample.cpp

13
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -34,7 +34,6 @@
#include "../DynamicControlDemo/MotorDemo.h" #include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/RobotControlExample.h"
#include "../SharedMemory/PhysicsClientExample.h" #include "../SharedMemory/PhysicsClientExample.h"
#include "../Constraints/TestHingeTorque.h" #include "../Constraints/TestHingeTorque.h"
#include "../RenderingExamples/TimeSeriesExample.h" #include "../RenderingExamples/TimeSeriesExample.h"
@@ -204,12 +203,12 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(0,"Experiments"), ExampleEntry(0,"Experiments"),
ExampleEntry(1,"Robot Control (Velocity)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", // ExampleEntry(1,"Robot Control (Velocity)", "Perform some robot control tasks, using physics server and client that communicate over shared memory",
RobotControlExampleCreateFunc,ROBOT_VELOCITY_CONTROL), // RobotControlExampleCreateFunc,ROBOT_VELOCITY_CONTROL),
ExampleEntry(1,"Robot Control (PD)", "Perform some robot control tasks, using physics server and client that communicate over shared memory", // ExampleEntry(1,"Robot Control (PD)", "Perform some robot control tasks, using physics server and client that communicate over shared memory",
RobotControlExampleCreateFunc,ROBOT_PD_CONTROL), // RobotControlExampleCreateFunc,ROBOT_PD_CONTROL),
ExampleEntry(1,"Robot Joint Feedback", "Apply some small ping-pong target velocity jitter, and read the joint reaction forces, using physics server and client that communicate over shared memory.", // ExampleEntry(1,"Robot Joint Feedback", "Apply some small ping-pong target velocity jitter, and read the joint reaction forces, using physics server and client that communicate over shared memory.",
RobotControlExampleCreateFunc,ROBOT_PING_PONG_JOINT_FEEDBACK), // RobotControlExampleCreateFunc,ROBOT_PING_PONG_JOINT_FEEDBACK),
ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory", ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
PhysicsServerCreateFunc), PhysicsServerCreateFunc),

1
examples/ExampleBrowser/premake4.lua
View File

@@ -56,7 +56,6 @@
"../SharedMemory/PhysicsClientC_API.h", "../SharedMemory/PhysicsClientC_API.h",
"../SharedMemory/PhysicsServerExample.cpp", "../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsClientExample.cpp", "../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/RobotControlExample.cpp",
"../SharedMemory/PhysicsServer.cpp", "../SharedMemory/PhysicsServer.cpp",
"../SharedMemory/PhysicsClient.cpp", "../SharedMemory/PhysicsClient.cpp",
"../SharedMemory/PosixSharedMemory.cpp", "../SharedMemory/PosixSharedMemory.cpp",

107
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -18,6 +18,19 @@ enum JointType
ePrismaticType = 1, ePrismaticType = 1,
}; };
struct TmpFloat3
{
float m_x;
float m_y;
float m_z;
};
TmpFloat3 CreateTmpFloat3(float x,float y, float z)
{
TmpFloat3 tmp; tmp.m_x = x;tmp.m_y = y;tmp.m_z = z; return tmp;
}
struct PhysicsClientSharedMemoryInternalData struct PhysicsClientSharedMemoryInternalData
{ {
SharedMemoryInterface* m_sharedMemory; SharedMemoryInterface* m_sharedMemory;
@@ -25,9 +38,11 @@ struct PhysicsClientSharedMemoryInternalData
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData; btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
btAlignedObjectArray<btVector3> m_debugLinesFrom; btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
btAlignedObjectArray<btVector3> m_debugLinesTo; btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
btAlignedObjectArray<btVector3> m_debugLinesColor; btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
SharedMemoryStatus m_lastServerStatus;
int m_counter; int m_counter;
bool m_serverLoadUrdfOK; bool m_serverLoadUrdfOK;
@@ -146,20 +161,18 @@ bool PhysicsClientSharedMemory::connect()
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus) const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
{ {
bool hasStatus = false; SharedMemoryStatus* stat = 0;
if (!m_data->m_testBlock1) if (!m_data->m_testBlock1)
{ {
serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED; return 0;
return true;
} }
if (!m_data->m_waitingForServer) if (!m_data->m_waitingForServer)
{ {
serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND; return 0;
return true;
} }
if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands) if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
@@ -167,8 +180,8 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);
const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
hasStatus = true; m_data->m_lastServerStatus = serverCmd;
serverStatus = serverCmd;
EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
//consume the command //consume the command
switch (serverCmd.m_type) switch (serverCmd.m_type)
@@ -424,6 +437,21 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
break; break;
} }
case CMD_RESET_SIMULATION_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
}
for (int i=0;i<m_data->m_robotMultiBodyData.size();i++)
{
delete m_data->m_robotMultiBodyData[i];
}
m_data->m_robotMultiBodyData.clear();
m_data->m_jointInfo.clear();
break;
}
case CMD_DEBUG_LINES_COMPLETED: case CMD_DEBUG_LINES_COMPLETED:
{ {
if (m_data->m_verboseOutput) if (m_data->m_verboseOutput)
@@ -442,14 +470,15 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
for (int i=0;i<numLines;i++) for (int i=0;i<numLines;i++)
{ {
btVector3 from(linesFrom[i*3],linesFrom[i*3+1],linesFrom[i*3+2]); TmpFloat3 from = CreateTmpFloat3(linesFrom[i*3],linesFrom[i*3+1],linesFrom[i*3+2]);
btVector3 to(linesTo[i*3],linesTo[i*3+1],linesTo[i*3+2]); TmpFloat3 to = CreateTmpFloat3(linesTo[i*3],linesTo[i*3+1],linesTo[i*3+2]);
btVector3 color(linesColor[i*3],linesColor[i*3+1],linesColor[i*3+2]); TmpFloat3 color = CreateTmpFloat3(linesColor[i*3],linesColor[i*3+1],linesColor[i*3+2]);
m_data->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = from; m_data->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = from;
m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = to; m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = to;
m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = color; m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex+i] = color;
} }
break; break;
} }
case CMD_DEBUG_LINES_OVERFLOW_FAILED: case CMD_DEBUG_LINES_OVERFLOW_FAILED:
@@ -481,6 +510,20 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
{ {
m_data->m_waitingForServer = true; m_data->m_waitingForServer = true;
} }
if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines>0))
{
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//continue requesting debug lines for drawing
command.m_type =CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_startingLineIndex = serverCmd.m_sendDebugLinesArgs.m_numDebugLines+serverCmd.m_sendDebugLinesArgs.m_startingLineIndex;
submitClientCommand(command);
return 0;
}
return &m_data->m_lastServerStatus;
} else } else
{ {
if (m_data->m_verboseOutput) if (m_data->m_verboseOutput)
@@ -489,7 +532,7 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
m_data->m_testBlock1->m_numProcessedServerCommands); m_data->m_testBlock1->m_numProcessedServerCommands);
} }
} }
return hasStatus; return 0;
} }
bool PhysicsClientSharedMemory::canSubmitCommand() const bool PhysicsClientSharedMemory::canSubmitCommand() const
@@ -497,6 +540,11 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const
return (m_data->m_isConnected && !m_data->m_waitingForServer); return (m_data->m_isConnected && !m_data->m_waitingForServer);
} }
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand()
{
return &m_data->m_testBlock1->m_clientCommands[0];
}
bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command)
{ {
///at the moment we allow a maximum of 1 outstanding command, so we check for this ///at the moment we allow a maximum of 1 outstanding command, so we check for this
@@ -505,19 +553,10 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
if (!m_data->m_waitingForServer) if (!m_data->m_waitingForServer)
{ {
//a reset simulation command needs special attention, cleanup state if (&m_data->m_testBlock1->m_clientCommands[0] != &command)
if (command.m_type==CMD_RESET_SIMULATION) {
{ m_data->m_testBlock1->m_clientCommands[0] = command;
for (int i=0;i<m_data->m_robotMultiBodyData.size();i++) }
{
delete m_data->m_robotMultiBodyData[i];
}
m_data->m_robotMultiBodyData.clear();
m_data->m_jointInfo.clear();
}
m_data->m_testBlock1->m_clientCommands[0] = command;
m_data->m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
m_data->m_waitingForServer = true; m_data->m_waitingForServer = true;
return true; return true;
@@ -541,27 +580,27 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data,
} }
const btVector3* PhysicsClientSharedMemory::getDebugLinesFrom() const const float* PhysicsClientSharedMemory::getDebugLinesFrom() const
{ {
if (m_data->m_debugLinesFrom.size()) if (m_data->m_debugLinesFrom.size())
{ {
return &m_data->m_debugLinesFrom[0]; return &m_data->m_debugLinesFrom[0].m_x;
} }
return 0; return 0;
} }
const btVector3* PhysicsClientSharedMemory::getDebugLinesTo() const const float* PhysicsClientSharedMemory::getDebugLinesTo() const
{ {
if (m_data->m_debugLinesTo.size()) if (m_data->m_debugLinesTo.size())
{ {
return &m_data->m_debugLinesTo[0]; return &m_data->m_debugLinesTo[0].m_x;
} }
return 0; return 0;
} }
const btVector3* PhysicsClientSharedMemory::getDebugLinesColor() const const float* PhysicsClientSharedMemory::getDebugLinesColor() const
{ {
if (m_data->m_debugLinesColor.size()) if (m_data->m_debugLinesColor.size())
{ {
return &m_data->m_debugLinesColor[0]; return &m_data->m_debugLinesColor[0].m_x;
} }
return 0; return 0;
} }

16
examples/SharedMemory/PhysicsClient.h
View File

@@ -1,7 +1,7 @@
#ifndef BT_PHYSICS_CLIENT_API_H #ifndef BT_PHYSICS_CLIENT_API_H
#define BT_PHYSICS_CLIENT_API_H #define BT_PHYSICS_CLIENT_API_H
#include "SharedMemoryCommands.h" //#include "SharedMemoryCommands.h"
#include "LinearMath/btVector3.h" #include "LinearMath/btVector3.h"
class PhysicsClientSharedMemory class PhysicsClientSharedMemory
@@ -22,15 +22,17 @@ public:
virtual bool isConnected() const; virtual bool isConnected() const;
// return true if there is a status, and fill in 'serverStatus' // return true if there is a status, and fill in 'serverStatus'
virtual bool processServerStatus(SharedMemoryStatus& serverStatus); virtual const struct SharedMemoryStatus* processServerStatus();
virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const; virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const SharedMemoryCommand& command); virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumJoints() const; virtual int getNumJoints() const;
virtual void getJointInfo(int index, b3JointInfo& info) const; virtual void getJointInfo(int index, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key); virtual void setSharedMemoryKey(int key);
@@ -38,9 +40,9 @@ public:
virtual int getNumDebugLines() const; virtual int getNumDebugLines() const;
virtual const btVector3* getDebugLinesFrom() const; virtual const float* getDebugLinesFrom() const;
virtual const btVector3* getDebugLinesTo() const; virtual const float* getDebugLinesTo() const;
virtual const btVector3* getDebugLinesColor() const; virtual const float* getDebugLinesColor() const;
}; };

223
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2,10 +2,19 @@
#include "PhysicsClient.h" #include "PhysicsClient.h"
#include "Bullet3Common/b3Scalar.h" #include "Bullet3Common/b3Scalar.h"
#include <string.h> #include <string.h>
#include "SharedMemoryCommands.h"
int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfFileName)
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{ {
b3Assert(command); PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_URDF; command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName); int len = strlen(urdfFileName);
if (len<MAX_URDF_FILENAME_LENGTH) if (len<MAX_URDF_FILENAME_LENGTH)
@@ -17,14 +26,26 @@ int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfF
} }
command->m_updateFlags = URDF_ARGS_FILE_NAME; command->m_updateFlags = URDF_ARGS_FILE_NAME;
return 0; return (b3SharedMemoryCommandHandle) command;
}
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_updateFlags |=URDF_ARGS_USE_FIXED_BASE;
command->m_urdfArguments.m_useFixedBase = useFixedBase;
return 0;
} }
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF); b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_urdfArguments.m_initialPosition[0] = startPosX; command->m_urdfArguments.m_initialPosition[0] = startPosX;
@@ -33,8 +54,10 @@ int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, doubl
command->m_updateFlags|=URDF_ARGS_INITIAL_POSITION; command->m_updateFlags|=URDF_ARGS_INITIAL_POSITION;
return 0; return 0;
} }
int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF); b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_urdfArguments.m_initialOrientation[0] = startOrnX; command->m_urdfArguments.m_initialOrientation[0] = startOrnX;
@@ -45,16 +68,21 @@ int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, do
return 0; return 0;
} }
int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command) b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)
{ {
b3Assert(command); PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; command->m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
command->m_updateFlags = 0; command->m_updateFlags = 0;
return 0; return (b3SharedMemoryCommandHandle) command;
} }
int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz) int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_gravityAcceleration[0] = gravx; command->m_physSimParamArgs.m_gravityAcceleration[0] = gravx;
command->m_physSimParamArgs.m_gravityAcceleration[1] = gravy; command->m_physSimParamArgs.m_gravityAcceleration[1] = gravy;
@@ -63,83 +91,130 @@ int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gra
return 0; return 0;
} }
int b3PhysicsParamSetTimeStep(struct SharedMemoryCommand* command, double timeStep) int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_deltaTime = timeStep; command->m_physSimParamArgs.m_deltaTime = timeStep;
return 0; return 0;
} }
int b3InitStepSimulationCommand(struct SharedMemoryCommand* command) b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{ {
b3Assert(command); PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_STEP_FORWARD_SIMULATION; command->m_type = CMD_STEP_FORWARD_SIMULATION;
command->m_updateFlags = 0; command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
return 0;
} }
int b3InitResetSimulationCommand(struct SharedMemoryCommand* command) b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
{ {
b3Assert(command); PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
command->m_type = CMD_RESET_SIMULATION; b3Assert(cl);
command->m_updateFlags = 0; b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_RESET_SIMULATION;
command->m_updateFlags = 0;
return 0; return (b3SharedMemoryCommandHandle) command;
} }
int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode) b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE; command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_updateFlags = 0; command->m_updateFlags = 0;
return 0; return (b3SharedMemoryCommandHandle) command;
} }
int b3JointControlSetDesiredVelocity(struct SharedMemoryCommand* command, int dofIndex, double value) int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = 1; command->m_sendDesiredStateCommandArgument.m_desiredStateQ[dofIndex] = value;
return 0;
}
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
return 0;
}
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
return 0;
}
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
return 0; return 0;
} }
int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, int dofIndex, double value) int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
return 0; return 0;
} }
int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int dofIndex, double value) int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
return 0; return 0;
} }
int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command) b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE; command->m_type =CMD_REQUEST_ACTUAL_STATE;
return 0; return (b3SharedMemoryCommandHandle) command;
} }
int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command) b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
command->m_type = CMD_CREATE_BOX_COLLISION_SHAPE; command->m_type = CMD_CREATE_BOX_COLLISION_SHAPE;
command->m_updateFlags =0; command->m_updateFlags =0;
return 0; return (b3SharedMemoryCommandHandle) command;
} }
int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ) int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_POSITION; command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_POSITION;
@@ -150,8 +225,9 @@ int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, doub
return 0; return 0;
} }
int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_ORIENTATION; command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_ORIENTATION;
@@ -163,9 +239,9 @@ int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, d
return 0; return 0;
} }
int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ) int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE); b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS; command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS;
@@ -179,17 +255,24 @@ int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double
int b3CreateSensorCommandInit(struct SharedMemoryCommand* command) b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
command->m_type = CMD_CREATE_SENSOR; command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0; command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0; command->m_createSensorArguments.m_numJointSensorChanges = 0;
return 0; return (b3SharedMemoryCommandHandle) command;
} }
int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int jointIndex, int enable) int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_SENSOR); b3Assert(command->m_type == CMD_CREATE_SENSOR);
int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; int curIndex = command->m_createSensorArguments.m_numJointSensorChanges;
@@ -201,8 +284,9 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c
return 0; return 0;
} }
int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int linkIndex, int enable) int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_SENSOR); b3Assert(command->m_type == CMD_CREATE_SENSOR);
int curIndex = command->m_createSensorArguments.m_numJointSensorChanges; int curIndex = command->m_createSensorArguments.m_numJointSensorChanges;
@@ -214,11 +298,13 @@ int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int link
} }
b3PhysicsClientHandle b3ConnectSharedMemory() b3PhysicsClientHandle b3ConnectSharedMemory(int key)
{ {
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
///client should never create shared memory, only the server does ///client should never create shared memory, only the server does
cl->connect(); cl->setSharedMemoryKey(key);
cl->connect();
return (b3PhysicsClientHandle ) cl; return (b3PhysicsClientHandle ) cl;
} }
@@ -228,22 +314,37 @@ void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
delete cl; delete cl;
} }
int b3ProcessServerStatus(b3PhysicsClientHandle physClient, struct SharedMemoryStatus* status) b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
return (int)cl->processServerStatus(*status); const SharedMemoryStatus* stat = cl->processServerStatus();
return (b3SharedMemoryStatusHandle) stat;
} }
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
if (status)
{
return status->m_type;
}
return 0;
}
int b3CanSubmitCommand(b3PhysicsClientHandle physClient) int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
return (int)cl->canSubmitCommand(); return (int)cl->canSubmitCommand();
} }
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command) int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{ {
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
return (int)cl->submitClientCommand(*command); PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
return (int)cl->submitClientCommand(*command);
} }
@@ -262,3 +363,33 @@ void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3Jo
} }
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_DEBUG_LINES;
command->m_requestDebugLinesArguments.m_debugMode = debugMode;
command->m_requestDebugLinesArguments.m_startingLineIndex = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines)
{
PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient;
b3Assert(lines);
if (lines)
{
lines->m_numDebugLines = cl->getNumDebugLines();
lines->m_linesFrom = cl->getDebugLinesFrom();
lines->m_linesTo = cl->getDebugLinesTo();
lines->m_linesColor = cl->getDebugLinesColor();
}
}

68
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -1,55 +1,68 @@
#ifndef PHYSICS_CLIENT_C_API_H #ifndef PHYSICS_CLIENT_C_API_H
#define PHYSICS_CLIENT_C_API_H #define PHYSICS_CLIENT_C_API_H
#include "SharedMemoryBlock.h" //#include "SharedMemoryBlock.h"
#include "SharedMemoryPublic.h"
#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name #define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
B3_DECLARE_HANDLE(b3PhysicsClientHandle); B3_DECLARE_HANDLE(b3PhysicsClientHandle);
B3_DECLARE_HANDLE(b3PhysicsRobotHandle); B3_DECLARE_HANDLE(b3SharedMemoryCommandHandle);
B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
///make sure to start the server first! ///make sure to start the server first!
b3PhysicsClientHandle b3ConnectSharedMemory(); b3PhysicsClientHandle b3ConnectSharedMemory(int key);
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
int b3ProcessServerStatus(b3PhysicsClientHandle physClient, struct SharedMemoryStatus* status); b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3CanSubmitCommand(b3PhysicsClientHandle physClient); int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command); int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
int b3GetNumJoints(b3PhysicsClientHandle physClient); int b3GetNumJoints(b3PhysicsClientHandle physClient);
void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info); void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info);
int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command); b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(struct SharedMemoryCommand* command, double timeStep);
int b3InitStepSimulationCommand(struct SharedMemoryCommand* command); void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
int b3InitResetSimulationCommand(struct SharedMemoryCommand* command);
int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfFileName); b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
///all those commands are optional, except for the *Init ///all those commands are optional, except for the *Init
int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ); int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(struct SharedMemoryCommand* command, int useMultiBody); int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(struct SharedMemoryCommand* command, int useFixedBase); int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
///Set joint control variables such as desired position/angle, desired velocity, ///Set joint control variables such as desired position/angle, desired velocity,
///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)
int b3JointControlCommandInit(struct SharedMemoryCommand* command, int controlMode); 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 b3JointControlSetKp(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(struct SharedMemoryCommand* command, int dofIndex, double value); int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetMaximumForce(struct SharedMemoryCommand* command, 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(struct SharedMemoryCommand* command, int dofIndex, double value); int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///the creation of collision shapes and rigid bodies etc is likely going to change, ///the creation of collision shapes and rigid bodies etc is likely going to change,
@@ -57,17 +70,18 @@ int b3JointControlSetDesiredForceTorque(struct SharedMemoryCommand* command, int
//create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1) //create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1)
//after that, you can optionally adjust the initial position, orientation and size //after that, you can optionally adjust the initial position, orientation and size
int b3CreateBoxShapeCommandInit(struct SharedMemoryCommand* command); b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient);
int b3CreateBoxCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ); int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreateBoxCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreateBoxCommandSetHalfExtents(struct SharedMemoryCommand* command, double halfExtentsX,double halfExtentsY,double halfExtentsZ); int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
int b3CreateSensorCommandInit(struct SharedMemoryCommand* command); b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* command, int dofIndex, int enable); int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int dofIndex, int enable);
int b3CreateSensorEnableIMUForLink(struct SharedMemoryCommand* command, int linkIndex, int enable); int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient);
int b3RequestActualStateCommandInit(struct SharedMemoryCommand* command);
#ifdef __cplusplus #ifdef __cplusplus

404
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -7,7 +7,9 @@
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsClient.h" #include "PhysicsClient.h"
#include "SharedMemoryCommands.h" //#include "SharedMemoryCommands.h"
#include "PhysicsClientC_API.h"
struct MyMotorInfo2 struct MyMotorInfo2
{ {
@@ -16,16 +18,17 @@ struct MyMotorInfo2
int m_uIndex; int m_uIndex;
}; };
#define MAX_NUM_MOTORS 128 #define MAX_NUM_MOTORS 128
class PhysicsClientExample : public SharedMemoryCommon class PhysicsClientExample : public SharedMemoryCommon
{ {
protected: protected:
PhysicsClientSharedMemory m_physicsClient; b3PhysicsClientHandle m_physicsClientHandle;
bool m_wantsTermination; bool m_wantsTermination;
btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests; btAlignedObjectArray<int> m_userCommandRequests;
int m_sharedMemoryKey;
void createButton(const char* name, int id, bool isTrigger ); void createButton(const char* name, int id, bool isTrigger );
@@ -62,18 +65,22 @@ public:
virtual bool isConnected() virtual bool isConnected()
{ {
return m_physicsClient.isConnected(); return (m_physicsClientHandle!=0);
} }
void enqueueCommand(const SharedMemoryCommand& orgCommand);
void enqueueCommand(int commandId);
void prepareAndSubmitCommand(int commandId);
virtual void exitPhysics(){}; virtual void exitPhysics(){};
virtual void renderScene() virtual void renderScene()
{ {
int numLines = m_physicsClient.getNumDebugLines(); b3DebugLines debugLines;
const btVector3* fromLines = m_physicsClient.getDebugLinesFrom(); b3GetDebugLines(m_physicsClientHandle,&debugLines);
const btVector3* toLines = m_physicsClient.getDebugLinesTo(); int numLines = debugLines.m_numDebugLines;
const btVector3* colorLines = m_physicsClient.getDebugLinesColor();
int lineWidth = 1; int lineWidth = 1;
@@ -86,12 +93,12 @@ public:
for (int i=0;i<numLines;i++) for (int i=0;i<numLines;i++)
{ {
points[i*2].m_floats[0] = fromLines[i].x(); points[i*2].m_floats[0] = debugLines.m_linesFrom[i*3+0];
points[i*2].m_floats[1] = fromLines[i].y(); points[i*2].m_floats[1] = debugLines.m_linesFrom[i*3+1];
points[i*2].m_floats[2] = fromLines[i].z(); points[i*2].m_floats[2] = debugLines.m_linesFrom[i*3+2];
points[i*2+1].m_floats[0] = toLines[i].x(); points[i*2+1].m_floats[0] = debugLines.m_linesTo[i*3+0];
points[i*2+1].m_floats[1] = toLines[i].y(); points[i*2+1].m_floats[1] = debugLines.m_linesTo[i*3+1];
points[i*2+1].m_floats[2] = toLines[i].z(); points[i*2+1].m_floats[2] = debugLines.m_linesTo[i*3+2];
indices[i*2] = i*2; indices[i*2] = i*2;
indices[i*2+1] = i*2+1; indices[i*2+1] = i*2+1;
} }
@@ -108,32 +115,21 @@ public:
{ {
for (int i=0;i<numLines;i++) for (int i=0;i<numLines;i++)
{ {
m_guiHelper->getRenderInterface()->drawLine(fromLines[i],toLines[i],colorLines[i],lineWidth); m_guiHelper->getRenderInterface()->drawLine(debugLines.m_linesFrom,debugLines.m_linesTo,debugLines.m_linesColor,lineWidth);
} }
} }
} }
void prepareControlCommand(SharedMemoryCommand& command) void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
{ {
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<m_numMotors;i++) for (int i=0;i<m_numMotors;i++)
{ {
btScalar targetVel = m_motorTargetVelocities[i].m_velTarget; btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
int uIndex = m_motorTargetVelocities[i].m_uIndex; int uIndex = m_motorTargetVelocities[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
} }
} }
virtual void physicsDebugDraw(int debugFlags){} virtual void physicsDebugDraw(int debugFlags){}
virtual bool mouseMoveCallback(float x,float y){return false;}; virtual bool mouseMoveCallback(float x,float y){return false;};
@@ -143,7 +139,7 @@ public:
virtual void setSharedMemoryKey(int key) virtual void setSharedMemoryKey(int key)
{ {
m_physicsClient.setSharedMemoryKey(key); m_sharedMemoryKey = key;
} }
}; };
@@ -154,102 +150,103 @@ public:
void MyCallback(int buttonId, bool buttonState, void* userPtr) void MyCallback(int buttonId, bool buttonState, void* userPtr)
{ {
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr; PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
if (buttonState)
{
cl->enqueueCommand(buttonId);
}
SharedMemoryCommand command;
switch (buttonId)
{
case CMD_LOAD_URDF:
{
command.m_type =CMD_LOAD_URDF;
sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf");
command.m_urdfArguments.m_initialPosition[0] = 0.0;
command.m_updateFlags =
URDF_ARGS_FILE_NAME| URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION|URDF_ARGS_USE_MULTIBODY|URDF_ARGS_USE_FIXED_BASE;
command.m_urdfArguments.m_initialPosition[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
command.m_urdfArguments.m_initialOrientation[1] = 0.0;
command.m_urdfArguments.m_initialOrientation[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[3] = 1.0;
command.m_urdfArguments.m_useFixedBase = false;
command.m_urdfArguments.m_useMultiBody = true;
cl->enqueueCommand(command);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
cl->enqueueCommand(command);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
command.m_type =CMD_REQUEST_ACTUAL_STATE;
cl->enqueueCommand(command);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
{
command.m_type =CMD_STEP_FORWARD_SIMULATION;
cl->enqueueCommand(command);
command.m_type =CMD_REQUEST_DEBUG_LINES;
command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//:DBG_DrawConstraints;
command.m_requestDebugLinesArguments.m_startingLineIndex = 0;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_DESIRED_STATE:
{
command.m_type =CMD_SEND_DESIRED_STATE;
cl->prepareControlCommand(command);
cl->enqueueCommand(command);
break;
}
case CMD_RESET_SIMULATION:
{
command.m_type = CMD_RESET_SIMULATION;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
command.m_type = buttonId;
cl->enqueueCommand(command);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
} }
void PhysicsClientExample::enqueueCommand(const SharedMemoryCommand& orgCommand) void PhysicsClientExample::enqueueCommand(int commandId)
{ {
m_userCommandRequests.push_back(orgCommand); m_userCommandRequests.push_back(commandId);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1]; }
//b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size()); void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
} {
switch (commandId)
{
case CMD_LOAD_URDF:
{
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "r2d2.urdf");//kuka_lwr/kuka.urdf");
//setting the initial position, orientation and other arguments are optional
double startPosX = 0;
double startPosY = 0;
double startPosZ = 0;
int ret = b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
ret = b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
};
case CMD_STEP_FORWARD_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_REQUEST_DEBUG_LINES:
{
b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_DESIRED_STATE:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
break;
}
case CMD_RESET_SIMULATION:
{
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
#if 0
//this worked, but needs C-API and a streaming options, similar to debug lines
command.m_type = buttonId;
cl->enqueueCommand(command);
#endif
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper) PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper), :SharedMemoryCommon(helper),
m_wantsTermination(false), m_wantsTermination(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_numMotors(0) m_numMotors(0)
{ {
b3Printf("Started PhysicsClientExample\n"); b3Printf("Started PhysicsClientExample\n");
@@ -299,27 +296,11 @@ void PhysicsClientExample::initPhysics()
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this); MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_RESET_SIMULATION,true,this); MyCallback(CMD_RESET_SIMULATION,true,this);
// MyCallback(CMD_LOAD_URDF, true, this);
// MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
// MyCallback(CMD_RESET_SIMULATION,true,this);
/*
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
*/
} }
if (!m_physicsClient.connect()) m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
{ if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client"); b3Warning("Cannot connect to physics client");
} }
@@ -328,112 +309,95 @@ void PhysicsClientExample::initPhysics()
void PhysicsClientExample::stepSimulation(float deltaTime) void PhysicsClientExample::stepSimulation(float deltaTime)
{ {
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (m_physicsClient.isConnected()) if (hasStatus)
{ {
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status); int statusType = b3GetStatusType(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
if (statusType == CMD_URDF_LOADING_COMPLETED)
{ {
for (int i=0;i<m_physicsClient.getNumJoints();i++) int numJoints = b3GetNumJoints(m_physicsClientHandle);
for (int i=0;i<numJoints;i++)
{ {
b3JointInfo info; b3JointInfo info;
m_physicsClient.getJointInfo(i,info); b3GetJointInfo(m_physicsClientHandle,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
} }
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED) for (int i=0;i<numJoints;i++)
{ {
for (int i=0;i<m_physicsClient.getNumJoints();i++) b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{ {
b3JointInfo info; if (m_numMotors<MAX_NUM_MOTORS)
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{ {
if (m_numMotors<MAX_NUM_MOTORS) char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{ {
char motorName[1024]; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
} }
m_numMotors++;
} }
} }
} }
} }
if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED) }
{ if (b3CanSubmitCommand(m_physicsClientHandle))
SharedMemoryCommand command; {
if (status.m_sendDebugLinesArgs.m_numRemainingDebugLines>0) if (m_userCommandRequests.size())
{ {
//continue requesting debug lines for drawing //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
command.m_type =CMD_REQUEST_DEBUG_LINES; int commandId = m_userCommandRequests[0];
command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//DBG_DrawConstraints;
command.m_requestDebugLinesArguments.m_startingLineIndex = status.m_sendDebugLinesArgs.m_numDebugLines+status.m_sendDebugLinesArgs.m_startingLineIndex;
enqueueCommand(command);
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
SharedMemoryCommand command = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands //a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++) for (int i=1;i<m_userCommandRequests.size();i++)
{ {
m_userCommandRequests[i-1] = m_userCommandRequests[i]; m_userCommandRequests[i-1] = m_userCommandRequests[i];
} }
m_userCommandRequests.pop_back(); m_userCommandRequests.pop_back();
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (command.m_type==CMD_RESET_SIMULATION) if (commandId ==CMD_RESET_SIMULATION)
{ {
if (m_guiHelper->getParameterInterface()) if (m_guiHelper->getParameterInterface())
{ {
m_guiHelper->getParameterInterface()->removeAllParameters(); m_guiHelper->getParameterInterface()->removeAllParameters();
} }
m_numMotors=0; m_numMotors=0;
createButtons(); createButtons();
} }
m_physicsClient.submitClientCommand(command); prepareAndSubmitCommand(commandId);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION; } else
enqueueCommand(command); {
if (m_numMotors)
command.m_type = CMD_REQUEST_ACTUAL_STATE; {
enqueueCommand(command); enqueueCommand(CMD_SEND_DESIRED_STATE);
} enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
} enqueueCommand(CMD_REQUEST_DEBUG_LINES);
} enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
} }
}
}
} }

2
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -1087,7 +1087,7 @@ void PhysicsServerSharedMemory::processClientCommands()
deleteDynamicsWorld(); deleteDynamicsWorld();
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_RESET_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd); m_data->submitServerStatus(serverCmd);
break; break;

1
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -5,7 +5,6 @@
#include "PhysicsServer.h" #include "PhysicsServer.h"
#include "SharedMemoryCommon.h" #include "SharedMemoryCommon.h"

5
examples/SharedMemory/RobotControlExample.cpp
View File

@@ -3,7 +3,7 @@
#include "RobotControlExample.h" #include "RobotControlExample.h"
#if 0
#include "../CommonInterfaces/CommonParameterInterface.h" #include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsServer.h" #include "PhysicsServer.h"
@@ -15,7 +15,7 @@
#include <string> #include <string>
//const char* blaatnaam = "basename"; //const char* blaatnaam = "basename";
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE 1024
struct MyMotorInfo struct MyMotorInfo
{ {
@@ -665,5 +665,6 @@ class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExam
} }
return example; return example;
} }
#endif

1
examples/SharedMemory/SharedMemoryBlock.h
View File

@@ -1,7 +1,6 @@
#ifndef SHARED_MEMORY_BLOCK_H #ifndef SHARED_MEMORY_BLOCK_H
#define SHARED_MEMORY_BLOCK_H #define SHARED_MEMORY_BLOCK_H
#define SHARED_MEMORY_KEY 12347
#define SHARED_MEMORY_MAGIC_NUMBER 64738 #define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 32 #define SHARED_MEMORY_MAX_COMMANDS 32
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024) #define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)

70
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -4,6 +4,8 @@
//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc) //this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
#include "SharedMemoryPublic.h"
#ifdef __GNUC__ #ifdef __GNUC__
#include <stdint.h> #include <stdint.h>
typedef int32_t smInt32_t; typedef int32_t smInt32_t;
@@ -22,51 +24,7 @@
typedef unsigned long long int smUint64_t; typedef unsigned long long int smUint64_t;
#endif #endif
enum EnumSharedMemoryClientCommand
{
CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,
// CMD_CREATE_RIGID_BODY,
// CMD_DELETE_RIGID_BODY,
CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
// CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED
CMD_INIT_POSE,
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE,
CMD_REQUEST_DEBUG_LINES,
CMD_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION,
CMD_MAX_CLIENT_COMMANDS
};
enum EnumSharedMemoryServerStatus
{
CMD_SHARED_MEMORY_NOT_INITIALIZED=0,
CMD_WAITING_FOR_CLIENT_COMMAND,
//CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client
CMD_CLIENT_COMMAND_COMPLETED,
//the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
CMD_UNKNOWN_COMMAND_FLUSHED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,
CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED,
CMD_RIGID_BODY_CREATION_COMPLETED,
CMD_SET_JOINT_FEEDBACK_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_FAILED,
CMD_DEBUG_LINES_COMPLETED,
CMD_DEBUG_LINES_OVERFLOW_FAILED,
CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED,
CMD_MAX_SERVER_COMMANDS
};
#define SHARED_MEMORY_SERVER_TEST_C #define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 256 #define MAX_DEGREE_OF_FREEDOM 256
@@ -108,13 +66,7 @@ struct SetJointFeedbackArgs
int m_isEnabled; int m_isEnabled;
}; };
//todo: discuss and decide about control mode and combinations
enum {
// POSITION_CONTROL=0,
CONTROL_MODE_VELOCITY=0,
CONTROL_MODE_TORQUE,
CONTROL_MODE_POSITION_VELOCITY_PD,
};
///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position ///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position
///No motors or controls are needed to initialize the pose. It is similar to ///No motors or controls are needed to initialize the pose. It is similar to
@@ -294,22 +246,6 @@ struct SharedMemoryStatus
typedef struct SharedMemoryStatus SharedMemoryStatus_t; typedef struct SharedMemoryStatus SharedMemoryStatus_t;
enum JointInfoFlags
{
JOINT_HAS_MOTORIZED_POWER=1,
};
struct b3JointInfo
{
char* m_linkName;
char* m_jointName;
int m_jointType;
int m_qIndex;
int m_uIndex;
///
int m_flags;
};
#endif //SHARED_MEMORY_COMMANDS_H #endif //SHARED_MEMORY_COMMANDS_H

84
examples/SharedMemory/SharedMemoryPublic.h Normal file
View File

@@ -0,0 +1,84 @@
#ifndef SHARED_MEMORY_PUBLIC_H
#define SHARED_MEMORY_PUBLIC_H
#define SHARED_MEMORY_KEY 12347
enum EnumSharedMemoryClientCommand
{
CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,
// CMD_CREATE_RIGID_BODY,
// CMD_DELETE_RIGID_BODY,
CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
// CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED
CMD_INIT_POSE,
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE,
CMD_REQUEST_DEBUG_LINES,
CMD_STEP_FORWARD_SIMULATION,
CMD_RESET_SIMULATION,
CMD_MAX_CLIENT_COMMANDS
};
enum EnumSharedMemoryServerStatus
{
CMD_SHARED_MEMORY_NOT_INITIALIZED=0,
CMD_WAITING_FOR_CLIENT_COMMAND,
//CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client
CMD_CLIENT_COMMAND_COMPLETED,
//the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
CMD_UNKNOWN_COMMAND_FLUSHED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,
CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED,
CMD_RIGID_BODY_CREATION_COMPLETED,
CMD_SET_JOINT_FEEDBACK_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_FAILED,
CMD_DEBUG_LINES_COMPLETED,
CMD_DEBUG_LINES_OVERFLOW_FAILED,
CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED,
CMD_RESET_SIMULATION_COMPLETED,
CMD_MAX_SERVER_COMMANDS
};
enum JointInfoFlags
{
JOINT_HAS_MOTORIZED_POWER=1,
};
struct b3JointInfo
{
char* m_linkName;
char* m_jointName;
int m_jointType;
int m_qIndex;
int m_uIndex;
///
int m_flags;
};
struct b3DebugLines
{
int m_numDebugLines;
const float* m_linesFrom;//float x,y,z times 'm_numDebugLines'.
const float* m_linesTo;//float x,y,z times 'm_numDebugLines'.
const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'.
};
//todo: discuss and decide about control mode and combinations
enum {
// POSITION_CONTROL=0,
CONTROL_MODE_VELOCITY=0,
CONTROL_MODE_TORQUE,
CONTROL_MODE_POSITION_VELOCITY_PD,
};
#endif//SHARED_MEMORY_PUBLIC_H

10
examples/SharedMemory/premake4.lua
View File

@@ -16,8 +16,14 @@ links {
language "C++" language "C++"
files { files {
"**.cpp", "PhysicsClient.cpp",
"**.h", "PhysicsClientExample.cpp",
"PhysicsServerExample.cpp",
"main.cpp",
"PhysicsClientC_API.cpp",
"PhysicsServer.cpp",
"PosixSharedMemory.cpp",
"Win32SharedMemory.cpp",
"../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h", "../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
"../Importers/ImportURDFDemo/MultiBodyCreationInterface.h", "../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp", "../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",

132
test/SharedMemory/test.c
View File

@@ -1,6 +1,5 @@
#include "SharedMemoryBlock.h" //#include "SharedMemoryCommands.h"
#include "PhysicsClientC_API.h" #include "PhysicsClientC_API.h"
#include "SharedMemoryCommands.h"
#include "Bullet3Common/b3Logging.h" #include "Bullet3Common/b3Logging.h"
#include <string.h> #include <string.h>
@@ -24,39 +23,39 @@ int main(int argc, char* argv[])
double startPosX, startPosY,startPosZ; double startPosX, startPosY,startPosZ;
int imuLinkIndex = -1; int imuLinkIndex = -1;
SharedMemoryCommand_t command;
SharedMemoryStatus_t status;
b3PhysicsClientHandle sm; b3PhysicsClientHandle sm;
b3Printf("timeout = %d\n",timeout); b3Printf("timeout = %d\n",timeout);
printf("hello world\n"); printf("hello world\n");
sm = b3ConnectSharedMemory( allowSharedMemoryInitialization); sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
if (b3CanSubmitCommand(sm)) if (b3CanSubmitCommand(sm))
{ {
ret = b3InitPhysicsParamCommand(&command); {
ret = b3PhysicsParamSetGravity(&command, gravx,gravy, gravz); b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
ret = b3PhysicsParamSetTimeStep(&command, timeStep); ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz);
ret = b3SubmitClientCommand(sm, &command); ret = b3PhysicsParamSetTimeStep(command, timeStep);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3Printf("timeout = %d\n",timeout); b3Printf("timeout = %d\n",timeout);
}
{
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
//setting the initial position, orientation and other arguments are optional
startPosX =2;
startPosY =3;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
ret = b3LoadUrdfCommandInit(&command, urdfFileName); ret = b3SubmitClientCommand(sm, command);
//setting the initial position, orientation and other arguments are optional timeout = MAX_TIMEOUT;
startPosX =2; while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
startPosY =3; b3Printf("timeout = %d\n",timeout);
startPosZ = 1; }
ret = b3LoadUrdfCommandSetStartPosition(&command, startPosX,startPosY,startPosZ);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
b3Printf("timeout = %d\n",timeout);
numJoints = b3GetNumJoints(sm); numJoints = b3GetNumJoints(sm);
for (i=0;i<numJoints;i++) for (i=0;i<numJoints;i++)
@@ -85,70 +84,80 @@ int main(int argc, char* argv[])
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0)) if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{ {
ret = b3CreateSensorCommandInit(&command); b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
if (imuLinkIndex>=0) if (imuLinkIndex>=0)
{ {
ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1); ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
} }
if (sensorJointIndexLeft>=0) if (sensorJointIndexLeft>=0)
{ {
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1); ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
} }
if(sensorJointIndexRight>=0) if(sensorJointIndexRight>=0)
{ {
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1); ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
} }
ret = b3SubmitClientCommand(sm, &command); ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
ret = b3CreateBoxShapeCommandInit(&command);
ret = b3CreateBoxCommandSetStartPosition(&command, 0,0,-1);
ret = b3CreateBoxCommandSetStartOrientation(&command,0,0,0,1);
ret = b3CreateBoxCommandSetHalfExtents(&command, 10,10,1);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
{
b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1);
ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1);
ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
b3RequestActualStateCommandInit(&command); {
ret = b3SubmitClientCommand(sm, &command); b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
timeout = MAX_TIMEOUT; ret = b3SubmitClientCommand(sm, command);
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
#if 0
posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ; posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU; dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("posVarCount = %d\n",posVarCount); b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount); printf("dofCount = %d\n",dofCount);
#endif
b3JointControlCommandInit(&command, CONTROL_MODE_VELOCITY);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{ {
b3JointControlSetDesiredVelocity(&command,dofIndex,1); b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
b3JointControlSetMaximumForce(&command,dofIndex,100); for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{
b3JointControlSetDesiredVelocity(command,dofIndex,1);
b3JointControlSetMaximumForce(command,dofIndex,100);
}
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
///perform some simulation steps for testing ///perform some simulation steps for testing
for ( i=0;i<100;i++) for ( i=0;i<100;i++)
{ {
ret = b3InitStepSimulationCommand(&command);
ret = b3SubmitClientCommand(sm, &command); b3SharedMemoryCommandHandle command = b3InitStepSimulationCommand(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
b3RequestActualStateCommandInit(&command); {
ret = b3SubmitClientCommand(sm, &command); b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
timeout = MAX_TIMEOUT; ret = b3SubmitClientCommand(sm, command);
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
#if 0
if (sensorJointIndexLeft>=0) if (sensorJointIndexLeft>=0)
{ {
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
@@ -164,12 +173,13 @@ int main(int argc, char* argv[])
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1], status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]); status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]);
} }
#endif
ret = b3InitResetSimulationCommand(&command); {
ret = b3SubmitClientCommand(sm, &command); b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT; timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
} }