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

add gripper with deformable cloth demo

Browse Source
This commit is contained in:
Xuchen Han
2019-12-13 14:33:54 -08:00
parent 08321b96ba
commit d38ea87027
6 changed files with 239 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -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),

129
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -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)
{

1
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -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);

36
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
View File

@@ -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;

53
examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
View File

@@ -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<int> 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();