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

Potential fix for Linux slow performance (usleep(0) takes a lot of time)

Browse Source 
Remove b3RobotSimAPI, use RobotSimulator/b3RobotSimulatorClientAPI.h instead
This commit is contained in:
Erwin Coumans
2017-05-13 13:37:49 -07:00
parent 0944790577
commit ef7a7f9004
10 changed files with 204 additions and 1434 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -60,6 +60,46 @@ void b3RobotSimulatorClientAPI::renderScene()
} }
} }
void b3RobotSimulatorClientAPI::debugDraw(int debugDrawMode)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
if (m_data->m_guiHelper)
{
b3InProcessDebugDrawInternal(m_data->m_physicsClientHandle,debugDrawMode);
}
}
bool b3RobotSimulatorClientAPI::mouseMoveCallback(float x,float y)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
if (m_data->m_guiHelper)
{
return b3InProcessMouseMoveCallback(m_data->m_physicsClientHandle, x,y);
}
return false;
}
bool b3RobotSimulatorClientAPI::mouseButtonCallback(int button, int state, float x, float y)
{
if (!isConnected())
{
b3Warning("Not connected");
return false;
}
if (m_data->m_guiHelper)
{
return b3InProcessMouseButtonCallback(m_data->m_physicsClientHandle, button,state,x,y);
}
return false;
}
bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey) bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
@@ -106,7 +146,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
{ {
key = portOrKey; key = portOrKey;
} }
sm = b3ConnectSharedMemory(key); sm = b3ConnectSharedMemory2(key);
break; break;
} }
case eCONNECT_UDP: case eCONNECT_UDP:
@@ -1088,4 +1128,19 @@ void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string& profileN
b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds); b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
} }
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}
void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double collisionMargin)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClientHandle);
b3LoadBunnySetScale(command, scale);
b3LoadBunnySetMass(command, mass);
b3LoadBunnySetCollisionMargin(command, collisionMargin);
b3SubmitClientCommand(m_data->m_physicsClientHandle, command);
} }

20
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -148,11 +148,6 @@ public:
b3RobotSimulatorClientAPI(); b3RobotSimulatorClientAPI();
virtual ~b3RobotSimulatorClientAPI(); virtual ~b3RobotSimulatorClientAPI();
//setGuiHelper is only used when embedded in existing example browser
void setGuiHelper(struct GUIHelperInterface* guiHelper);
//renderScene is only used when embedded in existing example browser
virtual void renderScene();
bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
void disconnect(); void disconnect();
@@ -231,6 +226,21 @@ public:
void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1); void submitProfileTiming(const std::string& profileName, int durationInMicroSeconds=1);
//////////////// INTERNAL
void loadBunny(double scale, double mass, double collisionMargin);
//setGuiHelper is only used when embedded in existing example browser
void setGuiHelper(struct GUIHelperInterface* guiHelper);
//renderScene is only used when embedded in existing example browser
virtual void renderScene();
//debugDraw is only used when embedded in existing example browser
virtual void debugDraw(int debugDrawMode);
virtual bool mouseMoveCallback(float x,float y);
virtual bool mouseButtonCallback(int button, int state, float x, float y);
////////////////INTERNAL
}; };
#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H #endif //B3_ROBOT_SIMULATOR_CLIENT_API_H

167
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -13,7 +13,7 @@
#include "../SharedMemory/SharedMemoryPublic.h" #include "../SharedMemory/SharedMemoryPublic.h"
#include <string> #include <string>
#include "b3RobotSimAPI.h" #include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h" #include "../Utils/b3Clock.h"
static btScalar sGripperVerticalVelocity = 0.f; static btScalar sGripperVerticalVelocity = 0.f;
@@ -23,7 +23,7 @@ class GripperGraspExample : public CommonExampleInterface
{ {
CommonGraphicsApp* m_app; CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper; GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim; b3RobotSimulatorClientAPI m_robotSim;
int m_options; int m_options;
int m_gripperIndex; int m_gripperIndex;
@@ -55,8 +55,12 @@ public:
} }
virtual void initPhysics() virtual void initPhysics()
{ {
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected); int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
b3Printf("robotSim connected = %d",connected);
if ((m_options & eGRIPPER_GRASP)!=0) if ((m_options & eGRIPPER_GRASP)!=0)
{ {
@@ -76,23 +80,18 @@ public:
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
} }
{ {
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results; b3RobotSimulatorLoadUrdfFileArgs args;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0, 0, .107); args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0); args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true; args.m_useMultiBody = true;
m_robotSim.loadFile(args, results); m_robotSim.loadURDF("cube_small.urdf", args);
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadFileResults results;
args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf"; m_robotSim.loadSDF("gripper/wsg50_with_r2d2_gripper.sdf",results);
args.m_fileType = B3_SDF_FILE; if (results.m_uniqueObjectIds.size()==1)
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{ {
m_gripperIndex = results.m_uniqueObjectIds[0]; m_gripperIndex = results.m_uniqueObjectIds[0];
@@ -111,7 +110,7 @@ public:
double fingerTargetPositions[2]={-0.04,0.04}; double fingerTargetPositions[2]={-0.04,0.04};
for (int i=0;i<2;i++) for (int i=0;i<2;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
controlArgs.m_targetPosition = fingerTargetPositions[i]; controlArgs.m_targetPosition = fingerTargetPositions[i];
controlArgs.m_kp = 5.0; controlArgs.m_kp = 5.0;
controlArgs.m_kd = 3.0; controlArgs.m_kd = 3.0;
@@ -124,7 +123,7 @@ public:
double maxTorqueValues[3]={40.0,50.0,50.0}; double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.; controlArgs.m_kd = 1.;
@@ -135,15 +134,7 @@ public:
if (1) if (1)
{ {
b3RobotSimLoadFileArgs args(""); m_robotSim.loadURDF("plane.urdf");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.setNumSimulationSubSteps(4); m_robotSim.setNumSimulationSubSteps(4);
@@ -152,24 +143,17 @@ public:
if ((m_options & eTWO_POINT_GRASP)!=0) if ((m_options & eTWO_POINT_GRASP)!=0)
{ {
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_small.urdf";
args.m_startPosition.setValue(0, 0, .107); args.m_startPosition.setValue(0, 0, .107);
args.m_startOrientation.setEulerZYX(0, 0, 0); m_robotSim.loadURDF("cube_small.urdf", args);
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_gripper_left.urdf";
args.m_startPosition.setValue(0.068, 0.02, 0.11); args.m_startPosition.setValue(0.068, 0.02, 0.11);
args.m_useMultiBody = true; m_robotSim.loadURDF("cube_gripper_left.urdf", args);
m_robotSim.loadFile(args, results);
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = -0.1; controlArgs.m_targetVelocity = -0.1;
controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_maxTorqueValue = 10.0;
controlArgs.m_kd = 1.; controlArgs.m_kd = 1.;
@@ -177,14 +161,11 @@ public:
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimLoadFileResults results;
args.m_fileName = "cube_gripper_right.urdf";
args.m_startPosition.setValue(-0.068, 0.02, 0.11); args.m_startPosition.setValue(-0.068, 0.02, 0.11);
args.m_useMultiBody = true; m_robotSim.loadURDF("cube_gripper_right.urdf", args);
m_robotSim.loadFile(args, results);
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = 0.1; controlArgs.m_targetVelocity = 0.1;
controlArgs.m_maxTorqueValue = 10.0; controlArgs.m_maxTorqueValue = 10.0;
controlArgs.m_kd = 1.; controlArgs.m_kd = 1.;
@@ -193,14 +174,7 @@ public:
if (1) if (1)
{ {
b3RobotSimLoadFileArgs args(""); m_robotSim.loadURDF("plane.urdf");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setGravity(b3MakeVector3(0,0,-10));
@@ -225,22 +199,17 @@ public:
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimLoadFileResults results;
args.m_fileName = "dinnerware/pan_tefal.urdf";
args.m_startPosition.setValue(0, -0.2, .47); args.m_startPosition.setValue(0, -0.2, .47);
args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0); args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0);
args.m_useMultiBody = true; m_robotSim.loadURDF("dinnerware/pan_tefal.urdf", args);
m_robotSim.loadFile(args, results);
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadFileResults args;
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; b3RobotSimulatorLoadFileResults results;
args.m_fileType = B3_SDF_FILE; m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results);
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results; if (results.m_uniqueObjectIds.size()==1)
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{ {
m_gripperIndex = results.m_uniqueObjectIds[0]; m_gripperIndex = results.m_uniqueObjectIds[0];
@@ -256,7 +225,7 @@ public:
for (int i=0;i<8;i++) for (int i=0;i<8;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0; controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
} }
@@ -266,15 +235,7 @@ public:
if (1) if (1)
{ {
b3RobotSimLoadFileArgs args(""); m_robotSim.loadURDF("plane.urdf");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setGravity(b3MakeVector3(0,0,-10));
@@ -316,8 +277,8 @@ public:
revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[1] = 0.0;
revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0;
revoluteJoint2.m_jointType = ePoint2PointType; revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1); m_robotSim.createConstraint(1, 2, 1, 4, &revoluteJoint1);
m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2); m_robotSim.createConstraint(1, 3, 1, 6, &revoluteJoint2);
} }
if ((m_options & eGRASP_SOFT_BODY)!=0) if ((m_options & eGRASP_SOFT_BODY)!=0)
@@ -337,13 +298,10 @@ public:
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadFileResults results;
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf",results);
args.m_fileType = B3_SDF_FILE;
args.m_useMultiBody = true; if (results.m_uniqueObjectIds.size()==1)
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{ {
m_gripperIndex = results.m_uniqueObjectIds[0]; m_gripperIndex = results.m_uniqueObjectIds[0];
@@ -359,7 +317,7 @@ public:
for (int i=0;i<8;i++) for (int i=0;i<8;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0; controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs); m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
} }
@@ -367,14 +325,10 @@ public:
} }
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,-0.2); args.m_startPosition.setValue(0,0,-0.2);
args.m_startOrientation.setEulerZYX(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true; m_robotSim.loadURDF("plane.urdf", args);
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setGravity(b3MakeVector3(0,0,-10));
@@ -418,23 +372,20 @@ public:
revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[1] = 0.0;
revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0;
revoluteJoint2.m_jointType = ePoint2PointType; revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
} }
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0) if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0)
{ {
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
args.m_fileName = "kuka_iiwa/model_free_base.urdf";
args.m_startPosition.setValue(0,1.0,2.0); args.m_startPosition.setValue(0,1.0,2.0);
args.m_startOrientation.setEulerZYX(0,0,1.57); args.m_startOrientation.setEulerZYX(0,0,1.57);
args.m_forceOverrideFixedBase = false; args.m_forceOverrideFixedBase = false;
args.m_useMultiBody = true; args.m_useMultiBody = true;
b3RobotSimLoadFileResults results; int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args);
m_robotSim.loadFile(args,results);
int kukaId = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(kukaId); int numJoints = m_robotSim.getNumJoints(kukaId);
b3Printf("numJoints = %d",numJoints); b3Printf("numJoints = %d",numJoints);
@@ -443,20 +394,18 @@ public:
b3JointInfo jointInfo; b3JointInfo jointInfo;
m_robotSim.getJointInfo(kukaId,i,&jointInfo); m_robotSim.getJointInfo(kukaId,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0; controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(kukaId,i,controlArgs); m_robotSim.setJointMotorControl(kukaId,i,controlArgs);
} }
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0); args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true; args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = false; args.m_useMultiBody = false;
b3RobotSimLoadFileResults results; m_robotSim.loadURDF("plane.urdf", args);
m_robotSim.loadFile(args,results);
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10)); m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.loadBunny(0.5,10.0,0.1); m_robotSim.loadBunny(0.5,10.0,0.1);
@@ -479,7 +428,7 @@ public:
double maxTorqueValues[3]={40.0,50.0,50.0}; double maxTorqueValues[3]={40.0,50.0,50.0};
for (int i=0;i<3;i++) for (int i=0;i<3;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.; controlArgs.m_kd = 1.;
@@ -496,7 +445,7 @@ public:
double maxTorqueValues[2]={800.0,800.0}; double maxTorqueValues[2]={800.0,800.0};
for (int i=0;i<2;i++) for (int i=0;i<2;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.; controlArgs.m_kd = 1.;
@@ -512,7 +461,7 @@ public:
double maxTorqueValues[2]={50.0,10.0}; double maxTorqueValues[2]={50.0,10.0};
for (int i=0;i<2;i++) for (int i=0;i<2;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i]; controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i]; controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.; controlArgs.m_kd = 1.;
@@ -531,17 +480,13 @@ public:
} }
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y) virtual bool mouseMoveCallback(float x,float y)
{ {
return false; return m_robotSim.mouseMoveCallback(x,y);
} }
virtual bool mouseButtonCallback(int button, int state, float x, float y) virtual bool mouseButtonCallback(int button, int state, float x, float y)
{ {
return false; return m_robotSim.mouseButtonCallback(button,state,x,y);
} }
virtual bool keyboardCallback(int key, int state) virtual bool keyboardCallback(int key, int state)
{ {

9
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -58,10 +58,7 @@ public:
} }
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics() virtual void initPhysics()
{ {
@@ -266,9 +263,9 @@ public:
} }
virtual void physicsDebugDraw() virtual void physicsDebugDraw(int debugDrawMode)
{ {
m_robotSim.debugDraw(debugDrawMode);
} }
virtual bool mouseMoveCallback(float x,float y) virtual bool mouseMoveCallback(float x,float y)
{ {

77
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -11,7 +11,7 @@
#include "../SharedMemory/PhysicsClientC_API.h" #include "../SharedMemory/PhysicsClientC_API.h"
#include <string> #include <string>
#include "b3RobotSimAPI.h" #include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h" #include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis ///quick demo showing the right-handed coordinate system and positive rotations around each axis
@@ -19,7 +19,7 @@ class R2D2GraspExample : public CommonExampleInterface
{ {
CommonGraphicsApp* m_app; CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper; GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim; b3RobotSimulatorClientAPI m_robotSim;
int m_options; int m_options;
int m_r2d2Index; int m_r2d2Index;
@@ -51,20 +51,22 @@ public:
} }
virtual void initPhysics() virtual void initPhysics()
{ {
bool connected = m_robotSim.connect(m_guiHelper); int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
b3Printf("robotSim connected = %d",connected); b3Printf("robotSim connected = %d",connected);
if ((m_options & eROBOTIC_LEARN_GRASP)!=0) if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
{ {
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
args.m_fileName = "r2d2.urdf";
args.m_startPosition.setValue(0,0,.5); args.m_startPosition.setValue(0,0,.5);
b3RobotSimLoadFileResults results; m_r2d2Index = m_robotSim.loadURDF("r2d2.urdf",args);
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
if (m_r2d2Index>=0)
{ {
m_r2d2Index = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_r2d2Index); int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
b3Printf("numJoints = %d",numJoints); b3Printf("numJoints = %d",numJoints);
@@ -78,7 +80,7 @@ public:
int wheelTargetVelocities[4]={-10,-10,-10,-10}; int wheelTargetVelocities[4]={-10,-10,-10,-10};
for (int i=0;i<4;i++) for (int i=0;i<4;i++)
{ {
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i]; controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30; controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs); m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
@@ -86,84 +88,67 @@ public:
} }
} }
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadFileResults results;
args.m_fileName = "kiva_shelf/model.sdf"; m_robotSim.loadSDF("kiva_shelf/model.sdf",results);
args.m_forceOverrideFixedBase = true;
args.m_fileType = B3_SDF_FILE;
args.m_startOrientation = b3Quaternion(0,0,0,1);
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
} }
{ {
b3RobotSimLoadFileArgs args(""); m_robotSim.loadURDF("results");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
} }
if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0) if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimLoadFileResults results; b3RobotSimulatorLoadFileResults results;
{ {
args.m_fileName = "cube_soft.urdf";
args.m_startPosition.setValue(0,0,2.5); args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results); m_r2d2Index = m_robotSim.loadURDF("cube_soft.urdf",args);
} }
{ {
args.m_fileName = "cube_no_friction.urdf";
args.m_startPosition.setValue(0,2,2.5); args.m_startPosition.setValue(0,2,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results); m_robotSim.loadURDF("cube_no_friction.urdf",args);
} }
{ {
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0); args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_startOrientation.setEulerZYX(0,0.2,0);
args.m_forceOverrideFixedBase = true; args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results; m_robotSim.loadURDF("plane.urdf",args);
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
} }
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
} }
if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0) if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0)
{ {
b3RobotSimLoadFileArgs args(""); b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimLoadFileResults results; b3RobotSimulatorLoadFileResults results;
{ {
args.m_fileName = "sphere2_rolling_friction.urdf";
args.m_startPosition.setValue(0,0,2.5); args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = true; args.m_useMultiBody = true;
m_robotSim.loadFile(args,results); m_robotSim.loadURDF("sphere2_rolling_friction.urdf",args);
} }
{ {
args.m_fileName = "sphere2.urdf";
args.m_startPosition.setValue(0,2,2.5); args.m_startPosition.setValue(0,2,2.5);
args.m_startOrientation.setEulerZYX(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0);
args.m_useMultiBody = true; args.m_useMultiBody = true;
m_robotSim.loadFile(args,results); m_robotSim.loadURDF("sphere2.urdf", args);
} }
{ {
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0); args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0.2,0); args.m_startOrientation.setEulerZYX(0,0.2,0);
args.m_useMultiBody = true; args.m_useMultiBody = true;
args.m_forceOverrideFixedBase = true; args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results; m_robotSim.loadURDF("plane.urdf", args);
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
} }
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
} }

1063
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

File diff suppressed because it is too large Load Diff

174
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -1,174 +0,0 @@
#ifndef B3_ROBOT_SIM_API_H
#define B3_ROBOT_SIM_API_H
///todo: remove those includes from this header
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3Transform.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include <string>
enum b3RigidSimFileType
{
B3_URDF_FILE=1,
B3_SDF_FILE,
B3_AUTO_DETECT_FILE//todo based on extension
};
struct b3JointStates
{
int m_bodyUniqueId;
int m_numDegreeOfFreedomQ;
int m_numDegreeOfFreedomU;
b3Transform m_rootLocalInertialFrame;
b3AlignedObjectArray<double> m_actualStateQ;
b3AlignedObjectArray<double> m_actualStateQdot;
b3AlignedObjectArray<double> m_jointReactionForces;
};
struct b3RobotSimLoadFileArgs
{
std::string m_fileName;
b3Vector3 m_startPosition;
b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase;
bool m_useMultiBody;
int m_fileType;
b3RobotSimLoadFileArgs(const std::string& fileName)
:m_fileName(fileName),
m_startPosition(b3MakeVector3(0,0,0)),
m_startOrientation(b3Quaternion(0,0,0,1)),
m_forceOverrideFixedBase(false),
m_useMultiBody(true),
m_fileType(B3_URDF_FILE)
{
}
};
struct b3RobotSimLoadFileResults
{
b3AlignedObjectArray<int> m_uniqueObjectIds;
b3RobotSimLoadFileResults()
{
}
};
struct b3JointMotorArgs
{
int m_controlMode;
double m_targetPosition;
double m_kp;
double m_targetVelocity;
double m_kd;
double m_maxTorqueValue;
b3JointMotorArgs(int controlMode)
:m_controlMode(controlMode),
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.9),
m_maxTorqueValue(1000)
{
}
};
enum b3InverseKinematicsFlags
{
B3_HAS_IK_TARGET_ORIENTATION=1,
B3_HAS_NULL_SPACE_VELOCITY=2,
B3_HAS_JOINT_DAMPING=4,
};
struct b3RobotSimInverseKinematicArgs
{
int m_bodyUniqueId;
// double* m_currentJointPositions;
// int m_numPositions;
double m_endEffectorTargetPosition[3];
double m_endEffectorTargetOrientation[4];
int m_endEffectorLinkIndex;
int m_flags;
int m_numDegreeOfFreedom;
b3AlignedObjectArray<double> m_lowerLimits;
b3AlignedObjectArray<double> m_upperLimits;
b3AlignedObjectArray<double> m_jointRanges;
b3AlignedObjectArray<double> m_restPoses;
b3AlignedObjectArray<double> m_jointDamping;
b3RobotSimInverseKinematicArgs()
:m_bodyUniqueId(-1),
m_endEffectorLinkIndex(-1),
m_flags(0)
{
m_endEffectorTargetPosition[0]=0;
m_endEffectorTargetPosition[1]=0;
m_endEffectorTargetPosition[2]=0;
m_endEffectorTargetOrientation[0]=0;
m_endEffectorTargetOrientation[1]=0;
m_endEffectorTargetOrientation[2]=0;
m_endEffectorTargetOrientation[3]=1;
}
};
struct b3RobotSimInverseKinematicsResults
{
int m_bodyUniqueId;
b3AlignedObjectArray<double> m_calculatedJointPositions;
};
class b3RobotSimAPI
{
struct b3RobotSimAPI_InternalData* m_data;
void processMultiThreadedGraphicsRequests();
b3SharedMemoryStatusHandle submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
public:
b3RobotSimAPI();
virtual ~b3RobotSimAPI();
bool connect(struct GUIHelperInterface* guiHelper);
void disconnect();
bool loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results);
int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo);
bool getJointStates(int bodyUniqueId, b3JointStates& state);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
void stepSimulation();
void setGravity(const b3Vector3& gravityAcceleration);
void setNumSimulationSubSteps(int numSubSteps);
void setNumSolverIterations(int numIterations);
bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results);
void renderScene();
void debugDraw(int debugDrawMode);
void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
void loadBunny(double scale, double mass, double collisionMargin);
};
#endif //B3_ROBOT_SIM_API_H

49
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -170,25 +170,6 @@ public:
double dt = double(dtMicro)/1000000.; double dt = double(dtMicro)/1000000.;
m_physicsServerExample->stepSimulation(dt); m_physicsServerExample->stepSimulation(dt);
#if 0
{
//if (btIsExampleBrowserMainThreadTerminated(m_data))
//{
// PhysicsClientSharedMemory::disconnectSharedMemory();
//}
}
//{
//unsigned long int ms = m_clock.getTimeMilliseconds();
//if (ms>20)
//{
// B3_PROFILE("m_clock.reset()");
//
// m_clock.reset();
//btUpdateInProcessExampleBrowserMainThread(m_data);
//}
}
#endif
{ {
b3Clock::usleep(0); b3Clock::usleep(0);
} }
@@ -206,16 +187,44 @@ public:
{ {
m_physicsServerExample->renderScene(); m_physicsServerExample->renderScene();
} }
virtual void debugDraw(int debugDrawMode)
{
m_physicsServerExample->physicsDebugDraw(debugDrawMode);
}
virtual bool mouseMoveCallback(float x, float y)
{
return m_physicsServerExample->mouseMoveCallback(x,y);
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return m_physicsServerExample->mouseButtonCallback(button,state,x,y);
}
}; };
void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
cl->debugDraw(debugDrawMode);
}
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle) void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle)
{ {
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle; InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
cl->renderScene(); cl->renderScene();
} }
int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle,float x,float y)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
return cl->mouseMoveCallback(x,y);
}
int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int button, int state, float x, float y)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
return cl->mouseButtonCallback(button,state, x,y);
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper) b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper)
{ {

8
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@@ -15,9 +15,13 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]); b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper); b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper);
///ignore the following APIs, they are for internal use for example browser
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle); void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);
void b3InProcessDebugDrawInternal(b3PhysicsClientHandle clientHandle, int debugDrawMode);
int b3InProcessMouseMoveCallback(b3PhysicsClientHandle clientHandle,float x,float y);
int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int button, int state, float x, float y);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

14
examples/Utils/b3Clock.cpp
View File

@@ -198,11 +198,13 @@ void b3Clock::usleep(int microSeconds)
Sleep(millis); Sleep(millis);
} }
#else #else
if (microSeconds>0)
::usleep(microSeconds); {
//struct timeval tv; ::usleep(microSeconds);
//tv.tv_sec = microSeconds/1000000L; //struct timeval tv;
//tv.tv_usec = microSeconds%1000000L; //tv.tv_sec = microSeconds/1000000L;
//return select(0, 0, 0, 0, &tv); //tv.tv_usec = microSeconds%1000000L;
//return select(0, 0, 0, 0, &tv);
}
#endif #endif
} }