diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2019-03-06 22:03:00 -0800 |
---|---|---|
committer | GitHub <noreply@github.com> | 2019-03-06 22:03:00 -0800 |
commit | 4d39b7caca3beedaa7dcd333536edbefe24d8641 (patch) | |
tree | 51eccaa0df4c03906acb25ab7e8cb7063d32e38e | |
parent | a9e350b0fb8b7fba0a1e33397338b7534641d541 (diff) | |
parent | 8b5a238b2f3feda5cc01be168cf0bd6bc4d81b81 (diff) | |
download | bullet3-4d39b7caca3beedaa7dcd333536edbefe24d8641.tar.gz |
Merge pull request #2142 from fuchuyuan/fixedJoint
Fixed joint
-rw-r--r-- | examples/BulletRobotics/FixJointBoxes.cpp | 117 | ||||
-rw-r--r-- | examples/BulletRobotics/FixJointBoxes.h | 21 | ||||
-rw-r--r-- | examples/ExampleBrowser/CMakeLists.txt | 1 | ||||
-rw-r--r-- | examples/ExampleBrowser/ExampleEntries.cpp | 5 |
4 files changed, 144 insertions, 0 deletions
diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp new file mode 100644 index 000000000..875a76b1d --- /dev/null +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -0,0 +1,117 @@ + +#include "FixJointBoxes.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 <string> +#include <vector> +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" + +class FixJointBoxes : public CommonExampleInterface +{ + GUIHelperInterface* m_guiHelper; + b3RobotSimulatorClientAPI m_robotSim; + int m_options; + +public: + FixJointBoxes(GUIHelperInterface* helper, int options) + : m_guiHelper(helper), + m_options(options) + { + } + + virtual ~FixJointBoxes() + { + } + + 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; + + size_t numCubes = 10; + std::vector<int> 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); + + 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) + { + m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); + } + + m_robotSim.loadURDF("plane.urdf"); + } + } + + 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; + // 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]); + } +}; + +class CommonExampleInterface* FixJointBoxesCreateFunc(struct CommonExampleOptions& options) +{ + return new FixJointBoxes(options.m_guiHelper, options.m_option); +} diff --git a/examples/BulletRobotics/FixJointBoxes.h b/examples/BulletRobotics/FixJointBoxes.h new file mode 100644 index 000000000..c8a305dfc --- /dev/null +++ b/examples/BulletRobotics/FixJointBoxes.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 FIX_JOINT_BOXES_H +#define FIX_JOINT_BOXES_H + +class CommonExampleInterface* FixJointBoxesCreateFunc(struct CommonExampleOptions& options); + +#endif //FIX_JOINT_BOXES_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 2678fa1b9..99dc6fd1d 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -144,6 +144,7 @@ SET(ExtendedTutorialsSources ) SET(BulletExampleBrowser_SRCS + ../BulletRobotics/FixJointBoxes.cpp ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 02789d034..059795646 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -3,6 +3,7 @@ #include "../BlockSolver/BlockSolverExample.h" #include "LinearMath/btAlignedObjectArray.h" #include "EmptyExample.h" +#include "../BulletRobotics/FixJointBoxes.h" #include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/RaytracerSetup.h" @@ -129,6 +130,10 @@ 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, "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), ExampleEntry(1, "TestJointTorque", "Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc), |