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

Merge pull request #661 from erwincoumans/master

Browse Source 
fix some issues related to controlling a robot/multibody beyond body …
This commit is contained in:
erwincoumans
2016-06-17 07:34:07 -07:00
committed by GitHub
parent 70c7ec6c45 53a0772257
commit c7365837cd
7 changed files with 248 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -151,7 +151,7 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand
}
b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int controlMode)
b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -160,7 +160,7 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = 0;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
@@ -434,7 +434,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient)
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -445,7 +445,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys
command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0;
command->m_createSensorArguments.m_bodyUniqueId = 0;
command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -97,7 +97,7 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
///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)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
@@ -134,7 +134,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
///This is rather inconsistent, to mix programmatical creation with loading from file.
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
///b3CreateSensorEnableIMUForLink is not implemented yet.
///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.

78
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -37,6 +37,9 @@ protected:
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
btAlignedObjectArray<int> m_bodyUniqueIds;
int m_sharedMemoryKey;
int m_selectedBody;
int m_prevSelectedBody;
@@ -66,7 +69,8 @@ protected:
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
int bodyIndex = comboIndex;
int itemIndex = int(atoi(name));
int bodyIndex = m_bodyUniqueIds[itemIndex];
if (m_selectedBody != bodyIndex)
{
m_selectedBody = bodyIndex;
@@ -154,14 +158,20 @@ protected:
void prepareControlCommand(b3SharedMemoryCommandHandle commandHandle)
{
for (int i=0;i<m_numMotors;i++)
{
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
int qIndex = m_motorTargetPositions[i].m_qIndex;
int uIndex = m_motorTargetPositions[i].m_uIndex;
static int serial=0;
serial++;
// b3Printf("# motors = %d, cmd[%d] qIndex = %d, uIndex = %d, targetPos = %f", m_numMotors, serial, qIndex,uIndex,targetPos);
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
b3JointControlSetKp(commandHandle, uIndex, 0.1);
b3JointControlSetKp(commandHandle, qIndex, 0.1);
b3JointControlSetKd(commandHandle, uIndex, 0);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
}
@@ -187,7 +197,7 @@ protected:
void MyComboBoxCallback (int combobox, const char* item, void* userPointer)
{
b3Printf("Item selected %s", item);
//b3Printf("Item selected %s", item);
PhysicsClientExample* cl = (PhysicsClientExample*) userPointer;
b3Assert(cl);
@@ -364,10 +374,17 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
}
case CMD_SEND_DESIRED_STATE:
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
if (m_selectedBody>=0)
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
// b3Printf("prepare control command for body %d", m_selectedBody);
prepareControlCommand(command);
b3SubmitClientCommand(m_physicsClientHandle, command);
}
break;
}
case CMD_RESET_SIMULATION:
@@ -471,9 +488,36 @@ void PhysicsClientExample::createButtons()
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
if (m_bodyUniqueIds.size())
{
if (m_selectedBody<0)
m_selectedBody = 0;
ComboBoxParams comboParams;
comboParams.m_comboboxId = 0;
comboParams.m_numItems = m_bodyUniqueIds.size();
comboParams.m_startItem = m_selectedBody;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
//todo: get the real object name
const char** blarray = new const char*[m_bodyUniqueIds.size()];
for (int i=0;i<m_bodyUniqueIds.size();i++)
{
char* bla = new char[16];
sprintf(bla,"%d", i);
blarray[i] = bla;
comboParams.m_items=blarray;//{&bla};
}
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
if (m_physicsClientHandle && m_selectedBody>=0)
{
m_numMotors = 0;
int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
for (int i=0;i<numJoints;i++)
{
@@ -664,11 +708,16 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
b3Warning("loadSDF number of bodies (%d) exceeds the internal body capacity (%d)",numBodies, bodyCapacity);
} else
{
/*
for (int i=0;i<numBodies;i++)
{
int bodyUniqueId = bodyIndicesOut[i];
m_bodyUniqueIds.push_back(bodyUniqueId);
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
if (numJoints>0)
{
m_selectedBody = bodyUniqueId;
}
/* int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyUniqueId);
b3Printf("numJoints = %d", numJoints);
for (int i=0;i<numJoints;i++)
{
@@ -676,8 +725,9 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
b3GetJointInfo(m_physicsClientHandle,bodyUniqueId,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
}
*/
}
}
//int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
@@ -715,6 +765,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
m_bodyUniqueIds.push_back(bodyIndex);
m_selectedBody = bodyIndex;
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
@@ -723,18 +775,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
comboParams.m_numItems = 1;
comboParams.m_startItem = 0;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
const char* bla = "bla";
const char* blarray[1];
blarray[0] = bla;
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
}
}
@@ -759,6 +800,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
{
m_selectedBody = -1;
m_numMotors=0;
m_bodyUniqueIds.clear();
createButtons();
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
if (m_options == eCLIENTEXAMPLE_SERVER)

24
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -382,7 +382,6 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
@@ -560,7 +559,6 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
}
m_data->m_urdfLinkNameMapper.clear();
m_data->m_multiBodyJointMotorMap.clear();
for (int i=0;i<m_data->m_strings.size();i++)
{
@@ -672,7 +670,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor);
mb->getLink(mbLinkIndex).m_userPtr = motor;
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
motor->finalizeMultiDof();
@@ -1436,10 +1434,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
if (motorPtr)
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = 0.f;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0)
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
@@ -1479,10 +1478,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
if (motorPtr)
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
if (motor)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0)
@@ -1644,10 +1644,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (supportsJointMotor(mb,l))
{
btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[l];
if (motorPtr && m_data->m_physicsDeltaTime>btScalar(0))
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
if (motor && m_data->m_physicsDeltaTime>btScalar(0))
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime;
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] =
force;

173
examples/pybullet/pybullet.c
View File

@@ -1,6 +1,6 @@
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/PhysicsDirectC_API.h"
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#ifdef __APPLE__
@@ -16,6 +16,8 @@ enum eCONNECT_METHOD
eCONNECT_SHARED_MEMORY=3,
};
static PyObject *SpamError;
static b3PhysicsClientHandle sm=0;
@@ -182,6 +184,25 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
return PyLong_FromLong(bodyIndex);
}
static float pybullet_internalGetFloatFromSequence(PyObject* seq, int index)
{
float v = 0.f;
PyObject* item;
if (PyList_Check(seq))
{
item = PyList_GET_ITEM(seq, index);
v = PyFloat_AsDouble(item);
}
else
{
item = PyTuple_GET_ITEM(seq,index);
v = PyFloat_AsDouble(item);
}
return v;
}
#define MAX_SDF_BODIES 512
static PyObject*
@@ -255,6 +276,109 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
return Py_None;
}
static int pybullet_setJointControl(PyObject* self, PyObject* args)
{
//todo(erwincoumans): set max forces, kp, kd
int size;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
size= PySequence_Size(args);
if (size==3)
{
int bodyIndex, controlMode;
PyObject* targetValues;
if (PyArg_ParseTuple(args, "iiO", &bodyIndex, &controlMode, &targetValues))
{
PyObject* seq;
int numJoints, len;
seq = PySequence_Fast(targetValues, "expected a sequence");
len = PySequence_Size(targetValues);
numJoints = b3GetNumJoints(sm,bodyIndex);
b3SharedMemoryCommandHandle commandHandle;
if (len!=numJoints)
{
PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints.");
Py_DECREF(seq);
return NULL;
}
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
{
PyErr_SetString(SpamError, "Illegral control mode.");
Py_DECREF(seq);
return NULL;
}
commandHandle = b3JointControlCommandInit(sm, bodyIndex,controlMode);
for (int qIndex=0;qIndex<numJoints;qIndex++)
{
float value = pybullet_internalGetFloatFromSequence(seq,qIndex);
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, qIndex, value);
break;
}
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, qIndex, value);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition( commandHandle, qIndex, value);
break;
}
default:
{
}
};
}
Py_DECREF(seq);
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "error in setJointControl.");
return NULL;
}
/*
///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)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
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);
// 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);
*/
// Set the gravity of the world with (x, y, z) arguments
static PyObject *
pybullet_setGravity(PyObject* self, PyObject* args)
@@ -448,6 +572,23 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args)
}
// TODO(hellojas): initialize all joint positions given a pylist of values
//
//
/*
///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
///base orientation and joint angles. This will set all velocities of base and joints to zero.
///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
*/
//
//
Py_INCREF(Py_None);
return Py_None;
}
@@ -765,6 +906,7 @@ pybullet_getJointState(PyObject* self, PyObject* args)
}
// internal function to set a float matrix[16]
// used to initialize camera position with
// a view and projection matrix in renderImage()
@@ -773,40 +915,23 @@ pybullet_getJointState(PyObject* self, PyObject* args)
// matrix - float[16] which will be set by values from objMat
static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
{
int i, len;
PyObject* item;
PyObject* seq;
seq = PySequence_Fast(objMat, "expected a sequence");
len = PySequence_Size(objMat);
if (len==16)
{
if (PyList_Check(seq))
{
for (i = 0; i < len; i++)
{
item = PyList_GET_ITEM(seq, i);
matrix[i] = PyFloat_AsDouble(item);
}
}
else
{
for (i = 0; i < len; i++)
{
item = PyTuple_GET_ITEM(seq,i);
matrix[i] = PyFloat_AsDouble(item);
}
matrix[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
Py_DECREF(seq);
return 1;
} else
{
}
Py_DECREF(seq);
return 0;
}
}
// Render an image from the current timestep of the simulation
//
@@ -960,6 +1085,10 @@ static PyMethodDef SpamMethods[] = {
{"getJointState", pybullet_getJointState, METH_VARARGS,
"Get the joint metadata info for a joint on a body."},
{"setJointControl", pybullet_setJointControl, METH_VARARGS,
"Set the joint control mode and desired target values."},
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
@@ -1018,6 +1147,10 @@ initpybullet(void)
PyModule_AddIntConstant (m, "DIRECT", eCONNECT_DIRECT); // user read
PyModule_AddIntConstant (m, "GUI", eCONNECT_GUI); // user read
PyModule_AddIntConstant (m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read
PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);

3
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -132,6 +132,8 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
@@ -149,6 +151,7 @@ btVector3 m_appliedConstraintForce; // In WORLD frame
m_jointFeedback(0),
m_linkName(0),
m_jointName(0),
m_userPtr(0),
m_jointDamping(0),
m_jointFriction(0)
{

2
test/SharedMemory/test.c
View File

@@ -125,7 +125,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{