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

Load the deformable object through obj file. Modify the demo for two way coupling between multibody and softbody.

Browse Source
This commit is contained in:
yunfeibai
2018-01-07 19:06:09 -08:00
parent 577ee86012
commit e3e9546960
5 changed files with 1574 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1360
data/bunny.obj Normal file
View File

File diff suppressed because it is too large Load Diff

4
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -279,10 +279,10 @@ 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),
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
ExampleEntry(1,"Softbody Multibody Coupling","Two way coupling between soft body and multibody experiment", GripperGraspExampleCreateFunc, eSOFTBODY_MULTIBODY_COUPLING),
#endif //USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifdef ENABLE_LUA
ExampleEntry(1,"Lua Script", "Create the dynamics world, collision shapes and rigid bodies using Lua scripting",

131
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -26,6 +26,11 @@ class GripperGraspExample : public CommonExampleInterface
b3RobotSimulatorClientAPI m_robotSim;
int m_options;
int m_gripperIndex;
double m_time;
b3Vector3 m_targetPos;
b3Vector3 m_worldPos;
b3Vector4 m_targetOri;
b3Vector4 m_worldOri;
b3AlignedObjectArray<int> m_movingInstances;
enum
@@ -379,11 +384,11 @@ public:
{
{
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0,1.0,2.0);
args.m_startOrientation.setEulerZYX(0,0,1.57);
args.m_forceOverrideFixedBase = false;
args.m_startPosition.setValue(-0.5,0,0.1);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args);
int kukaId = m_robotSim.loadURDF("kuka_iiwa/model.urdf", args);
int numJoints = m_robotSim.getNumJoints(kukaId);
b3Printf("numJoints = %d",numJoints);
@@ -394,7 +399,7 @@ public:
m_robotSim.getJointInfo(kukaId,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
controlArgs.m_maxTorqueValue = 500.0;
m_robotSim.setJointMotorControl(kukaId,i,controlArgs);
}
}
@@ -407,7 +412,7 @@ public:
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.loadBunny(0.5,10.0,0.1);
m_robotSim.loadBunny(0.3,10.0,0.1);
}
}
virtual void exitPhysics()
@@ -468,6 +473,120 @@ public:
}
}
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0)
{
float dt = deltaTime;
btClamp(dt,0.0001f,0.01f);
m_time+=dt;
m_targetPos.setValue(0, 0, 0.5+0.2*b3Cos( m_time));
m_targetOri.setValue(0, 1.0, 0, 0);
int numJoints = m_robotSim.getNumJoints(0);
if (numJoints==7)
{
double q_current[7]={0,0,0,0,0,0,0};
b3JointStates2 jointStates;
if (m_robotSim.getJointStates(0,jointStates))
{
//skip the base positions (7 values)
b3Assert(7+numJoints == jointStates.m_numDegreeOfFreedomQ);
for (int i=0;i<numJoints;i++)
{
q_current[i] = jointStates.m_actualStateQ[i+7];
}
}
// compute body position and orientation
b3LinkState linkState;
m_robotSim.getLinkState(0, 6, &linkState);
m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);
b3Vector3DoubleData targetPosDataOut;
m_targetPos.serializeDouble(targetPosDataOut);
b3Vector3DoubleData worldPosDataOut;
m_worldPos.serializeDouble(worldPosDataOut);
b3Vector3DoubleData targetOriDataOut;
m_targetOri.serializeDouble(targetOriDataOut);
b3Vector3DoubleData worldOriDataOut;
m_worldOri.serializeDouble(worldOriDataOut);
b3RobotSimulatorInverseKinematicArgs ikargs;
b3RobotSimulatorInverseKinematicsResults ikresults;
ikargs.m_bodyUniqueId = 0;
// ikargs.m_currentJointPositions = q_current;
// ikargs.m_numPositions = 7;
ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0];
ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION/* + B3_HAS_NULL_SPACE_VELOCITY*/;
ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
ikargs.m_endEffectorLinkIndex = 6;
// Settings based on default KUKA arm setting
ikargs.m_lowerLimits.resize(numJoints);
ikargs.m_upperLimits.resize(numJoints);
ikargs.m_jointRanges.resize(numJoints);
ikargs.m_restPoses.resize(numJoints);
ikargs.m_lowerLimits[0] = -2.32;
ikargs.m_lowerLimits[1] = -1.6;
ikargs.m_lowerLimits[2] = -2.32;
ikargs.m_lowerLimits[3] = -1.6;
ikargs.m_lowerLimits[4] = -2.32;
ikargs.m_lowerLimits[5] = -1.6;
ikargs.m_lowerLimits[6] = -2.4;
ikargs.m_upperLimits[0] = 2.32;
ikargs.m_upperLimits[1] = 1.6;
ikargs.m_upperLimits[2] = 2.32;
ikargs.m_upperLimits[3] = 1.6;
ikargs.m_upperLimits[4] = 2.32;
ikargs.m_upperLimits[5] = 1.6;
ikargs.m_upperLimits[6] = 2.4;
ikargs.m_jointRanges[0] = 5.8;
ikargs.m_jointRanges[1] = 4;
ikargs.m_jointRanges[2] = 5.8;
ikargs.m_jointRanges[3] = 4;
ikargs.m_jointRanges[4] = 5.8;
ikargs.m_jointRanges[5] = 4;
ikargs.m_jointRanges[6] = 6;
ikargs.m_restPoses[0] = 0;
ikargs.m_restPoses[1] = 0;
ikargs.m_restPoses[2] = 0;
ikargs.m_restPoses[3] = SIMD_HALF_PI;
ikargs.m_restPoses[4] = 0;
ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
ikargs.m_restPoses[6] = 0;
ikargs.m_numDegreeOfFreedom = numJoints;
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
{
//copy the IK result to the desired state of the motor/actuator
for (int i=0;i<numJoints;i++)
{
b3RobotSimulatorJointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
t.m_maxTorqueValue = 100.0;
t.m_kp= 1.0;
t.m_targetVelocity = 0;
t.m_kd = 1.0;
m_robotSim.setJointMotorControl(0,i,t);
}
}
}
}
m_robotSim.stepSimulation();
}
virtual void renderScene()

83
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -50,7 +50,7 @@
#include "../TinyAudio/b3SoundEngine.h"
#endif
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftBodySolvers.h"
#include "BulletSoftBody/btSoftBodyHelpers.h"
@@ -169,6 +169,9 @@ struct InternalBodyData
{
btMultiBody* m_multiBody;
btRigidBody* m_rigidBody;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btSoftBody* m_softBody;
#endif
int m_testData;
std::string m_bodyName;
@@ -191,6 +194,9 @@ struct InternalBodyData
{
m_multiBody=0;
m_rigidBody=0;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_softBody = 0;
#endif
m_testData=0;
m_bodyName="";
m_rootLocalInertialFrame.setIdentity();
@@ -1519,7 +1525,7 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBodyConstraintSolver* m_solver;
btDefaultCollisionConfiguration* m_collisionConfiguration;
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
btSoftBodySolver* m_softbodySolver;
btSoftBodyWorldInfo m_softBodyWorldInfo;
@@ -2166,7 +2172,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
//m_collisionConfiguration->setConvexConvexMultipointIterations();
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
#else
m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
@@ -2189,7 +2195,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_solver = new btMultiBodyConstraintSolver;
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_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);
@@ -2354,6 +2360,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
m_data->m_dynamicsWorld->removeMultiBody(mb);
delete mb;
}
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
for (i=m_data->m_dynamicsWorld->getSoftBodyArray().size()-1;i>=0;i--)
{
btSoftBody* sb = m_data->m_dynamicsWorld->getSoftBodyArray()[i];
m_data->m_dynamicsWorld->removeSoftBody(sb);
delete sb;
}
#endif
}
for (int i=0;i<constraints.size();i++)
@@ -5650,7 +5664,7 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
{
serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
bool hasStatus = true;
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
double scale = 0.1;
double mass = 0.1;
double collisionMargin = 0.02;
@@ -5666,6 +5680,7 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
{
collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin;
}
m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2;
m_data->m_softBodyWorldInfo.water_density = 0;
m_data->m_softBodyWorldInfo.water_offset = 0;
@@ -5673,7 +5688,7 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
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();
@@ -5690,6 +5705,60 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
psb->getCollisionShape()->setMargin(collisionMargin);
m_data->m_dynamicsWorld->addSoftBody(psb);
*/
{
char relativeFileName[1024];
char pathPrefix[1024];
pathPrefix[0] = 0;
if (b3ResourcePath::findResourcePath("bunny.obj", relativeFileName, 1024))
{
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
const std::string& error_message_prefix="";
std::string out_found_filename;
int out_type;
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
std::vector<tinyobj::shape_t> shapes;
std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
if (shapes.size()>0)
{
const tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices;
btAlignedObjectArray<int> indices;
for (int i=0;i<shape.mesh.positions.size();i++)
{
vertices.push_back(shape.mesh.positions[i]);
}
for (int i=0;i<shape.mesh.indices.size();i++)
{
indices.push_back(shape.mesh.indices[i]);
}
int numTris = indices.size()/3;
if (numTris>0)
{
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,&vertices[0],&indices[0],numTris);
btSoftBody::Material* pm=psb->appendMaterial();
pm->m_kLST = 0.5;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2,pm);
psb->m_cfg.piterations = 20;
psb->m_cfg.kDF = 0.5;
psb->randomizeConstraints();
psb->rotate(btQuaternion(0.70711,0,0,0.70711));
psb->translate(btVector3(-0.05,0,1.0));
psb->scale(btVector3(scale,scale,scale));
psb->setTotalMass(mass,true);
psb->getCollisionShape()->setMargin(collisionMargin);
m_data->m_dynamicsWorld->addSoftBody(psb);
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
bodyHandle->m_softBody = psb;
serverStatusOut.m_loadBunnyResultArguments.m_objectUniqueId = bodyUniqueId;
}
}
}
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
#endif
return hasStatus;
@@ -8758,7 +8827,7 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
}
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_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];

6
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -451,6 +451,11 @@ struct LoadBunnyArgs
double m_collisionMargin;
};
struct b3LoadBunnyResultArgs
{
int m_objectUniqueId;
};
struct RequestActualStateArgs
{
int m_bodyUniqueId;
@@ -1068,6 +1073,7 @@ struct SharedMemoryStatus
struct b3CustomCommandResultArgs m_customCommandResultArgs;
struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
struct b3StateSerializationArguments m_saveStateResultArgs;
struct b3LoadBunnyResultArgs m_loadBunnyResultArguments;
};
};