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)
@@ -106,7 +146,7 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
{
key = portOrKey;
}
sm = b3ConnectSharedMemory(key);
sm = b3ConnectSharedMemory2(key);
break;
}
case eCONNECT_UDP:
@@ -1088,4 +1128,19 @@ void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string& profileN
b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
}
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();
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);
void disconnect();
@@ -231,6 +226,21 @@ public:
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

167
examples/RoboticsLearning/GripperGraspExample.cpp
View File

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

77
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -11,7 +11,7 @@
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "b3RobotSimAPI.h"
#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h"
///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;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
b3RobotSimulatorClientAPI m_robotSim;
int m_options;
int m_r2d2Index;
@@ -51,20 +51,22 @@ public:
}
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);
if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
{
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "r2d2.urdf";
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0,0,.5);
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
m_r2d2Index = m_robotSim.loadURDF("r2d2.urdf",args);
if (m_r2d2Index>=0)
{
m_r2d2Index = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
b3Printf("numJoints = %d",numJoints);
@@ -78,7 +80,7 @@ public:
int wheelTargetVelocities[4]={-10,-10,-10,-10};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
@@ -86,84 +88,67 @@ public:
}
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kiva_shelf/model.sdf";
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);
b3RobotSimulatorLoadFileResults results;
m_robotSim.loadSDF("kiva_shelf/model.sdf",results);
}
{
b3RobotSimLoadFileArgs args("");
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.loadURDF("results");
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimulatorLoadFileResults results;
{
args.m_fileName = "cube_soft.urdf";
args.m_startPosition.setValue(0,0,2.5);
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_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_startOrientation.setEulerZYX(0,0.2,0);
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.loadURDF("plane.urdf",args);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0)
{
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimulatorLoadFileResults results;
{
args.m_fileName = "sphere2_rolling_friction.urdf";
args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0,0);
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_startOrientation.setEulerZYX(0,0,0);
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_startOrientation.setEulerZYX(0,0.2,0);
args.m_useMultiBody = true;
args.m_forceOverrideFixedBase = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.loadURDF("plane.urdf", args);
}
}
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.;
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);
}
@@ -206,16 +187,44 @@ public:
{
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)
{
InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
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)
{

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 b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper);
///ignore the following APIs, they are for internal use for example browser
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
}
#endif

14
examples/Utils/b3Clock.cpp
View File

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