Skip to content

Commit

Permalink
Merge pull request #2550 from xhan0619/master
Browse files Browse the repository at this point in the history 
Group deformable constraint solves by islands
  • Loading branch information
erwincoumans authored Dec 21, 2019
2 parents d6bde92 + 6a8973d commit 830f0a9
Show file tree
Hide file tree
Showing 15 changed files with 806 additions and 544 deletions.
1 change: 1 addition & 0 deletions examples/ExampleBrowser/ExampleEntries.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -303,6 +303,7 @@ static ExampleEntry gDefaultExamples[] =
ExampleEntry(1, "Rolling friction", "Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_ROLLING_FRICTION),
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, "Grasp Deformable Cloth", "Grasp experiment with deformable cloth", GripperGraspExampleCreateFunc, eGRASP_DEFORMABLE_CLOTH),
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),
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
ExampleEntry(1, "Grasp Soft Body", "Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
Expand Down
131 changes: 126 additions & 5 deletions examples/RoboticsLearning/GripperGraspExample.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -294,6 +294,102 @@ class GripperGraspExample : public CommonExampleInterface
slider.m_maxVal = 1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
b3RobotSimulatorLoadFileResults results;
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);

if (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++)
{
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(m_gripperIndex, i, controlArgs);
}
}
}
{
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0, 0, -0.2);
args.m_startOrientation.setEulerZYX(0, 0, 0);
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
args.m_startPosition.setValue(0, 0, 5);
args.m_startOrientation.setValue(1, 0, 0, 1);
m_robotSim.loadSoftBody("bunny.obj", args);

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.createConstraint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
}

if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
{
m_robotSim.resetSimulation(RESET_USE_DEFORMABLE_WORLD);
{
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);
}

{
b3RobotSimulatorLoadFileResults results;
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
Expand Down Expand Up @@ -326,10 +422,19 @@ class GripperGraspExample : public CommonExampleInterface
m_robotSim.loadURDF("plane.urdf", args);
}
m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
args.m_startPosition.setValue(0, 0, 5);
args.m_startOrientation.setValue(1, 0, 0, 1);
m_robotSim.loadSoftBody("bunny.obj", args);

m_robotSim.setGravity(btVector3(0, 0, -10));
b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
args.m_springElasticStiffness = .1;
args.m_springDampingStiffness = .0004;
args.m_springBendingStiffness = 1;
args.m_frictionCoeff = 1;
args.m_useSelfCollision = false;
// args.m_useFaceContact = true;
args.m_useBendingSprings = true;
args.m_startPosition.setValue(0, 0, 0);
args.m_startOrientation.setValue(0, 0, 1, 1);
m_robotSim.loadDeformableBody("flat_napkin.obj", args);

b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
Expand Down Expand Up @@ -371,7 +476,8 @@ class GripperGraspExample : public CommonExampleInterface
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
}
m_robotSim.setNumSimulationSubSteps(8);
}

if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
{
Expand Down Expand Up @@ -462,6 +568,21 @@ class GripperGraspExample : public CommonExampleInterface
m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
}
}

if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
{
int fingerJointIndices[2] = {0, 1};
double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
double maxTorqueValues[2] = {250.0, 50.0};
for (int i = 0; i < 2; i++)
{
b3RobotSimulatorJointMotorArgs 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);
}
}

if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
{
Expand Down
1 change: 1 addition & 0 deletions examples/RoboticsLearning/GripperGraspExample.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ enum GripperGraspExampleOptions
eONE_MOTOR_GRASP = 4,
eGRASP_SOFT_BODY = 8,
eSOFTBODY_MULTIBODY_COUPLING = 16,
eGRASP_DEFORMABLE_CLOTH = 32,
};

class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
Expand Down
42 changes: 42 additions & 0 deletions examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,19 @@ void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
}

void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle);
b3InitResetSimulationSetFlags(command, flag);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}

bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
{
if (!isConnected())
Expand Down Expand Up @@ -1151,6 +1164,35 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}

void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args)
{
if (!isConnected())
{
b3Warning("Not connected");
return;
}

b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
b3LoadSoftBodySetScale(command, args.m_scale);
b3LoadSoftBodySetMass(command, args.m_mass);
b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
if (args.m_NeoHookeanMu > 0)
{
b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
}
if (args.m_springElasticStiffness > 0)
{
b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
}
b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision);
b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}

void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
{
mouseEventsData->m_numMouseEvents = 0;
Expand Down
62 changes: 62 additions & 0 deletions examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,64 @@ struct b3RobotSimulatorLoadSoftBodyArgs
}
};


struct b3RobotSimulatorLoadDeformableBodyArgs
{
btVector3 m_startPosition;
btQuaternion m_startOrientation;
double m_scale;
double m_mass;
double m_collisionMargin;
double m_springElasticStiffness;
double m_springDampingStiffness;
double m_springBendingStiffness;
double m_NeoHookeanMu;
double m_NeoHookeanLambda;
double m_NeoHookeanDamping;
bool m_useSelfCollision;
bool m_useFaceContact;
bool m_useBendingSprings;
double m_frictionCoeff;

b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin)
: m_startPosition(startPos),
m_startOrientation(startOrn),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin),
m_springElasticStiffness(-1),
m_springDampingStiffness(-1),
m_springBendingStiffness(-1),
m_NeoHookeanMu(-1),
m_NeoHookeanDamping(-1),
m_useSelfCollision(false),
m_useFaceContact(false),
m_useBendingSprings(false),
m_frictionCoeff(0)
{
}

b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
{
b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
}

b3RobotSimulatorLoadDeformableBodyArgs()
{
b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
}

b3RobotSimulatorLoadDeformableBodyArgs(double scale, double mass, double collisionMargin)
: m_startPosition(btVector3(0, 0, 0)),
m_startOrientation(btQuaternion(0, 0, 0, 1)),
m_scale(scale),
m_mass(mass),
m_collisionMargin(collisionMargin)
{
}
};


struct b3RobotSimulatorLoadFileResults
{
btAlignedObjectArray<int> m_uniqueObjectIds;
Expand Down Expand Up @@ -534,6 +592,8 @@ class b3RobotSimulatorClientAPI_NoDirect
void syncBodies();

void resetSimulation();

void resetSimulation(int flag);

btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw);
btVector3 getEulerFromQuaternion(const btQuaternion &quat);
Expand Down Expand Up @@ -685,6 +745,8 @@ class b3RobotSimulatorClientAPI_NoDirect
int getConstraintUniqueId(int serialIndex);

void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);

void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args);

virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
virtual struct GUIHelperInterface *getGuiHelper();
Expand Down
6 changes: 6 additions & 0 deletions src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
btDispatcher* m_dispatcher;

btAlignedObjectArray<btCollisionObject*> m_bodies;
btAlignedObjectArray<btCollisionObject*> m_softBodies;
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
Expand Down Expand Up @@ -194,6 +195,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
{
m_bodies.push_back(bodies[i]);
}
else
{
m_softBodies.push_back(bodies[i]);
}
}
for (i = 0; i < numManifolds; i++)
m_manifolds.push_back(manifolds[i]);
Expand Down Expand Up @@ -231,6 +236,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_softBodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
Expand Down
46 changes: 46 additions & 0 deletions src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
//
// DeformableBodyInplaceSolverIslandCallback.h
// BulletSoftBody
//
// Created by Xuchen Han on 12/16/19.
//

#ifndef DeformableBodyInplaceSolverIslandCallback_h
#define DeformableBodyInplaceSolverIslandCallback_h

struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback
{
btDeformableMultiBodyConstraintSolver* m_deformableSolver;

DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
btDispatcher* dispatcher)
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
{
}


virtual void processConstraints(int islandId=-1)
{
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;

//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());

m_deformableSolver->solveDeformableBodyGroup(bodies, m_bodies.size(), softBodies, m_softBodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
{
m_deformableSolver->m_analyticsData.m_islandId = islandId;
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
}
m_bodies.resize(0);
m_softBodies.resize(0);
m_manifolds.resize(0);
m_constraints.resize(0);
m_multiBodyConstraints.resize(0);
}
};

#endif /* DeformableBodyInplaceSolverIslandCallback_h */
Loading

0 comments on commit 830f0a9

Please sign in to comment.