diff --git a/data/cube.mtl b/data/cube.mtl index 26b4b44e4..ffce2975d 100644 --- a/data/cube.mtl +++ b/data/cube.mtl @@ -11,4 +11,6 @@ newmtl cube Ke 0.0000 0.0000 0.0000 map_Ka cube.tga map_Kd cube.png + + diff --git a/data/cube.urdf b/data/cube.urdf new file mode 100644 index 000000000..679e106b5 --- /dev/null +++ b/data/cube.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/plane.mtl b/data/plane.mtl index 8ce4b8207..46c54e4be 100644 --- a/data/plane.mtl +++ b/data/plane.mtl @@ -1,11 +1,15 @@ -# Blender MTL File: 'None' -# Material Count: 1 - newmtl Material -Ns 96.078431 -Ka 0.000000 0.000000 0.000000 -Kd 0.640000 0.640000 0.640000 -Ks 0.500000 0.500000 0.500000 -Ni 1.000000 -d 1.000000 -illum 2 + Ns 10.0000 + Ni 1.5000 + d 1.0000 + Tr 0.0000 + Tf 1.0000 1.0000 1.0000 + illum 2 + Ka 0.0000 0.0000 0.0000 + Kd 0.5880 0.5880 0.5880 + Ks 0.0000 0.0000 0.0000 + Ke 0.0000 0.0000 0.0000 + map_Ka cube.tga + map_Kd checker_grid.jpg + + \ No newline at end of file diff --git a/data/plane.obj b/data/plane.obj index 64342328f..61e8e047c 100644 --- a/data/plane.obj +++ b/data/plane.obj @@ -2,11 +2,17 @@ # www.blender.org mtllib plane.mtl o Plane -v 1.000000 0.000000 -1.000000 -v 1.000000 0.000000 1.000000 -v -1.000000 0.000000 1.000000 -v -1.000000 0.000000 -1.000000 +v 5.000000 -5.000000 0.000000 +v 5.000000 5.000000 0.000000 +v -5.000000 5.000000 0.000000 +v -5.000000 -5.000000 0.000000 + +vt 1.000000 0.000000 +vt 1.000000 1.000000 +vt 0.000000 1.000000 +vt 0.000000 0.000000 + usemtl Material s off -f 1 2 3 -f 1 3 4 +f 1/1 2/2 3/3 +f 1/1 3/3 4/4 diff --git a/data/r2d2.urdf b/data/r2d2.urdf index 9f7c750c0..091369db8 100644 --- a/data/r2d2.urdf +++ b/data/r2d2.urdf @@ -79,7 +79,7 @@ - + diff --git a/data/sphere2.urdf b/data/sphere2.urdf index ec939e694..cf7618d93 100644 --- a/data/sphere2.urdf +++ b/data/sphere2.urdf @@ -11,6 +11,9 @@ + + + diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 114adb61f..b517a43c6 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -189,7 +189,10 @@ SET(BulletExampleBrowser_SRCS ../RenderingExamples/TimeSeriesCanvas.h ../RenderingExamples/TimeSeriesFontData.cpp ../RenderingExamples/TimeSeriesFontData.h - + ../RoboticsLearning/b3RobotSimAPI.cpp + ../RoboticsLearning/b3RobotSimAPI.h + ../RoboticsLearning/R2D2GraspExample.cpp + ../RoboticsLearning/R2D2GraspExample.h ../RenderingExamples/CoordinateSystemDemo.cpp ../RenderingExamples/CoordinateSystemDemo.h ../RenderingExamples/RaytracerSetup.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index b6004aeef..152ddff76 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -45,6 +45,7 @@ #include "../Tutorial/Dof6ConstraintTutorial.h" #include "../MultiThreading/MultiThreadingExample.h" #include "../InverseDynamics/InverseDynamicsExample.h" +#include "../RoboticsLearning/R2D2GraspExample.h" #ifdef ENABLE_LUA #include "../LuaDemo/LuaPhysicsSetup.h" @@ -90,11 +91,11 @@ struct ExampleEntry static ExampleEntry gDefaultExamples[]= { + ExampleEntry(0,"API"), ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc), - ExampleEntry(1,"Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc), ExampleEntry(1,"Constraints","Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.", @@ -114,8 +115,6 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",RigidBodySoftContactCreateFunc), - - ExampleEntry(0,"MultiBody"), ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc), ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc), @@ -241,11 +240,12 @@ static ExampleEntry gDefaultExamples[]= PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING), ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.", PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG), - ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc), ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), - + ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP), + ExampleEntry(1,"URDF Compliant Contact","Experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT), + #ifdef ENABLE_LUA diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 82c63e64d..522fffdaa 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -94,6 +94,7 @@ project "App_BulletExampleBrowser" "../ExtendedTutorials/Bridge.cpp", "../ExtendedTutorials/RigidBodyFromObj.cpp", "../Collision/*", + "../RoboticsLearning/*", "../Collision/Internal/*", "../Benchmarks/*", "../CommonInterfaces/*", diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 555acf530..1c24c2eba 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1005,6 +1005,17 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const return false; } +bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const +{ + UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); + if (linkPtr) + { + const UrdfLink* link = *linkPtr; + contactInfo = link->m_contactInfo; + return true; + } + return false; +} void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 14032e388..ea945aa33 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -37,6 +37,8 @@ public: virtual std::string getLinkName(int linkIndex) const; virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const; + + virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const; virtual std::string getJointName(int linkIndex) const; diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp index 51b09e079..4e9ddf6c4 100644 --- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp +++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp @@ -132,7 +132,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, if (gFileNameArray.size()==0) { - gFileNameArray.push_back("sphere2.urdf"); + gFileNameArray.push_back("r2d2.urdf"); } @@ -200,7 +200,7 @@ void ImportUrdfSetup::initPhysics() btVector3 gravity(0,0,0); - gravity[upAxis]=-9.8; + //gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index fbe9f13f8..a64bf44f5 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -392,9 +392,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col); - btScalar friction = 0.5f; + URDFLinkContactInfo contactInfo; + u2b.getLinkContactInfo(urdfLinkIndex,contactInfo); - col->setFriction(friction); + if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0) + { + col->setFriction(contactInfo.m_lateralFriction); + } + if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0) + { + col->setRollingFriction(contactInfo.m_rollingFriction); + } + if (mbLinkIndex>=0) //???? double-check +/- 1 { diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 345ef7d5f..e3299eda5 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -6,6 +6,7 @@ #include "LinearMath/btTransform.h" #include "URDFJointTypes.h" + class URDFImporterInterface { @@ -28,6 +29,9 @@ public: virtual std::string getLinkName(int linkIndex) const =0; /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;} + + ///this API will likely change, don't override it! + virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;} virtual std::string getJointName(int linkIndex) const = 0; diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index 0c50d2655..65d615ee3 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -11,6 +11,33 @@ enum UrdfJointTypes URDFPlanarJoint, URDFFixedJoint, }; +#include "LinearMath/btScalar.h" + +enum URDF_LinkContactFlags +{ + URDF_CONTACT_HAS_LATERAL_FRICTION=1, + URDF_CONTACT_HAS_ROLLING_FRICTION=2, + URDF_CONTACT_HAS_CONTACT_CFM=4, + URDF_CONTACT_HAS_CONTACT_ERP=8 +}; + +struct URDFLinkContactInfo +{ + btScalar m_lateralFriction; + btScalar m_rollingFriction; + btScalar m_contactCfm; + btScalar m_contactErp; + int m_flags; + + URDFLinkContactInfo() + :m_lateralFriction(0.5), + m_rollingFriction(0), + m_contactCfm(0), + m_contactErp(0) + { + m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION; + } +}; #endif //URDF_JOINT_TYPES_H diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index a6ec0a5c4..002a3cf93 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -569,6 +569,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi } } + { + //optional 'contact' parameters + TiXmlElement* ci = config->FirstChildElement("contact"); + if (ci) + { + TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction"); + if (friction_xml) + { + if (m_parseSDF) + { + link.m_contactInfo.m_lateralFriction = urdfLexicalCast(friction_xml->GetText()); + } else + { + if (!friction_xml->Attribute("value")) + { + logger->reportError("Link/contact: lateral_friction element must have value attribute"); + return false; + } + + link.m_contactInfo.m_lateralFriction = urdfLexicalCast(friction_xml->Attribute("value")); + } + } + } + } + // Inertial (optional) TiXmlElement *i = config->FirstChildElement("inertial"); if (i) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index b83c0b3c8..8e6dc6fb7 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -99,6 +99,8 @@ struct UrdfLink int m_linkIndex; + URDFLinkContactInfo m_contactInfo; + UrdfLink() :m_parentLink(0), m_parentJoint(0) diff --git a/examples/InverseDynamics/premake4.lua b/examples/InverseDynamics/premake4.lua index 53c4fa2ce..75105e702 100644 --- a/examples/InverseDynamics/premake4.lua +++ b/examples/InverseDynamics/premake4.lua @@ -91,7 +91,8 @@ files { "../Importers/ImportURDFDemo/UrdfParser.cpp", "../Importers/ImportURDFDemo/urdfStringSplit.cpp", "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", - "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../ThirdPartyLibs/stb_image/stb_image.cpp", + "../Utils/b3Clock.cpp", } if os.is("Linux") then initX11() end diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp new file mode 100644 index 000000000..b0f007940 --- /dev/null +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -0,0 +1,164 @@ + +#include "R2D2GraspExample.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include + +#include "b3RobotSimAPI.h" +#include "../Utils/b3Clock.h" + +///quick demo showing the right-handed coordinate system and positive rotations around each axis +class R2D2GraspExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + b3RobotSimAPI m_robotSim; + int m_options; + int m_r2d2Index; + + float m_x; + float m_y; + float m_z; + b3AlignedObjectArray m_movingInstances; + enum + { + numCubesX = 20, + numCubesY = 20 + }; +public: + + R2D2GraspExample(GUIHelperInterface* helper, int options) + :m_app(helper->getAppInterface()), + m_guiHelper(helper), + m_options(options), + m_r2d2Index(-1), + m_x(0), + m_y(0), + m_z(0) + { + m_app->setUpAxis(2); + } + virtual ~R2D2GraspExample() + { + m_app->m_renderer->enableBlend(false); + } + + + virtual void physicsDebugDraw(int debugDrawMode) + { + + } + virtual void initPhysics() + { + bool connected = m_robotSim.connect(m_guiHelper); + b3Printf("robotSim connected = %d",connected); + b3RobotSimLoadURDFArgs args(""); + + if ((m_options & eROBOTIC_LEARN_GRASP)!=0) + { + args.m_urdfFileName = "r2d2.urdf"; + args.m_startPosition.setValue(0,0,.5); + m_r2d2Index = m_robotSim.loadURDF(args); + int numJoints = m_robotSim.getNumJoints(m_r2d2Index); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;im_renderer->renderScene(); + } + + + virtual void physicsDebugDraw() + { + + } + virtual bool mouseMoveCallback(float x,float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 10; + float pitch = 50; + float yaw = 13; + float targetPos[3]={-1,0,-0.3}; + if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) + { + m_app->m_renderer->getActiveCamera()->setCameraDistance(dist); + m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch); + m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw); + m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]); + } + } + +}; + + +class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new R2D2GraspExample(options.m_guiHelper, options.m_option); +} + + + diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h new file mode 100644 index 000000000..353f684e5 --- /dev/null +++ b/examples/RoboticsLearning/R2D2GraspExample.h @@ -0,0 +1,28 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2016 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef R2D2_GRASP_EXAMPLE_H +#define R2D2_GRASP_EXAMPLE_H + +enum RobotLearningExampleOptions +{ + eROBOTIC_LEARN_GRASP=1, + eROBOTIC_LEARN_COMPLIANT_CONTACT=2, +}; + +class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options); + + +#endif //R2D2_GRASP_EXAMPLE_H diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp new file mode 100644 index 000000000..3310d82ce --- /dev/null +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -0,0 +1,729 @@ +#include "b3RobotSimAPI.h" + +//#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +//#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/PhysicsClientC_API.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, +}; + +#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) MultiThreadedOpenGLGuiHelper : 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; + + + MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) + :m_app(app) + ,m_cs(0), + m_texels(0), + m_textureId(-1) + { + m_childGuiHelper = guiHelper;; + + } + + virtual ~MultiThreadedOpenGLGuiHelper() + { + delete m_childGuiHelper; + } + + void setCriticalSection(b3CriticalSection* cs) + { + m_cs = cs; + } + + b3CriticalSection* getCriticalSection() + { + return m_cs; + } + + virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& 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) + { + } + } + + 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) + { + } + + virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied) + { + if (width) + *width = 0; + if (height) + *height = 0; + if (numPixelsCopied) + *numPixelsCopied = 0; + } + + virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) + { + } + + virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) + { + } +}; + + + + + + + + +struct b3RobotSimAPI_InternalData +{ + //GUIHelperInterface* m_guiHelper; + PhysicsServerSharedMemory m_physicsServer; + b3PhysicsClientHandle m_physicsClient; + + b3ThreadSupportInterface* m_threadSupport; + RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS]; + MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper; + + bool m_connected; + + b3RobotSimAPI_InternalData() + :m_multiThreadedHelper(0), + m_physicsClient(0), + 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); + +} + +b3RobotSimAPI::~b3RobotSimAPI() +{ + delete m_data; +} + +void b3RobotSimAPI::processMultiThreadedGraphicsRequests() +{ + 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 = 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 eRobotSimGUIHelperIdle: + default: + { + + } + } + + + + + #if 0 + if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK) + { + btClock rtc; + btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800); + + while (rtc.getTimeMilliseconds() 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::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args) +{ + b3SharedMemoryStatusHandle statusHandle; + switch (args.m_controlMode) + { + case CONTROL_MODE_VELOCITY: + { + b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_VELOCITY); + b3JointInfo jointInfo; + b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo); + int uIndex = jointInfo.m_uIndex; + 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 = b3JointControlCommandInit( m_data->m_physicsClient, 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 = b3JointControlCommandInit( m_data->m_physicsClient, 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"); + } + } +} + +int b3RobotSimAPI::loadURDF(const b3RobotSimLoadURDFArgs& args) +{ + int robotUniqueId = -1; + b3Assert(m_data->m_connected); + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_urdfFileName.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); + } + statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); + robotUniqueId = b3GetStatusBodyIndex(statusHandle); + } + + return robotUniqueId; +} + +bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) +{ + m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + + MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper); + + + + + m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS); + + for (int i=0;im_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;wm_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); + + m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper); + + b3Assert(m_data->m_connected); + + m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY); + int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient); + b3Assert(canSubmit); + return m_data->m_connected && canSubmit; +} + +void b3RobotSimAPI::disconnect() +{ + + for (int i=0;im_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::renderScene() +{ + if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms(); + } + + m_data->m_physicsServer.renderScene(); +} diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h new file mode 100644 index 000000000..35b56c67f --- /dev/null +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -0,0 +1,81 @@ +#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 + + +struct b3RobotSimLoadURDFArgs +{ + std::string m_urdfFileName; + b3Vector3 m_startPosition; + b3Quaternion m_startOrientation; + bool m_forceOverrideFixedBase; + + + b3RobotSimLoadURDFArgs(const std::string& urdfFileName) + :m_urdfFileName(urdfFileName), + m_startPosition(b3MakeVector3(0,0,0)), + m_startOrientation(b3Quaternion(0,0,0,1)), + m_forceOverrideFixedBase(false) + { + } +}; + +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.1), + m_maxTorqueValue(1000) + { + } +}; + + +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(); + + int loadURDF(const struct b3RobotSimLoadURDFArgs& args); + + int getNumJoints(int bodyUniqueId) const; + + bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); + + void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args); + + void stepSimulation(); + + void setGravity(const b3Vector3& gravityAcceleration); + + void renderScene(); +}; + +#endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 85d896a72..3f35f7cc5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1,5 +1,6 @@ +//todo(erwincoumans): re-use the upcoming b3RobotSimAPI here #include "PhysicsServerExample.h" @@ -12,8 +13,6 @@ #include "../Utils/b3Clock.h" #include "../MultiThreading/b3ThreadSupportInterface.h" -int blockme = false; -int blockme2 = false; void MotionThreadFunc(void* userPtr,void* lsMemory); void* MotionlsMemoryFunc(); @@ -116,6 +115,8 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) 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.)) { @@ -125,11 +126,9 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) } clock.reset(); +#endif + args->m_physicsServerPtr->processClientCommands(); - if (!blockme) - { - args->m_physicsServerPtr->processClientCommands(); - } } while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion); } else @@ -710,7 +709,6 @@ void PhysicsServerExample::stepSimulation(float deltaTime) } #endif - if (!blockme2) { if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index a4a696166..08411f40b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -634,13 +634,14 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* #define ENABLE_FRICTION #ifdef ENABLE_FRICTION solverConstraint.m_frictionIndex = frictionIndex; -#if ROLLING_FRICTION +//#define ROLLING_FRICTION +#ifdef ROLLING_FRICTION int rollingFriction=1; btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); + if (mbA) + angVelA = mbA->getVelocityVector()>getLink(fcA->m_link).l>getAngularVelocity(); + if (mbB) + angVelB = mbB->getAngularVelocity(); btVector3 relAngVel = angVelB-angVelA; if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index fab7129cc..0a8b6031d 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -58,6 +58,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) int statusType; int bodyIndicesOut[10];//MAX_SDF_BODIES = 10 int numJoints, numBodies; + int bodyUniqueId; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -65,7 +66,7 @@ void testSharedMemory(b3PhysicsClientHandle sm) numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); ASSERT_EQ(numBodies,1); - int bodyUniqueId = bodyIndicesOut[0]; + bodyUniqueId = bodyIndicesOut[0]; numJoints = b3GetNumJoints(sm,bodyUniqueId); ASSERT_EQ(numJoints,7);