Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #2145 from fuchuyuan/addExample

Browse Source 
Add example
This commit is contained in:
erwincoumans
2019-03-12 11:06:53 -07:00
committed by GitHub
parent d56b136155 bea2f6e65a
commit 18918389d3
10 changed files with 475 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

87
data/test_joints_MB.urdf Normal file
View File

@@ -0,0 +1,87 @@
<?xml version="1.0"?>
<robot name="physics">
<link name="DOF_ArmRotate">
<visual>
<geometry>
<box size="0.42 0.11 0.22"/>
</geometry>
<origin rpy="0 0 0" xyz="0.21 0 0"/>
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.42 0.11 0.22"/>
</geometry>
<origin rpy="0 0 0" xyz="0.21 0 0"/>
</collision>
<inertial>
<mass value="0"/>
<origin rpy="0 0 0" xyz="0.21 0 0 "/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<link name="DOF_Snodo1">
<visual>
<geometry>
<box size="0.68 0.23 0.64"/>
</geometry>
<origin rpy="0 0 0" xyz="0.34 0 0.32"/>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.68 0.23 0.64"/>
</geometry>
<origin rpy="0 0 0" xyz="0.34 0 0.32"/>
</collision>
<inertial>
<mass value="2"/>
<origin rpy="0 0 0" xyz="0.34 0 0.32"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="Rotate_White_Joint" type="revolute">
<axis xyz="0 1 0"/>
<limit effort="1000.0" lower="-0.785" upper="0.785" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.42 0.0 0.0"/>
<parent link="DOF_ArmRotate"/>
<child link="DOF_Snodo1"/>
</joint>
<link name="DOF_Snodo2">
<visual>
<geometry>
<box size="0.42 0.12 0.20"/>
</geometry>
<origin rpy="0 0 0" xyz="0.21 0 0.10"/>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
</visual>
<collision>
<geometry>
<box size="0.42 0.12 0.20"/>
</geometry>
<origin rpy="0 0 0" xyz="0.21 0 0.10"/>
</collision>
<inertial>
<mass value="1"/>
<origin rpy="0 0 0" xyz="0.21 0 0.10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="Rotate_Red_Joint" type="revolute">
<axis xyz="0 1 0"/>
<limit effort="1000.0" lower="-0.785" upper="0.785" velocity="0.5"/>
<origin rpy="0 0 0" xyz="0.68 0.0 0.64"/>
<parent link="DOF_Snodo1"/>
<child link="DOF_Snodo2"/>
</joint>
</robot>

119
examples/BulletRobotics/BoxStack.cpp Normal file
View File

@@ -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 <string>
#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);
}

21
examples/BulletRobotics/BoxStack.h Normal file
View File

@@ -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

108
examples/BulletRobotics/FixJointBoxes.cpp
View File

@@ -14,16 +14,26 @@
#include <vector> #include <vector>
#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" #include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
static btScalar numSolverIterations = 1000;
static btScalar solverId = 0;
class FixJointBoxes : public CommonExampleInterface class FixJointBoxes : public CommonExampleInterface
{ {
GUIHelperInterface* m_guiHelper; GUIHelperInterface* m_guiHelper;
b3RobotSimulatorClientAPI m_robotSim; b3RobotSimulatorClientAPI m_robotSim;
int m_options; int m_options;
b3RobotSimulatorSetPhysicsEngineParameters physicsArgs;
int solver;
const size_t numCubes = 30;
std::vector<int> cubeIds;
public: public:
FixJointBoxes(GUIHelperInterface* helper, int options) FixJointBoxes(GUIHelperInterface* helper, int options)
: m_guiHelper(helper), : m_guiHelper(helper),
m_options(options) m_options(options),
cubeIds(numCubes, 0),
solver(solverId)
{ {
} }
@@ -47,39 +57,85 @@ public:
m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0);
m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_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); b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0, i * 0.05, 1); b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args);
b3RobotJointInfo jointInfo; for (int i = 0; i < numCubes; i++)
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); 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.setCollisionFilterGroupMask(cubeIds[i], -1, 0, 0);
}
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() virtual void exitPhysics()
{ {
m_robotSim.disconnect(); m_robotSim.disconnect();
} }
void resetCubePosition()
{
for (int i = 0; i < numCubes; i++)
{
btVector3 pos (0, i * (btScalar)0.05, 1);
btQuaternion quar (0, 0, 0, 1);
m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar);
}
}
virtual void stepSimulation(float deltaTime) 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(); m_robotSim.stepSimulation();
} }
virtual void renderScene() virtual void renderScene()
@@ -102,11 +158,11 @@ public:
virtual void resetCamera() virtual void resetCamera()
{ {
float dist = 1; float dist = 1;
float pitch = -20; float pitch = -20;
float yaw = -30; float yaw = -30;
float targetPos[3] = {0, 0.2, 0.5}; float targetPos[3] = {0, 0.2, 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]);
} }
}; };

109
examples/BulletRobotics/JointLimit.cpp Normal file
View File

@@ -0,0 +1,109 @@
#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 <string>
#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;
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);
}

21
examples/BulletRobotics/JointLimit.h Normal file
View File

@@ -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

5
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -146,6 +146,11 @@ SET(ExtendedTutorialsSources
SET(BulletExampleBrowser_SRCS SET(BulletExampleBrowser_SRCS
../BulletRobotics/FixJointBoxes.cpp ../BulletRobotics/FixJointBoxes.cpp
../BulletRobotics/BoxStack.cpp
../BulletRobotics/JointLimit.cpp
# ../BulletRobotics/GraspBox.cpp
../BulletRobotics/FixJointBoxes.cpp
../TinyRenderer/geometry.cpp ../TinyRenderer/geometry.cpp
../TinyRenderer/model.cpp ../TinyRenderer/model.cpp
../TinyRenderer/tgaimage.cpp ../TinyRenderer/tgaimage.cpp

7
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -3,6 +3,10 @@
#include "../BlockSolver/BlockSolverExample.h" #include "../BlockSolver/BlockSolverExample.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include "EmptyExample.h" #include "EmptyExample.h"
#include "../BulletRobotics/BoxStack.h"
#include "../BulletRobotics/FixJointBoxes.h"
#include "../BulletRobotics/JointLimit.h"
// #include "../BulletRobotics/GraspBox.h"
#include "../BulletRobotics/FixJointBoxes.h" #include "../BulletRobotics/FixJointBoxes.h"
#include "../RenderingExamples/RenderInstancingDemo.h" #include "../RenderingExamples/RenderInstancingDemo.h"
#include "../RenderingExamples/CoordinateSystemDemo.h" #include "../RenderingExamples/CoordinateSystemDemo.h"
@@ -132,6 +136,9 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(0, "Bullet Robotics"), 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(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc),
ExampleEntry(0, "MultiBody"), ExampleEntry(0, "MultiBody"),

22
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -2417,3 +2417,25 @@ void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::stri
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); 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);
}

2
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -664,6 +664,8 @@ public:
return SHARED_MEMORY_MAGIC_NUMBER; return SHARED_MEMORY_MAGIC_NUMBER;
} }
void setAdditionalSearchPath(const std::string &path); 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 #endif //B3_ROBOT_SIMULATOR_CLIENT_API_NO_DIRECT_H