From 5d60c274c4ff0ee8b8f4b4ce5519feec3ca1ce43 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 4 Mar 2019 14:05:12 -0800 Subject: [PATCH 1/8] Add an example of a stack of boxes of large mass ratio. The default solver cannot solve it effectively. --- examples/BulletRobotics/BoxStack.cpp | 119 +++++++++++++++++++++ examples/BulletRobotics/BoxStack.h | 21 ++++ examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 2 +- 4 files changed, 143 insertions(+), 1 deletion(-) create mode 100644 examples/BulletRobotics/BoxStack.cpp create mode 100644 examples/BulletRobotics/BoxStack.h diff --git a/examples/BulletRobotics/BoxStack.cpp b/examples/BulletRobotics/BoxStack.cpp new file mode 100644 index 000000000..df4056465 --- /dev/null +++ b/examples/BulletRobotics/BoxStack.cpp @@ -0,0 +1,119 @@ + +#include "BoxStack.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.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 "../CommonInterfaces/CommonParameterInterface.h" +#include "../SharedMemory/SharedMemoryPublic.h" +#include + +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" +#include "../Utils/b3Clock.h" + +class BoxStackExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + b3RobotSimulatorClientAPI m_robotSim; + int m_options; + +public: + BoxStackExample(GUIHelperInterface* helper, int options) + : m_app(helper->getAppInterface()), + m_guiHelper(helper), + m_options(options) + { + m_app->setUpAxis(2); + } + + virtual ~BoxStackExample() + { + } + + virtual void physicsDebugDraw(int debugDrawMode) + { + m_robotSim.debugDraw(debugDrawMode); + } + virtual void initPhysics() + { + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + + b3Printf("robotSim connected = %d", connected); + + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); + + b3RobotSimulatorLoadUrdfFileArgs args; + b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; + int massRatio = 4; + int mass = 1; + for (int i = 0; i < 8; i++) + { + args.m_startPosition.setValue(0, 0, i * .06); + int boxIdx = m_robotSim.loadURDF("cube_small.urdf", args); + dynamicsArgs.m_mass = mass; + m_robotSim.changeDynamics(boxIdx, -1, dynamicsArgs); + mass *= massRatio; + } + + m_robotSim.loadURDF("plane.urdf"); + m_robotSim.setGravity(btVector3(0, 0, -10)); + } + + virtual void exitPhysics() + { + m_robotSim.disconnect(); + } + virtual void stepSimulation(float deltaTime) + { + m_robotSim.stepSimulation(); + } + virtual void renderScene() + { + m_robotSim.renderScene(); + } + + virtual bool mouseMoveCallback(float x, float y) + { + return m_robotSim.mouseMoveCallback(x, y); + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return m_robotSim.mouseButtonCallback(button, state, x, y); + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 1.5; + float pitch = -10; + float yaw = 18; + float targetPos[3] = {-0.2, 0.8, 0.3}; + + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + + 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* BoxStackExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new BoxStackExample(options.m_guiHelper, options.m_option); +} diff --git a/examples/BulletRobotics/BoxStack.h b/examples/BulletRobotics/BoxStack.h new file mode 100644 index 000000000..289b4b034 --- /dev/null +++ b/examples/BulletRobotics/BoxStack.h @@ -0,0 +1,21 @@ +/* +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 BOX_STACK_EXAMPLE_H +#define BOX_STACK_EXAMPLE_H + +class CommonExampleInterface* BoxStackExampleCreateFunc(struct CommonExampleOptions& options); + +#endif //BOX_STACK_EXAMPLE_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 99dc6fd1d..2b19f4161 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -146,6 +146,8 @@ SET(ExtendedTutorialsSources SET(BulletExampleBrowser_SRCS ../BulletRobotics/FixJointBoxes.cpp + ../BulletRobotics/BoxStack.cpp + ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp ../TinyRenderer/tgaimage.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 059795646..57c505879 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -130,8 +130,8 @@ 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, "Bullet Robotics"), + ExampleEntry(1, "Box Stack", "Create a stack of boxes using C API", BoxStackExampleCreateFunc), ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc), ExampleEntry(0, "MultiBody"), From 9141caf67e094d47c7246804420fb952bacc3570 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 4 Mar 2019 17:00:09 -0800 Subject: [PATCH 2/8] Add joint limit example. The demo artifically exceeds joint limit due to numerical errors caused by large ratio. --- examples/BulletRobotics/JointLimit.cpp | 112 +++++++++++++++++++++ examples/BulletRobotics/JointLimit.h | 21 ++++ examples/ExampleBrowser/CMakeLists.txt | 1 + examples/ExampleBrowser/ExampleEntries.cpp | 6 +- 4 files changed, 138 insertions(+), 2 deletions(-) create mode 100644 examples/BulletRobotics/JointLimit.cpp create mode 100644 examples/BulletRobotics/JointLimit.h diff --git a/examples/BulletRobotics/JointLimit.cpp b/examples/BulletRobotics/JointLimit.cpp new file mode 100644 index 000000000..1169b3039 --- /dev/null +++ b/examples/BulletRobotics/JointLimit.cpp @@ -0,0 +1,112 @@ + +#include "JointLimit.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.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 "../SharedMemory/SharedMemoryPublic.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include + +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" +#include "../Utils/b3Clock.h" + +class JointLimit : public CommonExampleInterface +{ + GUIHelperInterface* m_guiHelper; + b3RobotSimulatorClientAPI m_robotSim; + int m_options; + +public: + JointLimit(GUIHelperInterface* helper, int options) + : m_guiHelper(helper), + m_options(options) + { + } + + virtual ~JointLimit() + { + } + + virtual void physicsDebugDraw(int debugDrawMode) + { + m_robotSim.debugDraw(debugDrawMode); + } + virtual void initPhysics() + { + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + + b3Printf("robotSim connected = %d", connected); + + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); + + b3RobotSimulatorSetPhysicsEngineParameters physicsArgs; + physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG; + + b3Printf(m_robotSim.getSolvername()); + + physicsArgs.m_defaultGlobalCFM = 1e-6; + + m_robotSim.setNumSolverIterations(10); + + + b3RobotSimulatorLoadUrdfFileArgs loadArgs; + int humanoid = m_robotSim.loadURDF("test_joints_MB.urdf", loadArgs); + + b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; + dynamicsArgs.m_linearDamping = 0; + dynamicsArgs.m_angularDamping = 0; + m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs); + + m_robotSim.setGravity(btVector3(0, 0, -10)); + } + + virtual void exitPhysics() + { + m_robotSim.disconnect(); + } + virtual void stepSimulation(float deltaTime) + { + m_robotSim.stepSimulation(); + } + virtual void renderScene() + { + m_robotSim.renderScene(); + } + + virtual bool mouseMoveCallback(float x, float y) + { + return m_robotSim.mouseMoveCallback(x, y); + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return m_robotSim.mouseButtonCallback(button, state, x, y); + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 3; + float pitch = -10; + float yaw = 18; + float targetPos[3] = {0.6, 0.8, 0.3}; + + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } +}; + +class CommonExampleInterface* JointLimitCreateFunc(struct CommonExampleOptions& options) +{ + return new JointLimit(options.m_guiHelper, options.m_option); +} diff --git a/examples/BulletRobotics/JointLimit.h b/examples/BulletRobotics/JointLimit.h new file mode 100644 index 000000000..c8a125e53 --- /dev/null +++ b/examples/BulletRobotics/JointLimit.h @@ -0,0 +1,21 @@ +/* +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 JOINT_LIMIT_EXAMPLE_H +#define JOINT_LIMIT_EXAMPLE_H + +class CommonExampleInterface* JointLimitCreateFunc(struct CommonExampleOptions& options); + +#endif //JOINT_LIMIT_EXAMPLE_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 2b19f4161..137c2a71f 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -147,6 +147,7 @@ SET(BulletExampleBrowser_SRCS ../BulletRobotics/FixJointBoxes.cpp ../BulletRobotics/BoxStack.cpp + ../BulletRobotics/JointLimit.cpp ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 57c505879..c26197873 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -3,7 +3,9 @@ #include "../BlockSolver/BlockSolverExample.h" #include "LinearMath/btAlignedObjectArray.h" #include "EmptyExample.h" +#include "../BulletRobotics/BoxStack.h" #include "../BulletRobotics/FixJointBoxes.h" +#include "../BulletRobotics/JointLimit.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/RaytracerSetup.h" @@ -131,8 +133,8 @@ 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, "Bullet Robotics"), - ExampleEntry(1, "Box Stack", "Create a stack of boxes using C API", BoxStackExampleCreateFunc), - ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc), + ExampleEntry(1, "Box Stack", "Create a stack of boxes of large mass ratio.", BoxStackExampleCreateFunc), + ExampleEntry(1, "Joint Limit", "Create three objects joint together", JointLimitCreateFunc), ExampleEntry(0, "MultiBody"), ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc), From e81845249428b447ef50638c92a20a5c561b7dcb Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 5 Mar 2019 15:44:06 -0800 Subject: [PATCH 3/8] Add grasp box example and fixjoint boxes example. tmp gripper is working? fix joint example --- examples/BulletRobotics/FixJointBoxes.cpp | 107 +++++++++--- examples/BulletRobotics/GraspBox.cpp | 186 +++++++++++++++++++++ examples/BulletRobotics/GraspBox.h | 21 +++ examples/BulletRobotics/JointLimit.cpp | 13 +- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 5 + 6 files changed, 300 insertions(+), 34 deletions(-) create mode 100644 examples/BulletRobotics/GraspBox.cpp create mode 100644 examples/BulletRobotics/GraspBox.h diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp index 875a76b1d..3acf80758 100644 --- a/examples/BulletRobotics/FixJointBoxes.cpp +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -14,16 +14,26 @@ #include #include "../RobotSimulator/b3RobotSimulatorClientAPI.h" +static btScalar numSolverIterations = 1000; +static btScalar solverId = 0; + class FixJointBoxes : public CommonExampleInterface { GUIHelperInterface* m_guiHelper; b3RobotSimulatorClientAPI m_robotSim; int m_options; + b3RobotSimulatorSetPhysicsEngineParameters physicsArgs; + int solver; + + const size_t numCubes = 30; + std::vector cubeIds; public: FixJointBoxes(GUIHelperInterface* helper, int options) : m_guiHelper(helper), - m_options(options) + m_options(options), + cubeIds(numCubes, 0), + solver(solverId) { } @@ -47,39 +57,84 @@ public: m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); - b3RobotSimulatorLoadUrdfFileArgs args; - - size_t numCubes = 10; - std::vector cubeIds(numCubes, 0); - for (int i = 0; i < numCubes; i++) { - args.m_forceOverrideFixedBase = (i == 0); - args.m_startPosition.setValue(0, i * 0.05, 1); - cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args); + b3RobotSimulatorLoadUrdfFileArgs args; + b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; - b3JointInfo jointInfo; - - jointInfo.m_parentFrame[1] = -0.025; - jointInfo.m_childFrame[1] = 0.025; - jointInfo.m_jointType = eFixedType; - // jointInfo.m_jointType = ePoint2PointType; - // jointInfo.m_jointType = ePrismaticType; - - if (i > 0) + for (int i = 0; i < numCubes; i++) { - m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); - } + args.m_forceOverrideFixedBase = (i == 0); + args.m_startPosition.setValue(0, i * 0.05, 1); + cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args); - m_robotSim.loadURDF("plane.urdf"); + b3RobotJointInfo jointInfo; + + jointInfo.m_parentFrame[1] = -0.025; + jointInfo.m_childFrame[1] = 0.025; + + if (i > 0) + { + m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); + } + + m_robotSim.loadURDF("plane.urdf"); + } } + + { + SliderParams slider("Direct solver", &solverId); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("numSolverIterations", &numSolverIterations); + slider.m_minVal = 50; + slider.m_maxVal = 1e4; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + physicsArgs.m_defaultGlobalCFM = 1e-6; + m_robotSim.setPhysicsEngineParameter(physicsArgs); + + m_robotSim.setGravity(btVector3(0, 0, -10)); + m_robotSim.setNumSolverIterations((int)numSolverIterations); } virtual void exitPhysics() { m_robotSim.disconnect(); } + + void resetCubePosition() + { + for (int i = 0; i < numCubes; i++) + { + btVector3 pos = {0, i * 0.05, 1}; + btQuaternion quar = {0, 0, 0, 1}; + m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar); + } + } virtual void stepSimulation(float deltaTime) { + int newSolver = (int)(solverId + 0.5); + if (newSolver != solver) + { + printf("Switching solver, new %d, old %d\n", newSolver, solver); + solver = newSolver; + resetCubePosition(); + if (solver) + { + physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG; + } + else + { + physicsArgs.m_constraintSolverType = eConstraintSolverLCP_SI; + } + + m_robotSim.setPhysicsEngineParameter(physicsArgs); + } + m_robotSim.setNumSolverIterations((int)numSolverIterations); m_robotSim.stepSimulation(); } virtual void renderScene() @@ -102,12 +157,12 @@ public: virtual void resetCamera() { - // float dist = 1; - // float pitch = -20; - // float yaw = -30; - // float targetPos[3] = {0, 0.2, 0.5}; + float dist = 1.2; + float pitch = -20; + float yaw = 90; + float targetPos[3] = {.5, 0.6, 0.5}; - // m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } }; diff --git a/examples/BulletRobotics/GraspBox.cpp b/examples/BulletRobotics/GraspBox.cpp new file mode 100644 index 000000000..6a8d077fa --- /dev/null +++ b/examples/BulletRobotics/GraspBox.cpp @@ -0,0 +1,186 @@ + +#include "GraspBox.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.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 "../CommonInterfaces/CommonParameterInterface.h" +#include "../SharedMemory/SharedMemoryPublic.h" +#include +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" + +static btScalar x = 0.401f; +static btScalar y = 0.025f; +static btScalar z = 0.44f; + +class GraspBox : public CommonExampleInterface +{ + GUIHelperInterface* m_guiHelper; + b3RobotSimulatorClientAPI m_robotSim; + int m_options; + + int m_armIndex; + int boxId; + +public: + GraspBox(GUIHelperInterface* helper, int options) + : m_guiHelper(helper), + m_options(options) + { + } + + virtual ~GraspBox() + { + } + + virtual void physicsDebugDraw(int debugDrawMode) + { + m_robotSim.debugDraw(debugDrawMode); + } + virtual void initPhysics() + { + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + + b3Printf("robotSim connected = %d", connected); + + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); + + double cubePos[3] = {0.41, 0, 0.5}; + double cubeHalfLength = 0.025; + + { + SliderParams slider("position x", &x); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("position y", &y); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("position z", &z); + slider.m_minVal = 0; + slider.m_maxVal = 1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + b3RobotSimulatorLoadFileResults results; + m_robotSim.loadSDF("kuka_iiwa/kuka_with_gripper.sdf", results); + if (results.m_uniqueObjectIds.size() == 1) + { + m_armIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_armIndex); + b3Printf("numJoints = %d", numJoints); + + for (int i = 0; i < numJoints; i++) + { + b3RobotJointInfo jointInfo; + m_robotSim.getJointInfo(m_armIndex, i, &jointInfo); + b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName); + } + + //adjust arm position + m_robotSim.resetJointState(m_armIndex, 3, -SIMD_HALF_PI); + m_robotSim.resetJointState(m_armIndex, 5, SIMD_HALF_PI); + + // adjust gripper position + m_robotSim.resetJointState(m_armIndex, 8, -0.12); + m_robotSim.resetJointState(m_armIndex, 11, 0.12); + + m_robotSim.resetJointState(m_armIndex, 10, -0.12); + m_robotSim.resetJointState(m_armIndex, 13, 0.12); + + // { + // b3RobotJointInfo jointInfo; + // m_robotSim.getJointInfo(m_armIndex, 8, &jointInfo); + // jointInfo.m_jointType = eGearType; + // m_robotSim.createConstraint(m_armIndex, , cubeIds[i - 1], -1, &jointInfo); + // } + // for(int i = 0; i<14; i++){ + // b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + // controlArgs.m_maxTorqueValue = 0.0; + // m_robotSim.setJointMotorControl(m_armIndex, i, controlArgs); + // } + } + } + + { + b3RobotSimulatorLoadUrdfFileArgs args; + args.m_startPosition.setValue(x, y, z); + boxId = m_robotSim.loadURDF("cube_small.urdf", args); + } + m_robotSim.loadURDF("plane.urdf"); + m_robotSim.setGravity(btVector3(0, 0, -10)); + } + + virtual void exitPhysics() + { + m_robotSim.disconnect(); + } + + virtual void stepSimulation(float deltaTime) + { + btScalar sGripperVerticalVelocity = 0.f; + btScalar sGripperClosingTargetVelocity = -0.7f; + + int fingerJointIndices[4] = {8, 10, 11, 13}; + double fingerTargetVelocities[4] = {0.4, 0.4, -0.4, -0.4}; + + double maxTorqueValues[4] = {20.0, 20.0, 20.0, 20.0}; + for (int i = 0; i < 4; i++) + { + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); + controlArgs.m_targetVelocity = fingerTargetVelocities[i]; + controlArgs.m_maxTorqueValue = maxTorqueValues[i]; + controlArgs.m_kd = 1.; + m_robotSim.setJointMotorControl(m_armIndex, fingerJointIndices[i], controlArgs); + } + + m_robotSim.stepSimulation(); + } + virtual void renderScene() + { + m_robotSim.renderScene(); + } + + virtual bool mouseMoveCallback(float x, float y) + { + return m_robotSim.mouseMoveCallback(x, y); + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return m_robotSim.mouseButtonCallback(button, state, x, y); + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 0.8; + float pitch = 0; + float yaw = 0; + float targetPos[3] = {.4, 0.6, 0.45}; + + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } +}; + +class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options) +{ + return new GraspBox(options.m_guiHelper, options.m_option); +} diff --git a/examples/BulletRobotics/GraspBox.h b/examples/BulletRobotics/GraspBox.h new file mode 100644 index 000000000..70decb504 --- /dev/null +++ b/examples/BulletRobotics/GraspBox.h @@ -0,0 +1,21 @@ +/* +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 GRASP_BOX_EXAMPLE_H +#define GRASP_BOX_EXAMPLE_H + +class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options); + +#endif //GRASP_BOX_EXAMPLE_H diff --git a/examples/BulletRobotics/JointLimit.cpp b/examples/BulletRobotics/JointLimit.cpp index 1169b3039..6a4d91eef 100644 --- a/examples/BulletRobotics/JointLimit.cpp +++ b/examples/BulletRobotics/JointLimit.cpp @@ -51,20 +51,17 @@ public: b3RobotSimulatorSetPhysicsEngineParameters physicsArgs; physicsArgs.m_constraintSolverType = eConstraintSolverLCP_DANTZIG; - b3Printf(m_robotSim.getSolvername()); - physicsArgs.m_defaultGlobalCFM = 1e-6; - m_robotSim.setNumSolverIterations(10); - + m_robotSim.setNumSolverIterations(10); b3RobotSimulatorLoadUrdfFileArgs loadArgs; int humanoid = m_robotSim.loadURDF("test_joints_MB.urdf", loadArgs); - b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; - dynamicsArgs.m_linearDamping = 0; - dynamicsArgs.m_angularDamping = 0; - m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs); + b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; + dynamicsArgs.m_linearDamping = 0; + dynamicsArgs.m_angularDamping = 0; + m_robotSim.changeDynamics(humanoid, -1, dynamicsArgs); m_robotSim.setGravity(btVector3(0, 0, -10)); } diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 137c2a71f..6dded9dba 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -148,6 +148,8 @@ SET(BulletExampleBrowser_SRCS ../BulletRobotics/BoxStack.cpp ../BulletRobotics/JointLimit.cpp + ../BulletRobotics/GraspBox.cpp + ../BulletRobotics/FixJointBoxes.cpp ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c26197873..9693563fe 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -6,6 +6,8 @@ #include "../BulletRobotics/BoxStack.h" #include "../BulletRobotics/FixJointBoxes.h" #include "../BulletRobotics/JointLimit.h" +#include "../BulletRobotics/GraspBox.h" +#include "../BulletRobotics/FixJointBoxes.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/RaytracerSetup.h" @@ -132,9 +134,12 @@ 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, "Bullet Robotics"), ExampleEntry(1, "Box Stack", "Create a stack of boxes of large mass ratio.", BoxStackExampleCreateFunc), ExampleEntry(1, "Joint Limit", "Create three objects joint together", JointLimitCreateFunc), + ExampleEntry(1, "Grasp Box", "A robot arm of large mass tries to grasp a box of small mass", GraspBoxCreateFunc), + ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc), ExampleEntry(0, "MultiBody"), ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc), From 1660957f75279cb3e094d342c140aa473df79985 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 7 Mar 2019 16:49:24 -0800 Subject: [PATCH 4/8] Untrack Grasp box example for now. This example needs more adjustment. fix compile --- examples/BulletRobotics/GraspBox.cpp | 186 --------------------- examples/BulletRobotics/GraspBox.h | 21 --- examples/ExampleBrowser/CMakeLists.txt | 2 +- examples/ExampleBrowser/ExampleEntries.cpp | 4 +- 4 files changed, 3 insertions(+), 210 deletions(-) delete mode 100644 examples/BulletRobotics/GraspBox.cpp delete mode 100644 examples/BulletRobotics/GraspBox.h diff --git a/examples/BulletRobotics/GraspBox.cpp b/examples/BulletRobotics/GraspBox.cpp deleted file mode 100644 index 6a8d077fa..000000000 --- a/examples/BulletRobotics/GraspBox.cpp +++ /dev/null @@ -1,186 +0,0 @@ - -#include "GraspBox.h" - -#include "../CommonInterfaces/CommonGraphicsAppInterface.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 "../CommonInterfaces/CommonParameterInterface.h" -#include "../SharedMemory/SharedMemoryPublic.h" -#include -#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" - -static btScalar x = 0.401f; -static btScalar y = 0.025f; -static btScalar z = 0.44f; - -class GraspBox : public CommonExampleInterface -{ - GUIHelperInterface* m_guiHelper; - b3RobotSimulatorClientAPI m_robotSim; - int m_options; - - int m_armIndex; - int boxId; - -public: - GraspBox(GUIHelperInterface* helper, int options) - : m_guiHelper(helper), - m_options(options) - { - } - - virtual ~GraspBox() - { - } - - virtual void physicsDebugDraw(int debugDrawMode) - { - m_robotSim.debugDraw(debugDrawMode); - } - virtual void initPhysics() - { - int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; - m_robotSim.setGuiHelper(m_guiHelper); - bool connected = m_robotSim.connect(mode); - - b3Printf("robotSim connected = %d", connected); - - m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); - m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); - m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); - - double cubePos[3] = {0.41, 0, 0.5}; - double cubeHalfLength = 0.025; - - { - SliderParams slider("position x", &x); - slider.m_minVal = 0; - slider.m_maxVal = 1; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - } - - { - SliderParams slider("position y", &y); - slider.m_minVal = 0; - slider.m_maxVal = 1; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - } - { - SliderParams slider("position z", &z); - slider.m_minVal = 0; - slider.m_maxVal = 1; - m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); - } - - { - b3RobotSimulatorLoadFileResults results; - m_robotSim.loadSDF("kuka_iiwa/kuka_with_gripper.sdf", results); - if (results.m_uniqueObjectIds.size() == 1) - { - m_armIndex = results.m_uniqueObjectIds[0]; - int numJoints = m_robotSim.getNumJoints(m_armIndex); - b3Printf("numJoints = %d", numJoints); - - for (int i = 0; i < numJoints; i++) - { - b3RobotJointInfo jointInfo; - m_robotSim.getJointInfo(m_armIndex, i, &jointInfo); - b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName); - } - - //adjust arm position - m_robotSim.resetJointState(m_armIndex, 3, -SIMD_HALF_PI); - m_robotSim.resetJointState(m_armIndex, 5, SIMD_HALF_PI); - - // adjust gripper position - m_robotSim.resetJointState(m_armIndex, 8, -0.12); - m_robotSim.resetJointState(m_armIndex, 11, 0.12); - - m_robotSim.resetJointState(m_armIndex, 10, -0.12); - m_robotSim.resetJointState(m_armIndex, 13, 0.12); - - // { - // b3RobotJointInfo jointInfo; - // m_robotSim.getJointInfo(m_armIndex, 8, &jointInfo); - // jointInfo.m_jointType = eGearType; - // m_robotSim.createConstraint(m_armIndex, , cubeIds[i - 1], -1, &jointInfo); - // } - // for(int i = 0; i<14; i++){ - // b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); - // controlArgs.m_maxTorqueValue = 0.0; - // m_robotSim.setJointMotorControl(m_armIndex, i, controlArgs); - // } - } - } - - { - b3RobotSimulatorLoadUrdfFileArgs args; - args.m_startPosition.setValue(x, y, z); - boxId = m_robotSim.loadURDF("cube_small.urdf", args); - } - m_robotSim.loadURDF("plane.urdf"); - m_robotSim.setGravity(btVector3(0, 0, -10)); - } - - virtual void exitPhysics() - { - m_robotSim.disconnect(); - } - - virtual void stepSimulation(float deltaTime) - { - btScalar sGripperVerticalVelocity = 0.f; - btScalar sGripperClosingTargetVelocity = -0.7f; - - int fingerJointIndices[4] = {8, 10, 11, 13}; - double fingerTargetVelocities[4] = {0.4, 0.4, -0.4, -0.4}; - - double maxTorqueValues[4] = {20.0, 20.0, 20.0, 20.0}; - for (int i = 0; i < 4; i++) - { - b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); - controlArgs.m_targetVelocity = fingerTargetVelocities[i]; - controlArgs.m_maxTorqueValue = maxTorqueValues[i]; - controlArgs.m_kd = 1.; - m_robotSim.setJointMotorControl(m_armIndex, fingerJointIndices[i], controlArgs); - } - - m_robotSim.stepSimulation(); - } - virtual void renderScene() - { - m_robotSim.renderScene(); - } - - virtual bool mouseMoveCallback(float x, float y) - { - return m_robotSim.mouseMoveCallback(x, y); - } - virtual bool mouseButtonCallback(int button, int state, float x, float y) - { - return m_robotSim.mouseButtonCallback(button, state, x, y); - } - virtual bool keyboardCallback(int key, int state) - { - return false; - } - - virtual void resetCamera() - { - float dist = 0.8; - float pitch = 0; - float yaw = 0; - float targetPos[3] = {.4, 0.6, 0.45}; - - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); - } -}; - -class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options) -{ - return new GraspBox(options.m_guiHelper, options.m_option); -} diff --git a/examples/BulletRobotics/GraspBox.h b/examples/BulletRobotics/GraspBox.h deleted file mode 100644 index 70decb504..000000000 --- a/examples/BulletRobotics/GraspBox.h +++ /dev/null @@ -1,21 +0,0 @@ -/* -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 GRASP_BOX_EXAMPLE_H -#define GRASP_BOX_EXAMPLE_H - -class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options); - -#endif //GRASP_BOX_EXAMPLE_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 6dded9dba..0b76fdead 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -148,7 +148,7 @@ SET(BulletExampleBrowser_SRCS ../BulletRobotics/BoxStack.cpp ../BulletRobotics/JointLimit.cpp - ../BulletRobotics/GraspBox.cpp + # ../BulletRobotics/GraspBox.cpp ../BulletRobotics/FixJointBoxes.cpp ../TinyRenderer/geometry.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 9693563fe..15080e4dc 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -6,7 +6,7 @@ #include "../BulletRobotics/BoxStack.h" #include "../BulletRobotics/FixJointBoxes.h" #include "../BulletRobotics/JointLimit.h" -#include "../BulletRobotics/GraspBox.h" +// #include "../BulletRobotics/GraspBox.h" #include "../BulletRobotics/FixJointBoxes.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" @@ -138,7 +138,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(0, "Bullet Robotics"), ExampleEntry(1, "Box Stack", "Create a stack of boxes of large mass ratio.", BoxStackExampleCreateFunc), ExampleEntry(1, "Joint Limit", "Create three objects joint together", JointLimitCreateFunc), - ExampleEntry(1, "Grasp Box", "A robot arm of large mass tries to grasp a box of small mass", GraspBoxCreateFunc), + // ExampleEntry(1, "Grasp Box", "A robot arm of large mass tries to grasp a box of small mass", GraspBoxCreateFunc), ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc), ExampleEntry(0, "MultiBody"), From 2fb12400aca91f3a837820acdc8209de1322ea04 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 7 Mar 2019 17:29:36 -0800 Subject: [PATCH 5/8] Add urdf file for examples/BulletRobotics/JointLimit.cpp --- data/test_joints_MB.urdf | 87 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 data/test_joints_MB.urdf diff --git a/data/test_joints_MB.urdf b/data/test_joints_MB.urdf new file mode 100644 index 000000000..b575d5bf5 --- /dev/null +++ b/data/test_joints_MB.urdf @@ -0,0 +1,87 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From fa2368c6d63f62755bf223772044cd9c2051fde3 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Fri, 8 Mar 2019 13:50:12 -0800 Subject: [PATCH 6/8] Try to fix tests --- examples/BulletRobotics/FixJointBoxes.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp index 5a5bfa078..e492b4432 100644 --- a/examples/BulletRobotics/FixJointBoxes.cpp +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -60,7 +60,7 @@ public: { b3RobotSimulatorLoadUrdfFileArgs args; b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; - + for (int i = 0; i < numCubes; i++) { args.m_forceOverrideFixedBase = (i == 0); @@ -110,7 +110,7 @@ public: { for (int i = 0; i < numCubes; i++) { - btVector3 pos = {0, i * 0.05, 1}; + btVector3 pos = {0, i * (btScalar)0.05, 1}; btQuaternion quar = {0, 0, 0, 1}; m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar); } @@ -157,12 +157,11 @@ public: virtual void resetCamera() { - - float dist = 1; - float pitch = -20; - float yaw = -30; - float targetPos[3] = {0, 0.2, 0.5}; - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + float dist = 1; + float pitch = -20; + float yaw = -30; + float targetPos[3] = {0, 0.2, 0.5}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } }; From 81ab8cd1c6fea9d9382259bfa8d3132962f37142 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Fri, 8 Mar 2019 14:21:18 -0800 Subject: [PATCH 7/8] another fix --- examples/BulletRobotics/FixJointBoxes.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp index e492b4432..8034600f3 100644 --- a/examples/BulletRobotics/FixJointBoxes.cpp +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -110,8 +110,8 @@ public: { for (int i = 0; i < numCubes; i++) { - btVector3 pos = {0, i * (btScalar)0.05, 1}; - btQuaternion quar = {0, 0, 0, 1}; + btVector3 pos (0, i * (btScalar)0.05, 1); + btQuaternion quar (0, 0, 0, 1); m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar); } } From bea2f6e65a80798f3f524154204995ed15eb7af5 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 11 Mar 2019 13:12:49 -0700 Subject: [PATCH 8/8] Turn off self-collision for FixJointBoxes example --- examples/BulletRobotics/FixJointBoxes.cpp | 1 + .../b3RobotSimulatorClientAPI_NoDirect.cpp | 22 +++++++++++++++++++ .../b3RobotSimulatorClientAPI_NoDirect.h | 2 ++ 3 files changed, 25 insertions(+) diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp index 8034600f3..a5a89e329 100644 --- a/examples/BulletRobotics/FixJointBoxes.cpp +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -75,6 +75,7 @@ public: if (i > 0) { m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); + m_robotSim.setCollisionFilterGroupMask(cubeIds[i], -1, 0, 0); } m_robotSim.loadURDF("plane.urdf"); diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 284430847..9efed7f7e 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -2417,3 +2417,25 @@ void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::stri statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); } } + +void b3RobotSimulatorClientAPI_NoDirect::setCollisionFilterGroupMask(int bodyUniqueIdA, int linkIndexA, int collisionFilterGroup, int collisionFilterMask) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) + { + b3Warning("Not connected"); + return; + } + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3CollisionFilterCommandInit(sm); + b3SetCollisionFilterGroupMask(commandHandle, bodyUniqueIdA, linkIndexA, collisionFilterGroup, collisionFilterMask); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); +} + diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index cca09d974..297c7e203 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -664,6 +664,8 @@ public: return SHARED_MEMORY_MAGIC_NUMBER; } void setAdditionalSearchPath(const std::string &path); + + void setCollisionFilterGroupMask(int bodyUniqueIdA, int linkIndexA, int collisionFilterGroup, int collisionFilterMask); }; #endif //B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H