diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index 3ce9ee90a4..0e1683360d 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -9,6 +9,8 @@ INCLUDE_DIRECTORIES( ) SET(BulletRobotics_SRCS + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index d90c79822f..2677f2fe33 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -145,6 +145,8 @@ SET(BulletExampleBrowser_SRCS ../TinyRenderer/tgaimage.cpp ../TinyRenderer/our_gl.cpp ../TinyRenderer/TinyRenderer.cpp + ../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp + ../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h ../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp ../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp ../SharedMemory/IKTrajectoryHelper.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 2af26677dd..1e5eea67a9 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -117,6 +117,8 @@ project "App_BulletExampleBrowser" "../SharedMemory/b3PluginManager.cpp", "../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp", "../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", + "../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", + "../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../SharedMemory/SharedMemoryCommands.h", "../SharedMemory/SharedMemoryPublic.h", "../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index f0fc384871..6eee154186 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -595,7 +595,7 @@ void ConvertURDF2BulletInternal( } } else { - if (cache.m_bulletMultiBody->getBaseMass()==0) + if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumLinks()==0) { //col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT); col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT); diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index cfa1e426bb..591cfea400 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -1,6 +1,9 @@ myfiles = { + + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 0d0ea65e71..2d126396d3 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -1,5 +1,9 @@ SET(SharedMemory_SRCS + plugins/pdControlPlugin/pdControlPlugin.cpp + plugins/pdControlPlugin/pdControlPlugin.h + b3RobotSimulatorClientAPI_NoDirect.cpp + b3RobotSimulatorClientAPI_NoDirect.h IKTrajectoryHelper.cpp IKTrajectoryHelper.h PhysicsClient.cpp diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c0f6885fc7..ccc4f391f3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -45,9 +45,9 @@ #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" -#ifdef STATIC_PD_CONTROL_PLUGIN +#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN #include "plugins/pdControlPlugin/pdControlPlugin.h" -#endif //STATIC_PD_CONTROL_PLUGIN +#endif //SKIP_STATIC_PD_CONTROL_PLUGIN #ifdef STATIC_LINK_VR_PLUGIN #include "plugins/vrSyncPlugin/vrSyncPlugin.h" @@ -105,7 +105,6 @@ struct UrdfLinkNameMapUtil } virtual ~UrdfLinkNameMapUtil() { - delete m_memSerializer; } }; @@ -1599,7 +1598,7 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_worldImporters; - btAlignedObjectArray m_urdfLinkNameMapper; + btAlignedObjectArray m_strings; btAlignedObjectArray m_collisionShapes; @@ -1692,11 +1691,11 @@ struct PhysicsServerCommandProcessorInternalData m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin, preTickPluginCallback_vrSyncPlugin, 0, 0); #endif //STATIC_LINK_VR_PLUGIN -#ifdef STATIC_PD_CONTROL_PLUGIN +#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN { - m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, postTickPluginCallback_pdControlPlugin, 0); + m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0); } -#endif //STATIC_PD_CONTROL_PLUGIN +#endif //SKIP_STATIC_PD_CONTROL_PLUGIN #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN @@ -2482,12 +2481,13 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() } m_data->m_worldImporters.clear(); +#ifdef ENABLE_LINK_MAPPER for (int i=0;im_urdfLinkNameMapper.size();i++) { delete m_data->m_urdfLinkNameMapper[i]; } m_data->m_urdfLinkNameMapper.clear(); - +#endif //ENABLE_LINK_MAPPER for (int i=0;im_strings.size();i++) { @@ -3010,10 +3010,12 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* btMultiBody* mb = bodyHandle? bodyHandle->m_multiBody:0; if (mb) { - UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; - m_data->m_urdfLinkNameMapper.push_back(util); + UrdfLinkNameMapUtil utilBla; + UrdfLinkNameMapUtil* util = &utilBla; + btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + util->m_mb = mb; - util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + util->m_memSerializer = &ser; util->m_memSerializer->startSerialization(); //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); @@ -3046,9 +3048,11 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* btRigidBody* rb = bodyHandle? bodyHandle->m_rigidBody :0; if (rb) { - UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; - m_data->m_urdfLinkNameMapper.push_back(util); - util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + UrdfLinkNameMapUtil utilBla; + UrdfLinkNameMapUtil* util = &utilBla; + btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient); + + util->m_memSerializer = &ser; util->m_memSerializer->startSerialization(); util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody,bodyHandle->m_bodyName.c_str()); //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); @@ -6049,10 +6053,8 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); - } + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); @@ -6120,12 +6122,14 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; - +#ifdef ENABLE_LINK_MAPPER if (m_data->m_urdfLinkNameMapper.size()) { serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } +#endif serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 65ffcb9a49..52433e6fb8 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -1,6 +1,5 @@ #include "b3RobotSimulatorClientAPI_NoDirect.h" - #include "PhysicsClientC_API.h" #include "b3RobotSimulatorClientAPI_InternalData.h" @@ -35,71 +34,7 @@ b3RobotSimulatorClientAPI_NoDirect::~b3RobotSimulatorClientAPI_NoDirect() } -bool b3RobotSimulatorClientAPI_NoDirect::connect(int mode, const std::string& hostName, int portOrKey) -{ - if (m_data->m_physicsClientHandle) - { - b3Warning("Already connected, disconnect first."); - return false; - } - b3PhysicsClientHandle sm = 0; - int udpPort = 1234; - int tcpPort = 6667; - int key = SHARED_MEMORY_KEY; - bool connected = false; - - switch (mode) - { - - case eCONNECT_DIRECT: - { - break; - } - case eCONNECT_SHARED_MEMORY: - { - if (portOrKey >= 0) - { - key = portOrKey; - } - sm = b3ConnectSharedMemory(key); - break; - } - case eCONNECT_UDP: - { - if (portOrKey >= 0) - { - udpPort = portOrKey; - } - - break; - } - case eCONNECT_TCP: - { - if (portOrKey >= 0) - { - tcpPort = portOrKey; - } - break; - } - - default: - { - b3Warning("connectPhysicsServer unexpected argument"); - } - }; - if (sm) - { - m_data->m_physicsClientHandle = sm; - if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - disconnect(); - return false; - } - return true; - } - return false; -} bool b3RobotSimulatorClientAPI_NoDirect::isConnected() const { diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 62b7a02725..4a055d56c4 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -419,7 +419,8 @@ class b3RobotSimulatorClientAPI_NoDirect b3RobotSimulatorClientAPI_NoDirect(); virtual ~b3RobotSimulatorClientAPI_NoDirect(); - bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + //No 'connect', use setInternalData to bypass the connect method, pass an existing client + virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data); void disconnect(); @@ -580,7 +581,6 @@ class b3RobotSimulatorClientAPI_NoDirect bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo); - virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data); }; diff --git a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp index 7fd1c0913b..285b3e40e3 100644 --- a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp +++ b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp @@ -4,6 +4,7 @@ #include "../b3PluginContext.h" #include + #include "LinearMath/btScalar.h" #include "LinearMath/btAlignedObjectArray.h" @@ -52,31 +53,51 @@ B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* { //apply pd control here, apply forces using the PD gains MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer; + for (int i = 0; i < obj->m_controllers.size(); i++) { const MyPDControl& pdControl = obj->m_controllers[i]; - //compute torque - //apply torque + + int dof1 = 0; + b3JointSensorState actualState; + if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState)) + { + if (pdControl.m_maxForce>0) + { + //compute torque + btScalar qActual = actualState.m_jointPosition; + btScalar qdActual = actualState.m_jointVelocity; + + btScalar positionError = (pdControl.m_desiredPosition -qActual); + double desiredVelocity = 0; + btScalar velocityError = (pdControl.m_desiredVelocity-qdActual); + + btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError; + + btClamp(force,-pdControl.m_maxForce,pdControl.m_maxForce); + + //apply torque + b3RobotSimulatorJointMotorArgs args(CONTROL_MODE_TORQUE); + args.m_maxTorqueValue = force; + obj->m_api.setJointMotorControl(pdControl.m_objectUniqueId, pdControl.m_linkIndex, args); + } + } + } - //for each registered pd controller, execute PD control return 0; } -B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context) -{ - MyPDControlContainer* obj = (MyPDControlContainer* )context->m_userPointer; - obj->m_testData++; - return 0; -} B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) { MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer; - + + obj->m_api.syncBodies(); + int numObj = obj->m_api.getNumBodies(); - printf("numObj = %d\n", numObj); + //printf("numObj = %d\n", numObj); //protocol: //first int is the type of command @@ -89,20 +110,6 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c switch (arguments->m_ints[0]) { - case eAddPDControl: - { - if (arguments->m_numFloats < 5) - return -1; - MyPDControl controller; - controller.m_desiredPosition = arguments->m_floats[0]; - controller.m_desiredVelocity = arguments->m_floats[1]; - controller.m_kd = arguments->m_floats[2]; - controller.m_kp = arguments->m_floats[3]; - controller.m_maxForce = arguments->m_floats[4]; - controller.m_objectUniqueId = arguments->m_ints[1]; - controller.m_linkIndex = arguments->m_ints[2]; - obj->m_controllers.push_back(controller); - } case eSetPDControl: { if (arguments->m_numFloats < 5) @@ -116,14 +123,19 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c controller.m_objectUniqueId = arguments->m_ints[1]; controller.m_linkIndex = arguments->m_ints[2]; + int foundIndex = -1; for (int i = 0; i < obj->m_controllers.size(); i++) { if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex) { obj->m_controllers[i] = controller; - break; + foundIndex=i; } } + if (foundIndex<0) + { + obj->m_controllers.push_back(controller); + } break; } case eRemovePDControl: diff --git a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h index 591db08731..6760aaa38d 100644 --- a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h +++ b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h @@ -16,14 +16,12 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c /// enum PDControlCommandEnum { - eAddPDControl = 1, - eSetPDControl = 2, - eRemovePDControl = 4, + eSetPDControl = 1, + eRemovePDControl = 2, }; //all the APIs below are optional B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context); -B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context); #ifdef __cplusplus diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index 47f8be8602..be66e7e42a 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -20,6 +20,8 @@ language "C++" myfiles = { + "b3RobotSimulatorClientAPI_NoDirect.cpp", + "b3RobotSimulatorClientAPI_NoDirect.h", "IKTrajectoryHelper.cpp", "IKTrajectoryHelper.h", "PhysicsClient.cpp", @@ -58,6 +60,8 @@ myfiles = "PhysicsServerCommandProcessor.h", "b3PluginManager.cpp", "b3PluginManager.h", + "plugins/pdControlPlugin/pdControlPlugin.cpp", + "plugins/pdControlPlugin/pdControlPlugin.h", "../OpenGLWindow/SimpleCamera.cpp", "../OpenGLWindow/SimpleCamera.h", "../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h", diff --git a/examples/SharedMemory/tcp/premake4.lua b/examples/SharedMemory/tcp/premake4.lua index 675ac8071c..a4879953dd 100644 --- a/examples/SharedMemory/tcp/premake4.lua +++ b/examples/SharedMemory/tcp/premake4.lua @@ -91,7 +91,12 @@ myfiles = "../PhysicsServerCommandProcessor.h", "../b3PluginManager.cpp", "../PhysicsDirect.cpp", + "../PhysicsClientC_API.cpp", "../PhysicsClient.cpp", + "../plugins/pdControlPlugin/pdControlPlugin.cpp", + "../plugins/pdControlPlugin/pdControlPlugin.h", + "../b3RobotSimulatorClientAPI_NoDirect.cpp", + "../b3RobotSimulatorClientAPI_NoDirect.h", "../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", "../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp", "../../TinyRenderer/geometry.cpp", diff --git a/examples/SharedMemory/udp/premake4.lua b/examples/SharedMemory/udp/premake4.lua index 6a3acc0311..f9b7f8c4be 100644 --- a/examples/SharedMemory/udp/premake4.lua +++ b/examples/SharedMemory/udp/premake4.lua @@ -77,6 +77,10 @@ language "C++" myfiles = { + "../plugins/pdControlPlugin/pdControlPlugin.cpp", + "../plugins/pdControlPlugin/pdControlPlugin.h", + "../b3RobotSimulatorClientAPI_NoDirect.cpp", + "../b3RobotSimulatorClientAPI_NoDirect.h", "../IKTrajectoryHelper.cpp", "../IKTrajectoryHelper.h", "../SharedMemoryCommands.h", @@ -85,6 +89,7 @@ myfiles = "../PhysicsServerCommandProcessor.h", "../b3PluginManager.cpp", "../PhysicsDirect.cpp", + "../PhysicsClientC_API.cpp", "../PhysicsClient.cpp", "../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", "../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp", diff --git a/examples/TwoJoint/CMakeLists.txt b/examples/TwoJoint/CMakeLists.txt index cadb204b46..966b5e2a6f 100644 --- a/examples/TwoJoint/CMakeLists.txt +++ b/examples/TwoJoint/CMakeLists.txt @@ -10,6 +10,10 @@ INCLUDE_DIRECTORIES( SET(RobotSimulator_SRCS TwoJointMain.cpp + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h ../../examples/SharedMemory/IKTrajectoryHelper.cpp ../../examples/SharedMemory/IKTrajectoryHelper.h ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index e4116f96e9..d01ad16557 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -15,6 +15,10 @@ ENDIF() SET(pybullet_SRCS pybullet.c + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h ../../examples/SharedMemory/IKTrajectoryHelper.cpp ../../examples/SharedMemory/IKTrajectoryHelper.h ../../examples/ExampleBrowser/InProcessExampleBrowser.cpp diff --git a/examples/pybullet/examples/pdControl.py b/examples/pybullet/examples/pdControl.py new file mode 100644 index 0000000000..501a84dc0c --- /dev/null +++ b/examples/pybullet/examples/pdControl.py @@ -0,0 +1,62 @@ + +import pybullet as p +import time + +useMaximalCoordinates=False +p.connect(p.GUI) +pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates) +for i in range (p.getNumJoints(pole)): + #disable default constraint-based motors + p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0) + print("joint",i,"=",p.getJointInfo(pole,i)) + + +desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2) +desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0) +kpCartId = p.addUserDebugParameter("kpCart",0,500,300) +kdCartId = p.addUserDebugParameter("kpCart",0,300,150) +maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000) + + +desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0) +desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0) +kpPoleId = p.addUserDebugParameter("kpPole",0,500,200) +kdPoleId = p.addUserDebugParameter("kpPole",0,300,100) +maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000) + +pd = p.loadPlugin("pdControlPlugin") + + +p.setGravity(0,0,0) + +useRealTimeSim = True + +p.setRealTimeSimulation(useRealTimeSim) + +p.setTimeStep(0.001) + + +while p.isConnected(): + if (pd>=0): + desiredPosCart = p.readUserDebugParameter(desiredPosCartId) + desiredVelCart = p.readUserDebugParameter(desiredVelCartId) + kpCart = p.readUserDebugParameter(kpCartId) + kdCart = p.readUserDebugParameter(kdCartId) + maxForceCart = p.readUserDebugParameter(maxForceCartId) + link = 0 + p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosCart, desiredVelCart, kdCart, kpCart, maxForceCart]) + + desiredPosPole = p.readUserDebugParameter(desiredPosPoleId) + desiredVelPole = p.readUserDebugParameter(desiredVelPoleId) + kpPole = p.readUserDebugParameter(kpPoleId) + kdPole = p.readUserDebugParameter(kdPoleId) + maxForcePole = p.readUserDebugParameter(maxForcePoleId) + link = 1 + p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosPole, desiredVelPole, kdPole, kpPole, maxForcePole]) + + + if (not useRealTimeSim): + p.stepSimulation() + time.sleep(1./240.) + + \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf b/examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf index e69de29bb2..9b70df5756 100644 --- a/examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf +++ b/examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index db15fa67f3..04380b9b0a 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -103,6 +103,8 @@ if not _OPTIONS["no-enet"] then "../../examples/TinyRenderer/our_gl.cpp", "../../examples/TinyRenderer/TinyRenderer.cpp", "../../examples/SharedMemory/InProcessMemory.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", "../../examples/SharedMemory/PhysicsClient.cpp", "../../examples/SharedMemory/PhysicsClient.h", "../../examples/SharedMemory/PhysicsServer.cpp", @@ -153,6 +155,8 @@ if not _OPTIONS["no-enet"] then "../../examples/MultiThreading/b3PosixThreadSupport.cpp", "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", } if (_OPTIONS["enable_static_vr_plugin"]) then diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e89724b610..ee2ad24838 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8316,7 +8316,14 @@ static PyObject* pybullet_executePluginCommand(PyObject* self, float val = pybullet_internalGetFloatFromSequence(seqFloatArgs,i); b3CustomCommandExecuteAddFloatArgument(command, val); } - + if (seqFloatArgs) + { + Py_DECREF(seqFloatArgs); + } + if (seqIntArgs) + { + Py_DECREF(seqIntArgs); + } } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); diff --git a/setup.py b/setup.py index 570c95d9a4..ce17a665c2 100644 --- a/setup.py +++ b/setup.py @@ -49,6 +49,8 @@ +["examples/TinyRenderer/tgaimage.cpp"]\ +["examples/TinyRenderer/our_gl.cpp"]\ +["examples/TinyRenderer/TinyRenderer.cpp"]\ ++["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\ ++["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\ +["examples/SharedMemory/IKTrajectoryHelper.cpp"]\ +["examples/SharedMemory/InProcessMemory.cpp"]\ +["examples/SharedMemory/PhysicsClient.cpp"]\ diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index c9e11ecea6..78746108ba 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -29,66 +29,69 @@ ENDIF() ADD_EXECUTABLE(Test_PhysicsClientServer gtestwrap.cpp ../../examples/SharedMemory/PhysicsClient.cpp - ../../examples/SharedMemory/IKTrajectoryHelper.cpp - ../../examples/SharedMemory/IKTrajectoryHelper.h - ../../examples/SharedMemory/PhysicsClient.h - ../../examples/SharedMemory/PhysicsServer.cpp - ../../examples/SharedMemory/PhysicsServer.h - ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp - ../../examples/SharedMemory/PhysicsServerSharedMemory.h - ../../examples/SharedMemory/PhysicsDirect.cpp - ../../examples/SharedMemory/PhysicsDirect.h - ../../examples/SharedMemory/PhysicsDirectC_API.cpp - ../../examples/SharedMemory/PhysicsDirectC_API.h - ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp - ../../examples/SharedMemory/PhysicsServerCommandProcessor.h - ../../examples/SharedMemory/b3PluginManager.cpp - ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp - ../../examples/SharedMemory/PhysicsClientSharedMemory.h - ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp - ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h - - ../../examples/SharedMemory/PhysicsClientC_API.cpp - ../../examples/SharedMemory/PhysicsClientC_API.h - ../../examples/SharedMemory/PhysicsLoopBack.cpp - ../../examples/SharedMemory/PhysicsLoopBack.h - ../../examples/SharedMemory/PhysicsLoopBackC_API.cpp - ../../examples/SharedMemory/PhysicsLoopBackC_API.h - ../../examples/SharedMemory/Win32SharedMemory.cpp - ../../examples/SharedMemory/Win32SharedMemory.h - ../../examples/SharedMemory/PosixSharedMemory.cpp - ../../examples/SharedMemory/PosixSharedMemory.h - ../../examples/Utils/b3ResourcePath.cpp - ../../examples/Utils/b3ResourcePath.h - ../../examples/Utils/b3Clock.cpp - ../../examples/Utils/b3Clock.h - ../../examples/Utils/ChromeTraceUtil.cpp - ../../examples/Utils/ChromeTraceUtil.h - ../../examples/Utils/RobotLoggingUtil.cpp - ../../examples/Utils/RobotLoggingUtil.h - ../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp - ../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp - ../../examples/OpenGLWindow/SimpleCamera.cpp - ../../examples/OpenGLWindow/SimpleCamera.h - ../../examples/TinyRenderer/geometry.cpp - ../../examples/TinyRenderer/model.cpp - ../../examples/TinyRenderer/tgaimage.cpp - ../../examples/TinyRenderer/our_gl.cpp - ../../examples/TinyRenderer/TinyRenderer.cpp - ../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp - ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h - ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp - ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp - ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp - ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp - ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp - ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp - ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp - ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp - ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp - ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp - ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp + ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp + ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h + ../../examples/SharedMemory/IKTrajectoryHelper.cpp + ../../examples/SharedMemory/IKTrajectoryHelper.h + ../../examples/SharedMemory/PhysicsClient.h + ../../examples/SharedMemory/PhysicsServer.cpp + ../../examples/SharedMemory/PhysicsServer.h + ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp + ../../examples/SharedMemory/PhysicsServerSharedMemory.h + ../../examples/SharedMemory/PhysicsDirect.cpp + ../../examples/SharedMemory/PhysicsDirect.h + ../../examples/SharedMemory/PhysicsDirectC_API.cpp + ../../examples/SharedMemory/PhysicsDirectC_API.h + ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp + ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/b3PluginManager.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory.h + ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h + ../../examples/SharedMemory/PhysicsClientC_API.cpp + ../../examples/SharedMemory/PhysicsClientC_API.h + ../../examples/SharedMemory/PhysicsLoopBack.cpp + ../../examples/SharedMemory/PhysicsLoopBack.h + ../../examples/SharedMemory/PhysicsLoopBackC_API.cpp + ../../examples/SharedMemory/PhysicsLoopBackC_API.h + ../../examples/SharedMemory/Win32SharedMemory.cpp + ../../examples/SharedMemory/Win32SharedMemory.h + ../../examples/SharedMemory/PosixSharedMemory.cpp + ../../examples/SharedMemory/PosixSharedMemory.h + ../../examples/Utils/b3ResourcePath.cpp + ../../examples/Utils/b3ResourcePath.h + ../../examples/Utils/b3Clock.cpp + ../../examples/Utils/b3Clock.h + ../../examples/Utils/ChromeTraceUtil.cpp + ../../examples/Utils/ChromeTraceUtil.h + ../../examples/Utils/RobotLoggingUtil.cpp + ../../examples/Utils/RobotLoggingUtil.h + ../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp + ../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp + ../../examples/OpenGLWindow/SimpleCamera.cpp + ../../examples/OpenGLWindow/SimpleCamera.h + ../../examples/TinyRenderer/geometry.cpp + ../../examples/TinyRenderer/model.cpp + ../../examples/TinyRenderer/tgaimage.cpp + ../../examples/TinyRenderer/our_gl.cpp + ../../examples/TinyRenderer/TinyRenderer.cpp + ../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp + ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h + ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp + ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp + ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp + ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp + ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp + ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp + ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp + ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp + ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp + ../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp + ../../examples/ThirdPartyLibs/stb_image/stb_image.cpp ) diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 9e7b8619bc..f51f77e043 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -170,6 +170,10 @@ project ("Test_PhysicsServerLoopBack") files { "test.c", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/IKTrajectoryHelper.cpp", "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/SharedMemory/PhysicsClient.cpp", @@ -186,6 +190,8 @@ project ("Test_PhysicsServerLoopBack") "../../examples/SharedMemory/PhysicsLoopBack.h", "../../examples/SharedMemory/PhysicsLoopBackC_API.cpp", "../../examples/SharedMemory/PhysicsLoopBackC_API.h", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h", "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", "../../examples/SharedMemory/PhysicsClientSharedMemory.h", "../../examples/SharedMemory/PhysicsClientC_API.cpp", @@ -224,7 +230,7 @@ project ("Test_PhysicsServerLoopBack") "../../examples/Importers/ImportURDFDemo/UrdfParser.cpp", "../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp", "../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", + "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp", } if (_OPTIONS["enable_static_plugins"]) then @@ -256,6 +262,10 @@ end files { "test.c", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/IKTrajectoryHelper.cpp", "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/SharedMemory/PhysicsClient.cpp", @@ -360,6 +370,10 @@ project ("Test_PhysicsServerInProcessExampleBrowser") files { "test.c", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", + "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", + "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/IKTrajectoryHelper.cpp", "../../examples/SharedMemory/IKTrajectoryHelper.h", "../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",