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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
erwin coumans
2016-10-19 07:43:46 -07:00
parent 3b8090fcc1 6584bd8a97
commit eda400d14c
18 changed files with 730 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -267,6 +267,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
#ifdef ENABLE_LUA

163
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -217,7 +217,6 @@ public:
if ((m_options & eONE_MOTOR_GRASP)!=0)
{
{
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
slider.m_minVal=-2;
@@ -241,7 +240,6 @@ public:
args.m_useMultiBody = true;
m_robotSim.loadFile(args, results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
@@ -287,48 +285,6 @@ public:
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
/*
b3JointInfo sliderJoint1;
sliderJoint1.m_parentFrame[0] = 0;
sliderJoint1.m_parentFrame[1] = 0;
sliderJoint1.m_parentFrame[2] = 0.06;
sliderJoint1.m_parentFrame[3] = 0;
sliderJoint1.m_parentFrame[4] = 0;
sliderJoint1.m_parentFrame[5] = 0;
sliderJoint1.m_parentFrame[6] = 1.0;
sliderJoint1.m_childFrame[0] = 0;
sliderJoint1.m_childFrame[1] = 0;
sliderJoint1.m_childFrame[2] = 0;
sliderJoint1.m_childFrame[3] = 0;
sliderJoint1.m_childFrame[4] = 0;
sliderJoint1.m_childFrame[5] = 0;
sliderJoint1.m_childFrame[6] = 1.0;
sliderJoint1.m_jointAxis[0] = 1.0;
sliderJoint1.m_jointAxis[1] = 0.0;
sliderJoint1.m_jointAxis[2] = 0.0;
sliderJoint1.m_jointType = ePrismaticType;
b3JointInfo sliderJoint2;
sliderJoint2.m_parentFrame[0] = 0;
sliderJoint2.m_parentFrame[1] = 0;
sliderJoint2.m_parentFrame[2] = 0.06;
sliderJoint2.m_parentFrame[3] = 0;
sliderJoint2.m_parentFrame[4] = 0;
sliderJoint2.m_parentFrame[5] = 0;
sliderJoint2.m_parentFrame[6] = 1.0;
sliderJoint2.m_childFrame[0] = 0;
sliderJoint2.m_childFrame[1] = 0;
sliderJoint2.m_childFrame[2] = 0;
sliderJoint2.m_childFrame[3] = 0;
sliderJoint2.m_childFrame[4] = 0;
sliderJoint2.m_childFrame[5] = 1.0;
sliderJoint2.m_childFrame[6] = 0;
sliderJoint2.m_jointAxis[0] = 1.0;
sliderJoint2.m_jointAxis[1] = 0.0;
sliderJoint2.m_jointAxis[2] = 0.0;
sliderJoint2.m_jointType = ePrismaticType;
m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1);
m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2);
*/
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
revoluteJoint1.m_parentFrame[1] = 0;
@@ -371,6 +327,110 @@ public:
m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
}
if ((m_options & eGRASP_SOFT_BODY)!=0)
{
{
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
slider.m_minVal=-2;
slider.m_maxVal=2;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
);
slider.m_minVal=-1;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
args.m_fileType = B3_SDF_FILE;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
{
m_gripperIndex = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
for (int i=0;i<8;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
}
}
}
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.loadBunny();
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
revoluteJoint1.m_parentFrame[1] = 0;
revoluteJoint1.m_parentFrame[2] = 0.02;
revoluteJoint1.m_parentFrame[3] = 0;
revoluteJoint1.m_parentFrame[4] = 0;
revoluteJoint1.m_parentFrame[5] = 0;
revoluteJoint1.m_parentFrame[6] = 1.0;
revoluteJoint1.m_childFrame[0] = 0;
revoluteJoint1.m_childFrame[1] = 0;
revoluteJoint1.m_childFrame[2] = 0;
revoluteJoint1.m_childFrame[3] = 0;
revoluteJoint1.m_childFrame[4] = 0;
revoluteJoint1.m_childFrame[5] = 0;
revoluteJoint1.m_childFrame[6] = 1.0;
revoluteJoint1.m_jointAxis[0] = 1.0;
revoluteJoint1.m_jointAxis[1] = 0.0;
revoluteJoint1.m_jointAxis[2] = 0.0;
revoluteJoint1.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint2;
revoluteJoint2.m_parentFrame[0] = 0.055;
revoluteJoint2.m_parentFrame[1] = 0;
revoluteJoint2.m_parentFrame[2] = 0.02;
revoluteJoint2.m_parentFrame[3] = 0;
revoluteJoint2.m_parentFrame[4] = 0;
revoluteJoint2.m_parentFrame[5] = 0;
revoluteJoint2.m_parentFrame[6] = 1.0;
revoluteJoint2.m_childFrame[0] = 0;
revoluteJoint2.m_childFrame[1] = 0;
revoluteJoint2.m_childFrame[2] = 0;
revoluteJoint2.m_childFrame[3] = 0;
revoluteJoint2.m_childFrame[4] = 0;
revoluteJoint2.m_childFrame[5] = 0;
revoluteJoint2.m_childFrame[6] = 1.0;
revoluteJoint2.m_jointAxis[0] = 1.0;
revoluteJoint2.m_jointAxis[1] = 0.0;
revoluteJoint2.m_jointAxis[2] = 0.0;
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2);
}
}
virtual void exitPhysics()
{
@@ -414,6 +474,21 @@ public:
}
}
if ((m_options & eGRASP_SOFT_BODY)!=0)
{
int fingerJointIndices[2]={0,1};
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
};
double maxTorqueValues[2]={50.0,50.0};
for (int i=0;i<2;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
}
}
m_robotSim.stepSimulation();
}

1
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -21,6 +21,7 @@ enum GripperGraspExampleOptions
eGRIPPER_GRASP=1,
eTWO_POINT_GRASP=2,
eONE_MOTOR_GRASP=4,
eGRASP_SOFT_BODY=8,
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

8
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -961,10 +961,12 @@ void b3RobotSimAPI::renderScene()
}
}
if (m_data->m_clientServerDirect)
{
m_data->m_clientServerDirect->renderScene();
}
m_data->m_physicsServer.renderScene();
}
@@ -996,4 +998,10 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP
worldOrientation[2] = linkState.m_worldOrientation[2];
worldOrientation[3] = linkState.m_worldOrientation[3];
}
}
void b3RobotSimAPI::loadBunny()
{
b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient);
b3SubmitClientCommand(m_data->m_physicsClient, command);
}

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -164,6 +164,8 @@ public:
void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
void loadBunny();
};
#endif //B3_ROBOT_SIM_API_H

14
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -75,6 +75,20 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_BUNNY;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -222,6 +222,8 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
#ifdef __cplusplus
}

116
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -5,7 +5,6 @@
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
#include "TinyRendererVisualShapeConverter.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -27,6 +26,16 @@
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommands.h"
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftBodySolvers.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
#include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h"
#include "../SoftDemo/BunnyMesh.h"
#else
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#endif
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
btVector3 gLastPickPos(0, 0, 0);
@@ -431,9 +440,17 @@ struct PhysicsServerCommandProcessorInternalData
btCollisionDispatcher* m_dispatcher;
btMultiBodyConstraintSolver* m_solver;
btDefaultCollisionConfiguration* m_collisionConfiguration;
btMultiBodyDynamicsWorld* m_dynamicsWorld;
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
btSoftBodySolver* m_softbodySolver;
btSoftBodyWorldInfo m_softBodyWorldInfo;
#else
btMultiBodyDynamicsWorld* m_dynamicsWorld;
#endif
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
@@ -608,28 +625,35 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration);
m_data->m_broadphase = new btDbvtBroadphase();
m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
///collision configuration contains default setup for memory, collision setup
//m_collisionConfiguration->setConvexConvexMultipointIterations();
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
#else
m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
#endif
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration);
m_data->m_broadphase = new btDbvtBroadphase();
m_data->m_solver = new btMultiBodyConstraintSolver;
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#else
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
}
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -1599,6 +1623,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
case CMD_LOAD_BUNNY:
{
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2;
m_data->m_softBodyWorldInfo.water_density = 0;
m_data->m_softBodyWorldInfo.water_offset = 0;
m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0);
m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10);
m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase;
m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize();
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES);
btSoftBody::Material* pm=psb->appendMaterial();
pm->m_kLST = 1.0;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2,pm);
psb->m_cfg.piterations = 2;
psb->m_cfg.kDF = 0.5;
psb->randomizeConstraints();
psb->rotate(btQuaternion(0.70711,0,0,0.70711));
psb->translate(btVector3(0,0,3.0));
psb->scale(btVector3(0.1,0.1,0.1));
psb->setTotalMass(1,true);
psb->getCollisionShape()->setMargin(0.01);
m_data->m_dynamicsWorld->addSoftBody(psb);
#endif
break;
}
case CMD_CREATE_SENSOR:
@@ -2973,7 +3027,17 @@ void PhysicsServerCommandProcessor::renderScene()
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
}
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
for ( int i=0;i<m_data->m_dynamicsWorld->getSoftBodyArray().size();i++)
{
btSoftBody* psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags());
}
}
#endif
}
void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -7,6 +7,7 @@ enum EnumSharedMemoryClientCommand
{
CMD_LOAD_SDF,
CMD_LOAD_URDF,
CMD_LOAD_BUNNY,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,

3
examples/SoftDemo/SoftDemo.cpp
View File

@@ -1017,7 +1017,7 @@ static void Init_Bunny(SoftDemo* pdemo)
psb->m_cfg.kDF = 0.5;
psb->randomizeConstraints();
psb->scale(btVector3(6,6,6));
psb->setTotalMass(100,true);
psb->setTotalMass(100,true);
pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
pdemo->m_cutting=true;
@@ -2167,7 +2167,6 @@ void SoftDemo::initPhysics()
m_collisionShapes.push_back(cylinderCompound);
m_dispatcher=0;
///register some softbody collision algorithms on top of the default btDefaultCollisionConfiguration