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

Add grasp box example and fixjoint boxes example.

Browse Source 
tmp

gripper is working?

fix joint example
This commit is contained in:
Chuyuan Fu
2019-03-05 15:44:06 -08:00
parent 9141caf67e
commit e818452494
6 changed files with 300 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
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,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<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;
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]);
}
};

186
examples/BulletRobotics/GraspBox.cpp Normal file
View File

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

21
examples/BulletRobotics/GraspBox.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 GRASP_BOX_EXAMPLE_H
#define GRASP_BOX_EXAMPLE_H
class CommonExampleInterface* GraspBoxCreateFunc(struct CommonExampleOptions& options);
#endif //GRASP_BOX_EXAMPLE_H

13
examples/BulletRobotics/JointLimit.cpp
View File

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

2
examples/ExampleBrowser/CMakeLists.txt
View File

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

5
examples/ExampleBrowser/ExampleEntries.cpp
View File

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