diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 5e4eefa74..10e28a062 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -2,29 +2,32 @@ //#include "SharedMemoryCommands.h" -#include "SharedMemory/PhysicsClientC_API.h" +#include "../SharedMemory/PhysicsClientC_API.h" #ifdef BT_ENABLE_ENET -#include "SharedMemory/PhysicsClientUDP_C_API.h" +#include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //PHYSICS_UDP #ifdef BT_ENABLE_CLSOCKET -#include "SharedMemory/PhysicsClientTCP_C_API.h" +#include "../SharedMemory/PhysicsClientTCP_C_API.h" #endif //PHYSICS_TCP -#include "SharedMemory/PhysicsDirectC_API.h" +#include "../SharedMemory/PhysicsDirectC_API.h" -#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h" +#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" -#include "SharedMemory/SharedMemoryPublic.h" + +#include "../SharedMemory/SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" struct b3RobotSimulatorClientAPI_InternalData { b3PhysicsClientHandle m_physicsClientHandle; + struct GUIHelperInterface* m_guiHelper; b3RobotSimulatorClientAPI_InternalData() - : m_physicsClientHandle(0) + : m_physicsClientHandle(0), + m_guiHelper(0) { } }; @@ -39,6 +42,26 @@ b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() delete m_data; } +void b3RobotSimulatorClientAPI::setGuiHelper(struct GUIHelperInterface* guiHelper) +{ + m_data->m_guiHelper = guiHelper; +} + +void b3RobotSimulatorClientAPI::renderScene() +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return; + } + if (m_data->m_guiHelper) + { + b3InProcessRenderSceneInternal(m_data->m_physicsClientHandle); + } +} + + + bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey) { if (m_data->m_physicsClientHandle) @@ -55,6 +78,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i switch (mode) { + case eCONNECT_EXISTING_EXAMPLE_BROWSER: + { + sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper); + break; + } + case eCONNECT_GUI: { int argc = 0; @@ -644,8 +673,72 @@ bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, return false; } +bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& state) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, 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;i m_calculatedJointPositions; }; +struct b3JointStates2 +{ + int m_bodyUniqueId; + int m_numDegreeOfFreedomQ; + int m_numDegreeOfFreedomU; + b3Transform m_rootLocalInertialFrame; + b3AlignedObjectArray m_actualStateQ; + b3AlignedObjectArray m_actualStateQdot; + b3AlignedObjectArray m_jointReactionForces; +}; + ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet ///as documented in the pybullet Quickstart Guide ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA @@ -137,6 +148,11 @@ 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(); @@ -177,6 +193,8 @@ public: bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); + bool getJointStates(int bodyUniqueId, b3JointStates2& state); + bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index b116c0faa..3f0537a61 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -11,8 +11,8 @@ #include "../SharedMemory/PhysicsServerSharedMemory.h" #include "../SharedMemory/PhysicsClientC_API.h" #include +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" -#include "b3RobotSimAPI.h" #include "../Utils/b3Clock.h" ///quick demo showing the right-handed coordinate system and positive rotations around each axis @@ -20,7 +20,8 @@ class KukaGraspExample : public CommonExampleInterface { CommonGraphicsApp* m_app; GUIHelperInterface* m_guiHelper; - b3RobotSimAPI m_robotSim; + b3RobotSimulatorClientAPI m_robotSim; + int m_kukaIndex; IKTrajectoryHelper m_ikHelper; @@ -78,19 +79,18 @@ public: - - bool connected = m_robotSim.connect(m_guiHelper); + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + +// 0;//m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d",connected); { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "kuka_iiwa/model.urdf"; - args.m_startPosition.setValue(0,0,0); - b3RobotSimLoadFileResults results; - if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf"); + if (m_kukaIndex >=0) { - m_kukaIndex = results.m_uniqueObjectIds[0]; int numJoints = m_robotSim.getNumJoints(m_kukaIndex); b3Printf("numJoints = %d",numJoints); @@ -112,24 +112,9 @@ public: } */ } - - if (0) - { - 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); - } + { - 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.loadURDF("plane.urdf"); m_robotSim.setGravity(b3MakeVector3(0,0,0)); } @@ -157,9 +142,7 @@ public: { double q_current[7]={0,0,0,0,0,0,0}; - double world_position[3]={0,0,0}; - double world_orientation[4]={0,0,0,0}; - b3JointStates jointStates; + b3JointStates2 jointStates; if (m_robotSim.getJointStates(m_kukaIndex,jointStates)) { @@ -171,10 +154,13 @@ public: } } // compute body position and orientation - m_robotSim.getLinkState(0, 6, world_position, world_orientation); - m_worldPos.setValue(world_position[0], world_position[1], world_position[2]); - m_worldOri.setValue(world_orientation[0], world_orientation[1], world_orientation[2], world_orientation[3]); - + b3LinkState linkState; + m_robotSim.getLinkState(0, 6, &linkState); + + m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]); + m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]); + + b3Vector3DoubleData targetPosDataOut; m_targetPos.serializeDouble(targetPosDataOut); b3Vector3DoubleData worldPosDataOut; @@ -185,8 +171,8 @@ public: m_worldOri.serializeDouble(worldOriDataOut); - b3RobotSimInverseKinematicArgs ikargs; - b3RobotSimInverseKinematicsResults ikresults; + b3RobotSimulatorInverseKinematicArgs ikargs; + b3RobotSimulatorInverseKinematicsResults ikresults; ikargs.m_bodyUniqueId = m_kukaIndex; @@ -247,7 +233,7 @@ public: //copy the IK result to the desired state of the motor/actuator for (int i=0;iinitPhysics(); + m_physicsServerExample ->resetCamera(); + setSharedMemoryInterface(m_sharedMem); + m_clock.reset(); + m_prevTime = m_clock.getTimeMicroseconds(); + + } + virtual ~InProcessPhysicsClientExistingExampleBrowser() + { + m_physicsServerExample->exitPhysics(); + //s_instancingRenderer->removeAllInstances(); + delete m_physicsServerExample; + delete m_sharedMem; + } + + // return non-null if there is a status, nullptr otherwise + virtual const struct SharedMemoryStatus* processServerStatus() + { + //m_physicsServerExample->updateGraphics(); + + unsigned long long int curTime = m_clock.getTimeMicroseconds(); + unsigned long long int dtMicro = curTime - m_prevTime; + m_prevTime = curTime; + + 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); + } + const SharedMemoryStatus* stat = 0; + + { + stat = PhysicsClientSharedMemory::processServerStatus(); + } + + return stat; + + } + + virtual void renderScene() + { + m_physicsServerExample->renderScene(); + } + + +}; + +void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle) +{ + InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle; + cl->renderScene(); +} + +b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper) +{ + + InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper); + //InProcessPhysicsClientFromGuiHelper* cl = new InProcessPhysicsClientFromGuiHelper(guiHelper); + cl->connect(); + return (b3PhysicsClientHandle ) cl; +} \ No newline at end of file diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h index 7e68a64f8..625fb0f2f 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h @@ -14,8 +14,9 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]); +b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper); - +void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle); #ifdef __cplusplus } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index eccda8cfc..f451e7a79 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -503,6 +503,7 @@ enum eCONNECT_METHOD { eCONNECT_SHARED_MEMORY = 3, eCONNECT_UDP = 4, eCONNECT_TCP = 5, + eCONNECT_EXISTING_EXAMPLE_BROWSER=6, }; enum eURDF_Flags