diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 6bc8b589b..e59641d18 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -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), diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 8f5ec1a8e..c3afaee88 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -294,6 +294,102 @@ public: 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); @@ -326,10 +422,18 @@ public: 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(1, .01, 0.02); + args.m_springElasticStiffness = 1; + args.m_springDampingStiffness = .001; + args.m_springBendingStiffness = .2; + args.m_frictionCoeff = 2; + args.m_useSelfCollision = false; + 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; @@ -371,7 +475,7 @@ public: revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2); - } + } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0) { @@ -462,6 +566,21 @@ public: 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) { diff --git a/examples/RoboticsLearning/GripperGraspExample.h b/examples/RoboticsLearning/GripperGraspExample.h index 96387bc41..110068b8e 100644 --- a/examples/RoboticsLearning/GripperGraspExample.h +++ b/examples/RoboticsLearning/GripperGraspExample.h @@ -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); diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 27bc23163..d8cd3d495 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -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()) @@ -1151,6 +1164,29 @@ 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); + b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping); + b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness); + b3LoadSoftBodyUseSelfCollision(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; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index d8fd179c0..e119a37de 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -89,6 +89,55 @@ 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) + { + } + + 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 m_uniqueObjectIds; @@ -534,6 +583,8 @@ public: void syncBodies(); void resetSimulation(); + + void resetSimulation(int flag); btQuaternion getQuaternionFromEuler(const btVector3 &rollPitchYaw); btVector3 getEulerFromQuaternion(const btQuaternion &quat); @@ -685,6 +736,8 @@ public: 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(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 9c5390d1f..581d6feae 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -22,8 +22,32 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration { ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); +// +// int maxMotorIterations = 150; +// for (int iteration = 0; iteration < maxMotorIterations; ++iteration) +// { +// btScalar motorResidual = 0; +// for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) +// { +// int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; +// +// btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; +// +// btScalar residual = resolveSingleConstraintRowGeneric(constraint); +// motorResidual = btMax(motorResidual, residual * residual); +// if (constraint.m_multiBodyA) +// constraint.m_multiBodyA->setPosUpdated(false); +// if (constraint.m_multiBodyB) +// constraint.m_multiBodyB->setPosUpdated(false); +// } +// if (motorResidual < infoGlobal.m_leastSquaresResidualThreshold) +// { +// break; +// } +// } int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + maxIterations = 500; for (int iteration = 0; iteration < maxIterations; iteration++) { // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body