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

exposed a few more methods in the C++ b3RobotSimulatorClientAPI (_NoDirect base class)

Browse Source
This commit is contained in:
erwincoumans
2018-06-09 19:40:12 -07:00
parent 7ac3e263ab
commit 3eebcd40ca
4 changed files with 246 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
examples/RobotSimulator/HelloBulletRobotics.cpp
View File

@@ -6,6 +6,8 @@ int main(int argc, char* argv[])
b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI();
bool isConnected = sim->connect(eCONNECT_SHARED_MEMORY);
if (!isConnected)
{
printf("Using Direct mode\n");
@@ -14,23 +16,36 @@ int main(int argc, char* argv[])
{
printf("Using shared memory\n");
}
//remove all existing objects (if any)
sim->resetSimulation();
sim->setGravity(btVector3(0,0,-9.8));
sim->setNumSolverIterations(100);
b3RobotSimulatorSetPhysicsEngineParameters args;
sim->getPhysicsEngineParameters(args);
int planeUid = sim->loadURDF("plane.urdf");
printf("planeUid = %d\n", planeUid);
int r2d2Uid = sim->loadURDF("r2d2.urdf");
printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid));
btVector3 basePosition = btVector3(0,0,1);
btVector3 basePosition = btVector3(0,0,0.5);
btQuaternion baseOrientation = btQuaternion(0,0,0,1);
sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);
sim->setGravity(btVector3(0,0,-10));
sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);
while (sim->isConnected())
{
btVector3 basePos;
btQuaternion baseOrn;
sim->getBasePositionAndOrientation(r2d2Uid,basePos,baseOrn);
printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0],basePos[1],basePos[2]);
sim->stepSimulation();
}
delete sim;

23
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6859,24 +6859,45 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
bool hasStatus = true;
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED;
serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration;
serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
serverCmd.m_simulationParameterResultArgs.m_contactSlop = m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
serverCmd.m_simulationParameterResultArgs.m_deterministicOverlappingPairs = m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs;
serverCmd.m_simulationParameterResultArgs.m_enableConeFriction = (m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &SOLVER_DISABLE_IMPLICIT_CONE_FRICTION)? 0 : 1;
serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled();
serverCmd.m_simulationParameterResultArgs.m_frictionCFM = m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM;
serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP;
btVector3 grav = m_data->m_dynamicsWorld->getGravity();
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0];
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1];
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2];
serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags;
serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0;
if (gJointFeedbackInWorldSpace)
{
serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_WORLD_SPACE;
}
if (gJointFeedbackInJointFrame )
{
serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode |= JOINT_FEEDBACK_IN_JOINT_FRAME;
}
serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps;
serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;
serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold;
serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold;
serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation;
serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse;
return hasStatus;
}
@@ -7010,7 +7031,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM)
{
m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionCFM;
m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)

118
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -224,6 +224,63 @@ bool b3RobotSimulatorClientAPI_NoDirect::loadMJCF(const std::string& fileName, b
return true;
}
bool b3RobotSimulatorClientAPI_NoDirect::savePythonWorld(const std::string& fileName)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (fileName.length()==0)
{
return false;
}
command = b3SaveWorldCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SAVE_WORLD_COMPLETED)
{
return false;
}
return true;
}
bool b3RobotSimulatorClientAPI_NoDirect::saveBullet(const std::string& fileName)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (fileName.length()==0)
{
return false;
}
command = b3SaveBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_BULLET_SAVING_COMPLETED)
{
return false;
}
return true;
}
bool b3RobotSimulatorClientAPI_NoDirect::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
{
if (!isConnected())
@@ -534,6 +591,19 @@ void b3RobotSimulatorClientAPI_NoDirect::removeConstraint(int constraintId)
b3GetStatusType(statusHandle);
}
bool b3RobotSimulatorClientAPI_NoDirect::getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
if (b3GetUserConstraintInfo(m_data->m_physicsClientHandle, constraintUniqueId, &constraintInfo))
{
return true;
}
return false;
}
bool b3RobotSimulatorClientAPI_NoDirect::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{
@@ -1478,8 +1548,34 @@ bool b3RobotSimulatorClientAPI_NoDirect::setJointMotorControlArray(int bodyUniqu
return true;
}
bool b3RobotSimulatorClientAPI_NoDirect::getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
b3Warning("Not connected");
return false;
}
bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
{
b3SharedMemoryCommandHandle command = b3InitRequestPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType!=CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
{
return false;
}
b3GetStatusPhysicsSimulationParameters(statusHandle,&args);
}
return true;
}
bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters &args)
{
b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
if (sm == 0) {
@@ -1497,12 +1593,12 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo
b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode);
}
if (args.m_numSubSteps >= 0) {
b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps);
if (args.m_numSimulationSubSteps >= 0) {
b3PhysicsParamSetNumSubSteps(command, args.m_numSimulationSubSteps);
}
if (args.m_fixedTimeStep >= 0) {
b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep);
if (args.m_deltaTime >= 0) {
b3PhysicsParamSetTimeStep(command, args.m_deltaTime);
}
if (args.m_useSplitImpulse >= 0) {
@@ -1517,10 +1613,6 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo
b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold);
}
if (args.m_maxNumCmdPer1ms >= -1) {
b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms);
}
if (args.m_restitutionVelocityThreshold>=0) {
b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold);
}
@@ -1529,12 +1621,12 @@ bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(struct b3Robo
b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching);
}
if (args.m_erp>=0) {
b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp);
if (args.m_defaultNonContactERP>=0) {
b3PhysicsParamSetDefaultNonContactERP(command,args.m_defaultNonContactERP);
}
if (args.m_contactERP>=0) {
b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP);
if (args.m_defaultContactERP>=0) {
b3PhysicsParamSetDefaultContactERP(command,args.m_defaultContactERP);
}
if (args.m_frictionERP >=0) {

134
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -200,39 +200,50 @@ struct b3RobotSimulatorGetCameraImageArgs
}
};
struct b3RobotSimulatorSetPhysicsEngineParameters
{
double m_fixedTimeStep;
int m_numSolverIterations;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;
int m_numSubSteps;
int m_collisionFilterMode;
double m_contactBreakingThreshold;
int m_maxNumCmdPer1ms;
int m_enableFileCaching;
double m_restitutionVelocityThreshold;
double m_erp;
double m_contactERP;
double m_frictionERP;
double m_solverResidualThreshold;
struct b3RobotSimulatorSetPhysicsEngineParameters : b3PhysicsSimulationParameters
{
b3RobotSimulatorSetPhysicsEngineParameters()
: m_fixedTimeStep(-1),
m_numSolverIterations(-1),
m_useSplitImpulse(-1),
m_splitImpulsePenetrationThreshold(-1),
m_numSubSteps(-1),
m_collisionFilterMode(-1),
m_contactBreakingThreshold(-1),
m_maxNumCmdPer1ms(-1),
m_enableFileCaching(-1),
m_restitutionVelocityThreshold(-1),
m_erp(-1),
m_contactERP(-1),
m_frictionERP(-1),
m_solverResidualThreshold(-1)
{}
{
m_deltaTime=-1;
m_gravityAcceleration[3];
m_numSimulationSubSteps;
m_numSolverIterations=-1;
m_useRealTimeSimulation = -1;
m_useSplitImpulse = -1;
m_splitImpulsePenetrationThreshold =-1;
m_contactBreakingThreshold = -1;
m_internalSimFlags = -1;
m_defaultContactERP = -1;
m_collisionFilterMode = -1;
m_enableFileCaching = -1;
m_restitutionVelocityThreshold = -1;
m_defaultNonContactERP = -1;
m_frictionERP = -1;
m_defaultGlobalCFM = -1;
m_frictionCFM = -1;
m_enableConeFriction = -1;
m_deterministicOverlappingPairs = -1;
m_allowedCcdPenetration = -1;
m_jointFeedbackMode = -1;
m_solverResidualThreshold = -1;
m_contactSlop = -1;
m_collisionFilterMode=-1;
m_contactBreakingThreshold=-1;
m_enableFileCaching=-1;
m_restitutionVelocityThreshold=-1;
m_frictionERP=-1;
m_solverResidualThreshold=-1;
}
};
struct b3RobotSimulatorChangeDynamicsArgs
@@ -410,6 +421,54 @@ struct b3RobotSimulatorCreateMultiBodyArgs
};
struct b3RobotJointInfo : public b3JointInfo
{
b3RobotJointInfo()
{
m_linkName[0] = 0;
m_jointName[0] = 0;
m_jointType = eFixedType;
m_qIndex = -1;
m_uIndex = -1;
m_jointIndex = -1;
m_flags = 0;
m_jointDamping = 0;
m_jointFriction = 0;
m_jointLowerLimit = 1;
m_jointUpperLimit = -1;
m_jointMaxForce = 500;
m_jointMaxVelocity = 100;
m_parentIndex = -1;
//position
m_parentFrame[0] = 0;
m_parentFrame[1] = 0;
m_parentFrame[2] = 0;
//orientation quaternion [x,y,z,w]
m_parentFrame[3] = 0;
m_parentFrame[4] = 0;
m_parentFrame[5] = 0;
m_parentFrame[6] = 1;
//position
m_childFrame[0] = 0;
m_childFrame[1] = 0;
m_childFrame[2] = 0;
//orientation quaternion [x,y,z,w]
m_childFrame[3] = 0;
m_childFrame[4] = 0;
m_childFrame[5] = 0;
m_childFrame[6] = 1;
m_jointAxis[0] = 0;
m_jointAxis[1] = 0;
m_jointAxis[2] = 1;
}
};
class b3RobotSimulatorClientAPI_NoDirect
{
protected:
@@ -441,7 +500,10 @@ public:
bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs());
bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);
bool saveBullet(const std::string& fileName);
bool savePythonWorld(const std::string& fileName);
bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo);
bool getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const;
@@ -457,9 +519,11 @@ public:
int createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
int changeConstraint(int constraintId, b3JointInfo* jointInfo);
void removeConstraint(int constraintId);
bool getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo);
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
bool getJointStates(int bodyUniqueId, b3JointStates2& state);
@@ -540,7 +604,9 @@ public:
bool setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args);
bool setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args);
bool setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters&args);
bool getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters&args);
bool applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags);