Skip to content

Commit 830f0a9

Browse files
author
erwincoumans
authored
Merge pull request #2550 from xhan0619/master
Group deformable constraint solves by islands
2 parents d6bde92 + 6a8973d commit 830f0a9

15 files changed

+806
-544
lines changed

‎examples/ExampleBrowser/ExampleEntries.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -303,6 +303,7 @@ static ExampleEntry gDefaultExamples[] =
303303
ExampleEntry(1, "Rolling friction", "Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_ROLLING_FRICTION),
304304
ExampleEntry(1, "Gripper Grasp", "Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc, eGRIPPER_GRASP),
305305
ExampleEntry(1, "Two Point Grasp", "Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
306+
ExampleEntry(1, "Grasp Deformable Cloth", "Grasp experiment with deformable cloth", GripperGraspExampleCreateFunc, eGRASP_DEFORMABLE_CLOTH),
306307
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),
307308
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
308309
ExampleEntry(1, "Grasp Soft Body", "Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),

‎examples/RoboticsLearning/GripperGraspExample.cpp‎

Lines changed: 126 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -294,6 +294,102 @@ class GripperGraspExample : public CommonExampleInterface
294294
slider.m_maxVal = 1;
295295
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
296296
}
297+
{
298+
b3RobotSimulatorLoadFileResults results;
299+
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
300+
301+
if (results.m_uniqueObjectIds.size() == 1)
302+
{
303+
m_gripperIndex = results.m_uniqueObjectIds[0];
304+
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
305+
b3Printf("numJoints = %d", numJoints);
306+
307+
for (int i = 0; i < numJoints; i++)
308+
{
309+
b3JointInfo jointInfo;
310+
m_robotSim.getJointInfo(m_gripperIndex, i, &jointInfo);
311+
b3Printf("joint[%d].m_jointName=%s", i, jointInfo.m_jointName);
312+
}
313+
314+
for (int i = 0; i < 8; i++)
315+
{
316+
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
317+
controlArgs.m_maxTorqueValue = 0.0;
318+
m_robotSim.setJointMotorControl(m_gripperIndex, i, controlArgs);
319+
}
320+
}
321+
}
322+
{
323+
b3RobotSimulatorLoadUrdfFileArgs args;
324+
args.m_startPosition.setValue(0, 0, -0.2);
325+
args.m_startOrientation.setEulerZYX(0, 0, 0);
326+
m_robotSim.loadURDF("plane.urdf", args);
327+
}
328+
m_robotSim.setGravity(btVector3(0, 0, -10));
329+
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
330+
args.m_startPosition.setValue(0, 0, 5);
331+
args.m_startOrientation.setValue(1, 0, 0, 1);
332+
m_robotSim.loadSoftBody("bunny.obj", args);
333+
334+
b3JointInfo revoluteJoint1;
335+
revoluteJoint1.m_parentFrame[0] = -0.055;
336+
revoluteJoint1.m_parentFrame[1] = 0;
337+
revoluteJoint1.m_parentFrame[2] = 0.02;
338+
revoluteJoint1.m_parentFrame[3] = 0;
339+
revoluteJoint1.m_parentFrame[4] = 0;
340+
revoluteJoint1.m_parentFrame[5] = 0;
341+
revoluteJoint1.m_parentFrame[6] = 1.0;
342+
revoluteJoint1.m_childFrame[0] = 0;
343+
revoluteJoint1.m_childFrame[1] = 0;
344+
revoluteJoint1.m_childFrame[2] = 0;
345+
revoluteJoint1.m_childFrame[3] = 0;
346+
revoluteJoint1.m_childFrame[4] = 0;
347+
revoluteJoint1.m_childFrame[5] = 0;
348+
revoluteJoint1.m_childFrame[6] = 1.0;
349+
revoluteJoint1.m_jointAxis[0] = 1.0;
350+
revoluteJoint1.m_jointAxis[1] = 0.0;
351+
revoluteJoint1.m_jointAxis[2] = 0.0;
352+
revoluteJoint1.m_jointType = ePoint2PointType;
353+
b3JointInfo revoluteJoint2;
354+
revoluteJoint2.m_parentFrame[0] = 0.055;
355+
revoluteJoint2.m_parentFrame[1] = 0;
356+
revoluteJoint2.m_parentFrame[2] = 0.02;
357+
revoluteJoint2.m_parentFrame[3] = 0;
358+
revoluteJoint2.m_parentFrame[4] = 0;
359+
revoluteJoint2.m_parentFrame[5] = 0;
360+
revoluteJoint2.m_parentFrame[6] = 1.0;
361+
revoluteJoint2.m_childFrame[0] = 0;
362+
revoluteJoint2.m_childFrame[1] = 0;
363+
revoluteJoint2.m_childFrame[2] = 0;
364+
revoluteJoint2.m_childFrame[3] = 0;
365+
revoluteJoint2.m_childFrame[4] = 0;
366+
revoluteJoint2.m_childFrame[5] = 0;
367+
revoluteJoint2.m_childFrame[6] = 1.0;
368+
revoluteJoint2.m_jointAxis[0] = 1.0;
369+
revoluteJoint2.m_jointAxis[1] = 0.0;
370+
revoluteJoint2.m_jointAxis[2] = 0.0;
371+
revoluteJoint2.m_jointType = ePoint2PointType;
372+
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
373+
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
374+
}
375+
376+
if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
377+
{
378+
m_robotSim.resetSimulation(RESET_USE_DEFORMABLE_WORLD);
379+
{
380+
SliderParams slider("Vertical velocity", &sGripperVerticalVelocity);
381+
slider.m_minVal = -2;
382+
slider.m_maxVal = 2;
383+
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
384+
}
385+
386+
{
387+
SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
388+
slider.m_minVal = -1;
389+
slider.m_maxVal = 1;
390+
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
391+
}
392+
297393
{
298394
b3RobotSimulatorLoadFileResults results;
299395
m_robotSim.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", results);
@@ -326,10 +422,19 @@ class GripperGraspExample : public CommonExampleInterface
326422
m_robotSim.loadURDF("plane.urdf", args);
327423
}
328424
m_robotSim.setGravity(btVector3(0, 0, -10));
329-
b3RobotSimulatorLoadSoftBodyArgs args(0.1, 1, 0.02);
330-
args.m_startPosition.setValue(0, 0, 5);
331-
args.m_startOrientation.setValue(1, 0, 0, 1);
332-
m_robotSim.loadSoftBody("bunny.obj", args);
425+
426+
m_robotSim.setGravity(btVector3(0, 0, -10));
427+
b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
428+
args.m_springElasticStiffness = .1;
429+
args.m_springDampingStiffness = .0004;
430+
args.m_springBendingStiffness = 1;
431+
args.m_frictionCoeff = 1;
432+
args.m_useSelfCollision = false;
433+
// args.m_useFaceContact = true;
434+
args.m_useBendingSprings = true;
435+
args.m_startPosition.setValue(0, 0, 0);
436+
args.m_startOrientation.setValue(0, 0, 1, 1);
437+
m_robotSim.loadDeformableBody("flat_napkin.obj", args);
333438

334439
b3JointInfo revoluteJoint1;
335440
revoluteJoint1.m_parentFrame[0] = -0.055;
@@ -371,7 +476,8 @@ class GripperGraspExample : public CommonExampleInterface
371476
revoluteJoint2.m_jointType = ePoint2PointType;
372477
m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
373478
m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
374-
}
479+
m_robotSim.setNumSimulationSubSteps(8);
480+
}
375481

376482
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
377483
{
@@ -462,6 +568,21 @@ class GripperGraspExample : public CommonExampleInterface
462568
m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
463569
}
464570
}
571+
572+
if ((m_options & eGRASP_DEFORMABLE_CLOTH) != 0)
573+
{
574+
int fingerJointIndices[2] = {0, 1};
575+
double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
576+
double maxTorqueValues[2] = {250.0, 50.0};
577+
for (int i = 0; i < 2; i++)
578+
{
579+
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
580+
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
581+
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
582+
controlArgs.m_kd = 1.;
583+
m_robotSim.setJointMotorControl(m_gripperIndex, fingerJointIndices[i], controlArgs);
584+
}
585+
}
465586

466587
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)
467588
{

‎examples/RoboticsLearning/GripperGraspExample.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ enum GripperGraspExampleOptions
2323
eONE_MOTOR_GRASP = 4,
2424
eGRASP_SOFT_BODY = 8,
2525
eSOFTBODY_MULTIBODY_COUPLING = 16,
26+
eGRASP_DEFORMABLE_CLOTH = 32,
2627
};
2728

2829
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

‎examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,19 @@ void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
8989
m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
9090
}
9191

92+
void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag)
93+
{
94+
if (!isConnected())
95+
{
96+
b3Warning("Not connected");
97+
return;
98+
}
99+
b3SharedMemoryStatusHandle statusHandle;
100+
b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle);
101+
b3InitResetSimulationSetFlags(command, flag);
102+
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
103+
}
104+
92105
bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
93106
{
94107
if (!isConnected())
@@ -1151,6 +1164,35 @@ void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileNam
11511164
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
11521165
}
11531166

1167+
void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args)
1168+
{
1169+
if (!isConnected())
1170+
{
1171+
b3Warning("Not connected");
1172+
return;
1173+
}
1174+
1175+
b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
1176+
b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
1177+
b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
1178+
b3LoadSoftBodySetScale(command, args.m_scale);
1179+
b3LoadSoftBodySetMass(command, args.m_mass);
1180+
b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
1181+
if (args.m_NeoHookeanMu > 0)
1182+
{
1183+
b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
1184+
}
1185+
if (args.m_springElasticStiffness > 0)
1186+
{
1187+
b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
1188+
}
1189+
b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision);
1190+
b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
1191+
b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
1192+
b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);
1193+
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
1194+
}
1195+
11541196
void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
11551197
{
11561198
mouseEventsData->m_numMouseEvents = 0;

‎examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -89,6 +89,64 @@ struct b3RobotSimulatorLoadSoftBodyArgs
8989
}
9090
};
9191

92+
93+
struct b3RobotSimulatorLoadDeformableBodyArgs
94+
{
95+
btVector3 m_startPosition;
96+
btQuaternion m_startOrientation;
97+
double m_scale;
98+
double m_mass;
99+
double m_collisionMargin;
100+
double m_springElasticStiffness;
101+
double m_springDampingStiffness;
102+
double m_springBendingStiffness;
103+
double m_NeoHookeanMu;
104+
double m_NeoHookeanLambda;
105+
double m_NeoHookeanDamping;
106+
bool m_useSelfCollision;
107+
bool m_useFaceContact;
108+
bool m_useBendingSprings;
109+
double m_frictionCoeff;
110+
111+
b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn, const double &scale, const double &mass, const double &collisionMargin)
112+
: m_startPosition(startPos),
113+
m_startOrientation(startOrn),
114+
m_scale(scale),
115+
m_mass(mass),
116+
m_collisionMargin(collisionMargin),
117+
m_springElasticStiffness(-1),
118+
m_springDampingStiffness(-1),
119+
m_springBendingStiffness(-1),
120+
m_NeoHookeanMu(-1),
121+
m_NeoHookeanDamping(-1),
122+
m_useSelfCollision(false),
123+
m_useFaceContact(false),
124+
m_useBendingSprings(false),
125+
m_frictionCoeff(0)
126+
{
127+
}
128+
129+
b3RobotSimulatorLoadDeformableBodyArgs(const btVector3 &startPos, const btQuaternion &startOrn)
130+
{
131+
b3RobotSimulatorLoadSoftBodyArgs(startPos, startOrn, 1.0, 1.0, 0.02);
132+
}
133+
134+
b3RobotSimulatorLoadDeformableBodyArgs()
135+
{
136+
b3RobotSimulatorLoadSoftBodyArgs(btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1));
137+
}
138+
139+
b3RobotSimulatorLoadDeformableBodyArgs(double scale, double mass, double collisionMargin)
140+
: m_startPosition(btVector3(0, 0, 0)),
141+
m_startOrientation(btQuaternion(0, 0, 0, 1)),
142+
m_scale(scale),
143+
m_mass(mass),
144+
m_collisionMargin(collisionMargin)
145+
{
146+
}
147+
};
148+
149+
92150
struct b3RobotSimulatorLoadFileResults
93151
{
94152
btAlignedObjectArray<int> m_uniqueObjectIds;
@@ -534,6 +592,8 @@ class b3RobotSimulatorClientAPI_NoDirect
534592
void syncBodies();
535593

536594
void resetSimulation();
595+
596+
void resetSimulation(int flag);
537597

538598
btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw);
539599
btVector3 getEulerFromQuaternion(const btQuaternion &quat);
@@ -685,6 +745,8 @@ class b3RobotSimulatorClientAPI_NoDirect
685745
int getConstraintUniqueId(int serialIndex);
686746

687747
void loadSoftBody(const std::string &fileName, const struct b3RobotSimulatorLoadSoftBodyArgs &args);
748+
749+
void loadDeformableBody(const std::string &fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs &args);
688750

689751
virtual void setGuiHelper(struct GUIHelperInterface *guiHelper);
690752
virtual struct GUIHelperInterface *getGuiHelper();

‎src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
7676
btDispatcher* m_dispatcher;
7777

7878
btAlignedObjectArray<btCollisionObject*> m_bodies;
79+
btAlignedObjectArray<btCollisionObject*> m_softBodies;
7980
btAlignedObjectArray<btPersistentManifold*> m_manifolds;
8081
btAlignedObjectArray<btTypedConstraint*> m_constraints;
8182
btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
@@ -194,6 +195,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
194195
{
195196
m_bodies.push_back(bodies[i]);
196197
}
198+
else
199+
{
200+
m_softBodies.push_back(bodies[i]);
201+
}
197202
}
198203
for (i = 0; i < numManifolds; i++)
199204
m_manifolds.push_back(manifolds[i]);
@@ -231,6 +236,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
231236
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
232237
}
233238
m_bodies.resize(0);
239+
m_softBodies.resize(0);
234240
m_manifolds.resize(0);
235241
m_constraints.resize(0);
236242
m_multiBodyConstraints.resize(0);

‎src/BulletSoftBody/DeformableBodyInplaceSolverIslandCallback.h‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
//
2+
// DeformableBodyInplaceSolverIslandCallback.h
3+
// BulletSoftBody
4+
//
5+
// Created by Xuchen Han on 12/16/19.
6+
//
7+
8+
#ifndef DeformableBodyInplaceSolverIslandCallback_h
9+
#define DeformableBodyInplaceSolverIslandCallback_h
10+
11+
struct DeformableBodyInplaceSolverIslandCallback : public MultiBodyInplaceSolverIslandCallback
12+
{
13+
btDeformableMultiBodyConstraintSolver* m_deformableSolver;
14+
15+
DeformableBodyInplaceSolverIslandCallback(btDeformableMultiBodyConstraintSolver* solver,
16+
btDispatcher* dispatcher)
17+
: MultiBodyInplaceSolverIslandCallback(solver, dispatcher), m_deformableSolver(solver)
18+
{
19+
}
20+
21+
22+
virtual void processConstraints(int islandId=-1)
23+
{
24+
btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
25+
btCollisionObject** softBodies = m_softBodies.size() ? &m_softBodies[0] : 0;
26+
btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
27+
btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
28+
btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
29+
30+
//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
31+
32+
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);
33+
if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
34+
{
35+
m_deformableSolver->m_analyticsData.m_islandId = islandId;
36+
m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
37+
}
38+
m_bodies.resize(0);
39+
m_softBodies.resize(0);
40+
m_manifolds.resize(0);
41+
m_constraints.resize(0);
42+
m_multiBodyConstraints.resize(0);
43+
}
44+
};
45+
46+
#endif /* DeformableBodyInplaceSolverIslandCallback_h */

0 commit comments

Comments
 (0)