AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
Erwin Coumans 2019-03-13 09:52:38 -07:00
commit 1161a04c56
10 changed files with 475 additions and 26 deletions
Show Stats 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 "../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<int> cubeIds;
public:
FixJointBoxes(GUIHelperInterface* helper, int options)
: 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_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);
b3RobotSimulatorLoadUrdfFileArgs args;
b3RobotSimulatorChangeDynamicsArgs dynamicsArgs;
b3RobotJointInfo 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.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()
{
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)
{
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,11 +158,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]);
}
};

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
../BulletRobotics/FixJointBoxes.cpp
../BulletRobotics/BoxStack.cpp
../BulletRobotics/JointLimit.cpp
# ../BulletRobotics/GraspBox.cpp
../BulletRobotics/FixJointBoxes.cpp
../TinyRenderer/geometry.cpp
../TinyRenderer/model.cpp
../TinyRenderer/tgaimage.cpp

7
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -3,6 +3,10 @@
#include "../BlockSolver/BlockSolverExample.h"
#include "LinearMath/btAlignedObjectArray.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 "../RenderingExamples/RenderInstancingDemo.h"
#include "../RenderingExamples/CoordinateSystemDemo.h"
@ -132,6 +136,9 @@ 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, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc),
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);
}
}
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;
}
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