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.

82
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,11 +374,18 @@ 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;
}
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;