diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 10e28a062..79fddbd11 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -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); } \ No newline at end of file diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index 19330fa69..fadc8e79d 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -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 diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 60b4d3e4e..a01993c93 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -13,7 +13,7 @@ #include "../SharedMemory/SharedMemoryPublic.h" #include -#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) { diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 3f0537a61..abb3005d1 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -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) { diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index 064e70b67..7cae8046d 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -11,7 +11,7 @@ #include "../SharedMemory/PhysicsClientC_API.h" #include -#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)); + } } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp deleted file mode 100644 index f0e4224ac..000000000 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ /dev/null @@ -1,1063 +0,0 @@ -#include "b3RobotSimAPI.h" - -//#include "../CommonInterfaces/CommonGraphicsAppInterface.h" -#include "Bullet3Common/b3Quaternion.h" -#include "Bullet3Common/b3AlignedObjectArray.h" -#include "../CommonInterfaces/CommonRenderInterface.h" -//#include "../CommonInterfaces/CommonExampleInterface.h" -#include "../SharedMemory/PhysicsServerCommandProcessor.h" - -#include "../CommonInterfaces/CommonGUIHelperInterface.h" -#include "../SharedMemory/PhysicsServerSharedMemory.h" -#include "../SharedMemory/PhysicsServerSharedMemory.h" -#include "../SharedMemory/PhysicsClientC_API.h" -#include "../SharedMemory/PhysicsDirectC_API.h" -#include "../SharedMemory/PhysicsDirect.h" - -#include -#include "../Utils/b3Clock.h" - - -#include "../MultiThreading/b3ThreadSupportInterface.h" - - -void RobotThreadFunc(void* userPtr,void* lsMemory); -void* RobotlsMemoryFunc(); -#define MAX_ROBOT_NUM_THREADS 1 -enum - { - numCubesX = 20, - numCubesY = 20 - }; - - -enum TestRobotSimCommunicationEnums -{ - eRequestTerminateRobotSim= 13, - eRobotSimIsUnInitialized, - eRobotSimIsInitialized, - eRobotSimInitializationFailed, - eRobotSimHasTerminated -}; - -enum MultiThreadedGUIHelperCommunicationEnums -{ - eRobotSimGUIHelperIdle= 13, - eRobotSimGUIHelperRegisterTexture, - eRobotSimGUIHelperRegisterGraphicsShape, - eRobotSimGUIHelperRegisterGraphicsInstance, - eRobotSimGUIHelperCreateCollisionShapeGraphicsObject, - eRobotSimGUIHelperCreateCollisionObjectGraphicsObject, - eRobotSimGUIHelperRemoveAllGraphicsInstances, - eRobotSimGUIHelperCopyCameraImageData, - eRobotSimGUIHelperRemoveGraphicsInstance, -}; - -#include -//#include "BulletMultiThreaded/PlatformDefinitions.h" - -#ifndef _WIN32 -#include "../MultiThreading/b3PosixThreadSupport.h" - -b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads) -{ - b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("RobotSimThreads", - RobotThreadFunc, - RobotlsMemoryFunc, - numThreads); - b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo); - - return threadSupport; - -} - - -#elif defined( _WIN32) -#include "../MultiThreading/b3Win32ThreadSupport.h" - -b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads) -{ - b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("RobotSimThreads",RobotThreadFunc,RobotlsMemoryFunc,numThreads); - b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo); - return threadSupport; - -} -#endif - - - -struct RobotSimArgs -{ - RobotSimArgs() - :m_physicsServerPtr(0) - { - } - b3CriticalSection* m_cs; - - PhysicsServerSharedMemory* m_physicsServerPtr; - b3AlignedObjectArray m_positions; -}; - -struct RobotSimThreadLocalStorage -{ - int threadId; -}; - - -void RobotThreadFunc(void* userPtr,void* lsMemory) -{ - printf("RobotThreadFunc thread started\n"); -// RobotSimThreadLocalStorage* localStorage = (RobotSimThreadLocalStorage*) lsMemory; - - RobotSimArgs* args = (RobotSimArgs*) userPtr; -// int workLeft = true; - b3Clock clock; - clock.reset(); - bool init = true; - if (init) - { - - args->m_cs->lock(); - args->m_cs->setSharedParam(0,eRobotSimIsInitialized); - args->m_cs->unlock(); - - do - { -//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread? -#if 0 - double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.; - if (deltaTimeInSeconds<(1./260.)) - { - if (deltaTimeInSeconds<.001) - continue; - } - - clock.reset(); -#endif // - args->m_physicsServerPtr->processClientCommands(); - - } while (args->m_cs->getSharedParam(0)!=eRequestTerminateRobotSim); - } else - { - args->m_cs->lock(); - args->m_cs->setSharedParam(0,eRobotSimInitializationFailed); - args->m_cs->unlock(); - } - //do nothing -} - - - -void* RobotlsMemoryFunc() -{ - //don't create local store memory, just return 0 - return new RobotSimThreadLocalStorage; -} - - - -ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper2 : public GUIHelperInterface -{ -// CommonGraphicsApp* m_app; - - b3CriticalSection* m_cs; - - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - GUIHelperInterface* m_childGuiHelper; - - const unsigned char* m_texels; - int m_textureWidth; - int m_textureHeight; - - - int m_shapeIndex; - const float* m_position; - const float* m_quaternion; - const float* m_color; - const float* m_scaling; - - const float* m_vertices; - int m_numvertices; - const int* m_indices; - int m_numIndices; - int m_primitiveType; - int m_textureId; - int m_instanceId; - - - MultiThreadedOpenGLGuiHelper2( GUIHelperInterface* guiHelper) - : m_cs(0), - m_texels(0), - m_textureId(-1) - { - m_childGuiHelper = guiHelper;; - - } - - virtual ~MultiThreadedOpenGLGuiHelper2() - { - delete m_childGuiHelper; - } - - void setCriticalSection(b3CriticalSection* cs) - { - m_cs = cs; - } - - b3CriticalSection* getCriticalSection() - { - return m_cs; - } - - virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color) - { - createCollisionObjectGraphicsObject((btCollisionObject*)body, color); - } - - btCollisionObject* m_obj; - btVector3 m_color2; - - virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color) - { - m_obj = obj; - m_color2 = color; - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionObjectGraphicsObject); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - - } - - btCollisionShape* m_colShape; - virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape) - { - m_colShape = collisionShape; - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionShapeGraphicsObject); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - - } - - virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld) - { - //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects. - //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms - if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0) - { - m_childGuiHelper->syncPhysicsToGraphics(rbWorld); - } - } - - virtual void render(const btDiscreteDynamicsWorld* rbWorld) - { - m_childGuiHelper->render(0); - } - - virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){} - - virtual int registerTexture(const unsigned char* texels, int width, int height) - { - m_texels = texels; - m_textureWidth = width; - m_textureHeight = height; - - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterTexture); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - return m_textureId; - } - virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) - { - m_vertices = vertices; - m_numvertices = numvertices; - m_indices = indices; - m_numIndices = numIndices; - m_primitiveType = primitiveType; - m_textureId = textureId; - - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsShape); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - return m_shapeIndex; - } - virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) - { - m_shapeIndex = shapeIndex; - m_position = position; - m_quaternion = quaternion; - m_color = color; - m_scaling = scaling; - - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsInstance); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - return m_instanceId; - } - - virtual void removeAllGraphicsInstances() - { - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveAllGraphicsInstances); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - } - int m_graphicsInstanceRemove; - virtual void removeGraphicsInstance(int instanceUid) - { - m_cs->lock(); - m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveGraphicsInstance); - m_graphicsInstanceRemove = instanceUid; - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - } - - virtual Common2dCanvasInterface* get2dCanvasInterface() - { - return 0; - } - - virtual CommonParameterInterface* getParameterInterface() - { - return 0; - } - - virtual CommonRenderInterface* getRenderInterface() - { - return 0; - } - - virtual CommonGraphicsApp* getAppInterface() - { - return m_childGuiHelper->getAppInterface(); - } - - - virtual void setUpAxis(int axis) - { - m_childGuiHelper->setUpAxis(axis); - } - virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ) - { - } - - float m_viewMatrix[16]; - float m_projectionMatrix[16]; - unsigned char* m_pixelsRGBA; - int m_rgbaBufferSizeInPixels; - float* m_depthBuffer; - int m_depthBufferSizeInPixels; - int* m_segmentationMaskBuffer; - int m_segmentationMaskBufferSizeInPixels; - int m_startPixelIndex; - int m_destinationWidth; - int m_destinationHeight; - int* m_numPixelsCopied; - - virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], - unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, - float* depthBuffer, int depthBufferSizeInPixels, - int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, - int startPixelIndex, int destinationWidth, - int destinationHeight, int* numPixelsCopied) - { - m_cs->lock(); - for (int i=0;i<16;i++) - { - m_viewMatrix[i] = viewMatrix[i]; - m_projectionMatrix[i] = projectionMatrix[i]; - } - m_pixelsRGBA = pixelsRGBA; - m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels; - m_depthBuffer = depthBuffer; - m_depthBufferSizeInPixels = depthBufferSizeInPixels; - m_segmentationMaskBuffer = segmentationMaskBuffer; - m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels; - m_startPixelIndex = startPixelIndex; - m_destinationWidth = destinationWidth; - m_destinationHeight = destinationHeight; - m_numPixelsCopied = numPixelsCopied; - - m_cs->setSharedParam(1,eRobotSimGUIHelperCopyCameraImageData); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle) - { - } - } - - virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) - { - } - - virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) - { - } - - virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double textColorRGB[3], double size, double lifeTime) - { - return -1; - } - virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) - { - return -1; - } - virtual void removeUserDebugItem( int debugItemUniqueId) - { - } - virtual void removeAllUserDebugItems( ) - { - } -}; - - - - - -struct b3RobotSimAPI_InternalData -{ - //GUIHelperInterface* m_guiHelper; - PhysicsServerSharedMemory m_physicsServer; - b3PhysicsClientHandle m_physicsClient; - - - b3ThreadSupportInterface* m_threadSupport; - RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; - MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper; - PhysicsDirect* m_clientServerDirect; - - bool m_useMultiThreading; - bool m_connected; - - b3RobotSimAPI_InternalData() - : - m_physicsClient(0), - m_threadSupport(0), - m_multiThreadedHelper(0), - - m_clientServerDirect(0), - m_useMultiThreading(false), - m_connected(false) - { - } -}; - -b3RobotSimAPI::b3RobotSimAPI() -{ - m_data = new b3RobotSimAPI_InternalData; -} - -void b3RobotSimAPI::stepSimulation() -{ - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClient)); - if (b3CanSubmitCommand(m_data->m_physicsClient)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitStepSimulationCommand(m_data->m_physicsClient)); - statusType = b3GetStatusType(statusHandle); - b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); - } -} - -void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration) -{ - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); - -} - -void b3RobotSimAPI::setNumSolverIterations(int numIterations) -{ - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSolverIterations(command, numIterations); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); - -} - -void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps) -{ - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSubSteps(command, numSubSteps); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); -} - -/* -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, - const double* jointPositionsQ, double targetPosition[3]); - -int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, - int* bodyUniqueId, - int* dofCount, - double* jointPositions); -*/ -bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results) -{ - btAssert(args.m_endEffectorLinkIndex>=0); - btAssert(args.m_bodyUniqueId>=0); - - - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); - if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) - { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) - { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); - } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) - { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else - { - b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); - } - - if (args.m_flags & B3_HAS_JOINT_DAMPING) - { - b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); - } - - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - - - int numPos=0; - - bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - 0); - if (result && numPos) - { - results.m_calculatedJointPositions.resize(numPos); - result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - &results.m_calculatedJointPositions[0]); - - } - return result; -} - - -b3RobotSimAPI::~b3RobotSimAPI() -{ - delete m_data; -} - -void b3RobotSimAPI::processMultiThreadedGraphicsRequests() -{ - if (0==m_data->m_multiThreadedHelper) - return; - - switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) - { - case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_data->m_multiThreadedHelper->m_colShape); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - - break; - } - case eRobotSimGUIHelperCreateCollisionObjectGraphicsObject: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_data->m_multiThreadedHelper->m_obj, - m_data->m_multiThreadedHelper->m_color2); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRegisterTexture: - { - - m_data->m_multiThreadedHelper->m_textureId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_data->m_multiThreadedHelper->m_texels, - m_data->m_multiThreadedHelper->m_textureWidth,m_data->m_multiThreadedHelper->m_textureHeight); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - - break; - } - case eRobotSimGUIHelperRegisterGraphicsShape: - { - m_data->m_multiThreadedHelper->m_shapeIndex = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape( - m_data->m_multiThreadedHelper->m_vertices, - m_data->m_multiThreadedHelper->m_numvertices, - m_data->m_multiThreadedHelper->m_indices, - m_data->m_multiThreadedHelper->m_numIndices, - m_data->m_multiThreadedHelper->m_primitiveType, - m_data->m_multiThreadedHelper->m_textureId); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRegisterGraphicsInstance: - { - m_data->m_multiThreadedHelper->m_instanceId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance( - m_data->m_multiThreadedHelper->m_shapeIndex, - m_data->m_multiThreadedHelper->m_position, - m_data->m_multiThreadedHelper->m_quaternion, - m_data->m_multiThreadedHelper->m_color, - m_data->m_multiThreadedHelper->m_scaling); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRemoveAllGraphicsInstances: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); - int numRenderInstances; - numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); - b3Assert(numRenderInstances==0); - - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperRemoveGraphicsInstance: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->removeGraphicsInstance(m_data->m_multiThreadedHelper->m_graphicsInstanceRemove); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eRobotSimGUIHelperCopyCameraImageData: - { - m_data->m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_data->m_multiThreadedHelper->m_viewMatrix, - m_data->m_multiThreadedHelper->m_projectionMatrix, - m_data->m_multiThreadedHelper->m_pixelsRGBA, - m_data->m_multiThreadedHelper->m_rgbaBufferSizeInPixels, - m_data->m_multiThreadedHelper->m_depthBuffer, - m_data->m_multiThreadedHelper->m_depthBufferSizeInPixels, - m_data->m_multiThreadedHelper->m_segmentationMaskBuffer, - m_data->m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels, - m_data->m_multiThreadedHelper->m_startPixelIndex, - m_data->m_multiThreadedHelper->m_destinationWidth, - m_data->m_multiThreadedHelper->m_destinationHeight, - m_data->m_multiThreadedHelper->m_numPixelsCopied); - m_data->m_multiThreadedHelper->getCriticalSection()->lock(); - m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->getCriticalSection()->unlock(); - - break; - } - case eRobotSimGUIHelperIdle: - default: - { - - } - } - -} - -b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle) -{ - int timeout = 1024*1024*1024; - b3SharedMemoryStatusHandle statusHandle=0; - - b3SubmitClientCommand(physClient,commandHandle); - - while ((statusHandle==0) && (timeout-- > 0)) - { - statusHandle =b3ProcessServerStatus(physClient); - processMultiThreadedGraphicsRequests(); - } - return (b3SharedMemoryStatusHandle) statusHandle; -} - -int b3RobotSimAPI::getNumJoints(int bodyUniqueId) const -{ - return b3GetNumJoints(m_data->m_physicsClient,bodyUniqueId); -} - -bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) -{ - return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0); -} - -void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) -{ - b3SharedMemoryStatusHandle statusHandle; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClient)); - if (b3CanSubmitCommand(m_data->m_physicsClient)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitCreateUserConstraintCommand(m_data->m_physicsClient, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); - } -} - - -bool b3RobotSimAPI::getJointStates(int bodyUniqueId, b3JointStates& state) -{ - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - if (statusHandle) - { - // double rootInertialFrame[7]; - const double* rootLocalInertialFrame; - const double* actualStateQ; - const double* actualStateQdot; - const double* jointReactionForces; - - int stat = b3GetStatusActualState(statusHandle, - &state.m_bodyUniqueId, - &state.m_numDegreeOfFreedomQ, - &state.m_numDegreeOfFreedomU, - &rootLocalInertialFrame, - &actualStateQ, - &actualStateQdot, - &jointReactionForces); - if (stat) - { - state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ); - state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU); - - for (int i=0;im_physicsClient, bodyUniqueId, CONTROL_MODE_VELOCITY); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - break; - } - case CONTROL_MODE_POSITION_VELOCITY_PD: - { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - int qIndex = jointInfo.m_qIndex; - - b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); - b3JointControlSetKp(command,uIndex,args.m_kp); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - break; - } - case CONTROL_MODE_TORQUE: - { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClient, bodyUniqueId, CONTROL_MODE_TORQUE); - b3JointInfo jointInfo; - b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); - int uIndex = jointInfo.m_uIndex; - b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - break; - } - default: - { - b3Error("Unknown control command in b3RobotSimAPI::setJointMotorControl"); - } - } -} - -bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotSimLoadFileResults& results) -{ - bool statusOk = false; - - int robotUniqueId = -1; - b3Assert(m_data->m_connected); - switch (args.m_fileType) - { - case B3_URDF_FILE: - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str()); - - //setting the initial position, orientation and other arguments are optional - - b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], - args.m_startPosition[1], - args.m_startPosition[2]); - b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] - ,args.m_startOrientation[1] - ,args.m_startOrientation[2] - ,args.m_startOrientation[3]); - if (args.m_forceOverrideFixedBase) - { - b3LoadUrdfCommandSetUseFixedBase(command,true); - } - b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); - statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); - statusType = b3GetStatusType(statusHandle); - - b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); - robotUniqueId = b3GetStatusBodyIndex(statusHandle); - results.m_uniqueObjectIds.push_back(robotUniqueId); - statusOk = true; - break; - } - case B3_SDF_FILE: - { - b3SharedMemoryStatusHandle statusHandle; - int statusType; - b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClient, args.m_fileName.c_str()); - b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); - statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); - statusType = b3GetStatusType(statusHandle); - b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); - if (statusType == CMD_SDF_LOADING_COMPLETED) - { - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); - if (numBodies) - { - results.m_uniqueObjectIds.resize(numBodies); - int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); - - } - statusOk = true; - } - - break; - } - default: - { - b3Warning("Unknown file type in b3RobotSimAPI::loadFile"); - } - - } - - return statusOk; -} - -bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) -{ - if (m_data->m_useMultiThreading) - { - m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper); - - - m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); - - for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++) - { - RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i); - b3Assert(storage); - storage->threadId = i; - //storage->m_sharedMem = data->m_sharedMem; - } - - - - - for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++) - { - m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection(); - m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized); - int numMoving = 0; - m_data->m_args[w].m_positions.resize(numMoving); - m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer; - //int index = 0; - - m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w); - while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized) - { - } - } - - m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle); - m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs); - - bool serverConnected; - serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper); - b3Assert(serverConnected); - - m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); - } - else - { - PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor; - m_data->m_clientServerDirect = new PhysicsDirect(sdk,true); - bool connected; - connected = m_data->m_clientServerDirect->connect(guiHelper); - m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect; - - } - - //client connected - m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient); - - b3Assert(m_data->m_connected); - return m_data->m_connected && m_data->m_connected; -} - -void b3RobotSimAPI::disconnect() -{ - if (m_data->m_useMultiThreading) - { - for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++) - { - m_data->m_args[i].m_cs->lock(); - m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim); - m_data->m_args[i].m_cs->unlock(); - } - int numActiveThreads = MAX_ROBOT_NUM_THREADS; - - while (numActiveThreads) - { - int arg0, arg1; - if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0)) - { - numActiveThreads--; - printf("numActiveThreads = %d\n", numActiveThreads); - - } - else - { - } - }; - - printf("stopping threads\n"); - - delete m_data->m_threadSupport; - m_data->m_threadSupport = 0; - } - b3DisconnectSharedMemory(m_data->m_physicsClient); - m_data->m_physicsServer.disconnectSharedMemory(true); - - m_data->m_connected = false; -} - - -void b3RobotSimAPI::debugDraw(int debugDrawMode) -{ - if (m_data->m_clientServerDirect) - { - m_data->m_clientServerDirect->debugDraw(debugDrawMode); - } -} -void b3RobotSimAPI::renderScene() -{ - if (m_data->m_useMultiThreading) - { - if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) - { - m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); - } - } - - - if (m_data->m_clientServerDirect) - { - m_data->m_clientServerDirect->renderScene(); - } - - m_data->m_physicsServer.renderScene(); -} - -void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) -{ - b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClient, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) - { - b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); - } -} - -void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation) -{ - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); - - if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - b3LinkState linkState; - b3GetLinkState(m_data->m_physicsClient, statusHandle, linkIndex, &linkState); - worldPosition[0] = linkState.m_worldPosition[0]; - worldPosition[1] = linkState.m_worldPosition[1]; - worldPosition[2] = linkState.m_worldPosition[2]; - worldOrientation[0] = linkState.m_worldOrientation[0]; - worldOrientation[1] = linkState.m_worldOrientation[1]; - worldOrientation[2] = linkState.m_worldOrientation[2]; - worldOrientation[3] = linkState.m_worldOrientation[3]; - } -} - -void b3RobotSimAPI::loadBunny(double scale, double mass, double collisionMargin) -{ - b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient); - b3LoadBunnySetScale(command, scale); - b3LoadBunnySetMass(command, mass); - b3LoadBunnySetCollisionMargin(command, collisionMargin); - b3SubmitClientCommand(m_data->m_physicsClient, command); -} diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h deleted file mode 100644 index 7e196e1b6..000000000 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ /dev/null @@ -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 - -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 m_actualStateQ; - b3AlignedObjectArray m_actualStateQdot; - b3AlignedObjectArray 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 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 m_lowerLimits; - b3AlignedObjectArray m_upperLimits; - b3AlignedObjectArray m_jointRanges; - b3AlignedObjectArray m_restPoses; - b3AlignedObjectArray 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 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 diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 09018e496..70fd1c5fb 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -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) { diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h index 625fb0f2f..6d1bacc75 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h @@ -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 diff --git a/examples/Utils/b3Clock.cpp b/examples/Utils/b3Clock.cpp index 82014e630..bcf7d13ba 100644 --- a/examples/Utils/b3Clock.cpp +++ b/examples/Utils/b3Clock.cpp @@ -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 }