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

Grasp soft body with rigid fingers.

Browse Source
This commit is contained in:
yunfeibai
2016-11-01 15:46:09 -07:00
parent f01389ded2
commit 9d54f0cf8d
5 changed files with 518 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
data/gripper/wsg50_one_motor_gripper_left_finger.urdf Normal file
View File

@@ -0,0 +1,27 @@
<?xml version="0.0" ?>
<robot name="left_finger.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<inertia_scaling value="3.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<mass value=".2"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 4.71239" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/l_gripper_tip_scaled.stl" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.042"/>
<geometry>
<box size="0.02 0.02 0.15"/>
</geometry>
</collision>
</link>
</robot>

307
data/gripper/wsg50_one_motor_gripper_no_finger.sdf Normal file
View File

@@ -0,0 +1,307 @@
<?xml version="1.0" ?>
<sdf version='1.6'>
<world name='default'>
<model name='wsg50_with_gripper'>
<pose frame=''>0 0 0.4 3.14 0 0</pose>
<link name='world'>
</link>
<joint name='base_joint' type='prismatic'>
<parent>world</parent>
<child>base_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-10</lower>
<upper>10</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='base_link'>
<pose frame=''>0 0 0 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>1.2</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<visual name='base_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>meshes/WSG50_110.stl</uri>
</mesh>
</geometry>
<material>
</material>
</visual>
</link>
<link name='motor'>
<pose frame=''>0 0 0.03 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>0 0 0.01 0 0 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.02 </size>
</box>
</geometry>
</visual>
</link>
<joint name='base_joint_motor' type='prismatic'>
<child>motor</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-0.055</lower>
<upper>0.001</upper>
<effort>10.0</effort>
<velocity>10.0</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='left_hinge'>
<pose frame=''>0 0 0.04 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.035 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.07 </size>
</box>
</geometry>
</visual>
</link>
<joint name='motor_left_hinge_joint' type='revolute'>
<child>left_hinge</child>
<parent>motor</parent>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>-20.0</lower>
<upper>20.0</upper>
<effort>10</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='right_hinge'>
<pose frame=''>0 0 0.04 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.035 0 0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='motor_visual'>
<pose frame=''>0.03 0 0.01 0 1.2 0</pose>
<geometry>
<box>
<size>0.02 0.02 0.07 </size>
</box>
</geometry>
</visual>
</link>
<joint name='motor_right_hinge_joint' type='revolute'>
<child>right_hinge</child>
<parent>motor</parent>
<axis>
<xyz>0 1 0</xyz>
<limit>
<lower>-20.0</lower>
<upper>20.0</upper>
<effort>10</effort>
<velocity>10</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='gripper_left'>
<pose frame=''>-0.055 0 0.06 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='gripper_left_visual'>
<pose frame=''>0 0 -0.06 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
<pose frame=''>0 0 -0.037 0 0 0</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_left_hinge_joint' type='prismatic'>
<child>gripper_left</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.01</lower>
<upper>0.04</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='gripper_right'>
<pose frame=''>0.055 0 0.06 0 0 0</pose>
<inertial>
<pose frame=''>0 0 0.0115 0 -0 0</pose>
<mass>0.2</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='gripper_right_visual'>
<pose frame=''>0 0 -0.06 0 0 3.14159</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/GUIDE_WSG50_110.stl</uri>
</mesh>
</geometry>
</visual>
<visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
<pose frame=''>0 0 -0.037 0 0 3.14159</pose>
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>meshes/WSG-FMF.stl</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='gripper_right_hinge_joint' type='prismatic'>
<child>gripper_right</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-0.04</lower>
<upper>0.01</upper>
<effort>1</effort>
<velocity>1</velocity>
</limit>
<dynamics>
<damping>0</damping>
<friction>0</friction>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
</model>
</world>
</sdf>

27
data/gripper/wsg50_one_motor_gripper_right_finger.urdf Normal file
View File

@@ -0,0 +1,27 @@
<?xml version="0.0" ?>
<robot name="right_finger.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<inertia_scaling value="3.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0.05"/>
<mass value=".2"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
<visual>
<origin rpy="0 0 1.5708" xyz="0 0 0"/>
<geometry>
<mesh filename="meshes/l_gripper_tip_scaled.stl" scale="1 1 1"/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.042"/>
<geometry>
<box size="0.02 0.02 0.15"/>
</geometry>
</collision>
</link>
</robot>

162
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -346,7 +346,7 @@ public:
if (1)
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
args.m_fileName = "gripper/wsg50_one_motor_gripper_no_finger.sdf";
args.m_fileType = B3_SDF_FILE;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
@@ -365,7 +365,7 @@ public:
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
for (int i=0;i<8;i++)
for (int i=0;i<6;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
@@ -374,7 +374,27 @@ public:
}
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper_left_finger.urdf";
args.m_startPosition.setValue(-0.05,0,0.27);
args.m_startOrientation.setEulerZYX(0,0,3.14);
args.m_forceOverrideFixedBase = false;
args.m_useMultiBody = false;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "gripper/wsg50_one_motor_gripper_right_finger.urdf";
args.m_startPosition.setValue(0.05,0,0.27);
args.m_startOrientation.setEulerZYX(0,0,3.14);
args.m_forceOverrideFixedBase = false;
args.m_useMultiBody = false;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
if (1)
{
b3RobotSimLoadFileArgs args("");
@@ -391,16 +411,16 @@ public:
m_robotSim.loadBunny();
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.055;
revoluteJoint1.m_parentFrame[0] = -0.007;
revoluteJoint1.m_parentFrame[1] = 0;
revoluteJoint1.m_parentFrame[2] = 0.02;
revoluteJoint1.m_parentFrame[2] = 0.0735;
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[2] = -0.05;
revoluteJoint1.m_childFrame[3] = 0;
revoluteJoint1.m_childFrame[4] = 0;
revoluteJoint1.m_childFrame[5] = 0;
@@ -410,16 +430,16 @@ public:
revoluteJoint1.m_jointAxis[2] = 0.0;
revoluteJoint1.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint2;
revoluteJoint2.m_parentFrame[0] = 0.055;
revoluteJoint2.m_parentFrame[0] = 0.007;
revoluteJoint2.m_parentFrame[1] = 0;
revoluteJoint2.m_parentFrame[2] = 0.02;
revoluteJoint2.m_parentFrame[2] = 0.0735;
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[2] = -0.05;
revoluteJoint2.m_childFrame[3] = 0;
revoluteJoint2.m_childFrame[4] = 0;
revoluteJoint2.m_childFrame[5] = 0;
@@ -428,8 +448,128 @@ public:
revoluteJoint2.m_jointAxis[1] = 0.0;
revoluteJoint2.m_jointAxis[2] = 0.0;
revoluteJoint2.m_jointType = ePoint2PointType;
m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1);
m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2);
b3JointInfo revoluteJoint3;
revoluteJoint3.m_parentFrame[0] = -0.207;
revoluteJoint3.m_parentFrame[1] = 0;
revoluteJoint3.m_parentFrame[2] = 0.0735;
revoluteJoint3.m_parentFrame[3] = 0;
revoluteJoint3.m_parentFrame[4] = 0;
revoluteJoint3.m_parentFrame[5] = 0;
revoluteJoint3.m_parentFrame[6] = 1.0;
revoluteJoint3.m_childFrame[0] = -0.2;
revoluteJoint3.m_childFrame[1] = 0;
revoluteJoint3.m_childFrame[2] = -0.05;
revoluteJoint3.m_childFrame[3] = 0;
revoluteJoint3.m_childFrame[4] = 0;
revoluteJoint3.m_childFrame[5] = 0;
revoluteJoint3.m_childFrame[6] = 1.0;
revoluteJoint3.m_jointAxis[0] = 1.0;
revoluteJoint3.m_jointAxis[1] = 0.0;
revoluteJoint3.m_jointAxis[2] = 0.0;
revoluteJoint3.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint4;
revoluteJoint4.m_parentFrame[0] = 0.207;
revoluteJoint4.m_parentFrame[1] = 0;
revoluteJoint4.m_parentFrame[2] = 0.0735;
revoluteJoint4.m_parentFrame[3] = 0;
revoluteJoint4.m_parentFrame[4] = 0;
revoluteJoint4.m_parentFrame[5] = 0;
revoluteJoint4.m_parentFrame[6] = 1.0;
revoluteJoint4.m_childFrame[0] = 0.2;
revoluteJoint4.m_childFrame[1] = 0;
revoluteJoint4.m_childFrame[2] = -0.05;
revoluteJoint4.m_childFrame[3] = 0;
revoluteJoint4.m_childFrame[4] = 0;
revoluteJoint4.m_childFrame[5] = 0;
revoluteJoint4.m_childFrame[6] = 1.0;
revoluteJoint4.m_jointAxis[0] = 1.0;
revoluteJoint4.m_jointAxis[1] = 0.0;
revoluteJoint4.m_jointAxis[2] = 0.0;
revoluteJoint4.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint5;
revoluteJoint5.m_parentFrame[0] = -0.007;
revoluteJoint5.m_parentFrame[1] = 0;
revoluteJoint5.m_parentFrame[2] = 0.2735;
revoluteJoint5.m_parentFrame[3] = 0;
revoluteJoint5.m_parentFrame[4] = 0;
revoluteJoint5.m_parentFrame[5] = 0;
revoluteJoint5.m_parentFrame[6] = 1.0;
revoluteJoint5.m_childFrame[0] = 0;
revoluteJoint5.m_childFrame[1] = 0;
revoluteJoint5.m_childFrame[2] = 0.15;
revoluteJoint5.m_childFrame[3] = 0;
revoluteJoint5.m_childFrame[4] = 0;
revoluteJoint5.m_childFrame[5] = 0;
revoluteJoint5.m_childFrame[6] = 1.0;
revoluteJoint5.m_jointAxis[0] = 1.0;
revoluteJoint5.m_jointAxis[1] = 0.0;
revoluteJoint5.m_jointAxis[2] = 0.0;
revoluteJoint5.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint6;
revoluteJoint6.m_parentFrame[0] = 0.007;
revoluteJoint6.m_parentFrame[1] = 0;
revoluteJoint6.m_parentFrame[2] = 0.2735;
revoluteJoint6.m_parentFrame[3] = 0;
revoluteJoint6.m_parentFrame[4] = 0;
revoluteJoint6.m_parentFrame[5] = 0;
revoluteJoint6.m_parentFrame[6] = 1.0;
revoluteJoint6.m_childFrame[0] = 0;
revoluteJoint6.m_childFrame[1] = 0;
revoluteJoint6.m_childFrame[2] = 0.15;
revoluteJoint6.m_childFrame[3] = 0;
revoluteJoint6.m_childFrame[4] = 0;
revoluteJoint6.m_childFrame[5] = 0;
revoluteJoint6.m_childFrame[6] = 1.0;
revoluteJoint6.m_jointAxis[0] = 1.0;
revoluteJoint6.m_jointAxis[1] = 0.0;
revoluteJoint6.m_jointAxis[2] = 0.0;
revoluteJoint6.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint7;
revoluteJoint7.m_parentFrame[0] = -0.055;
revoluteJoint7.m_parentFrame[1] = 0;
revoluteJoint7.m_parentFrame[2] = 0.02;
revoluteJoint7.m_parentFrame[3] = 0;
revoluteJoint7.m_parentFrame[4] = 0;
revoluteJoint7.m_parentFrame[5] = 0;
revoluteJoint7.m_parentFrame[6] = 1.0;
revoluteJoint7.m_childFrame[0] = 0;
revoluteJoint7.m_childFrame[1] = 0;
revoluteJoint7.m_childFrame[2] = 0;
revoluteJoint7.m_childFrame[3] = 0;
revoluteJoint7.m_childFrame[4] = 0;
revoluteJoint7.m_childFrame[5] = 0;
revoluteJoint7.m_childFrame[6] = 1.0;
revoluteJoint7.m_jointAxis[0] = 1.0;
revoluteJoint7.m_jointAxis[1] = 0.0;
revoluteJoint7.m_jointAxis[2] = 0.0;
revoluteJoint7.m_jointType = ePoint2PointType;
b3JointInfo revoluteJoint8;
revoluteJoint8.m_parentFrame[0] = 0.055;
revoluteJoint8.m_parentFrame[1] = 0;
revoluteJoint8.m_parentFrame[2] = 0.02;
revoluteJoint8.m_parentFrame[3] = 0;
revoluteJoint8.m_parentFrame[4] = 0;
revoluteJoint8.m_parentFrame[5] = 0;
revoluteJoint8.m_parentFrame[6] = 1.0;
revoluteJoint8.m_childFrame[0] = 0;
revoluteJoint8.m_childFrame[1] = 0;
revoluteJoint8.m_childFrame[2] = 0;
revoluteJoint8.m_childFrame[3] = 0;
revoluteJoint8.m_childFrame[4] = 0;
revoluteJoint8.m_childFrame[5] = 0;
revoluteJoint8.m_childFrame[6] = 1.0;
revoluteJoint8.m_jointAxis[0] = 1.0;
revoluteJoint8.m_jointAxis[1] = 0.0;
revoluteJoint8.m_jointAxis[2] = 0.0;
revoluteJoint8.m_jointType = ePoint2PointType;
m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint1);
m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint2);
m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint3);
m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint4);
m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint5);
m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint6);
m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint7);
m_robotSim.createJoint(0, 3, 0, 5, &revoluteJoint8);
}
}
virtual void exitPhysics()
@@ -479,7 +619,7 @@ public:
int fingerJointIndices[2]={0,1};
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
};
double maxTorqueValues[2]={50.0,50.0};
double maxTorqueValues[2]={50.0,10.0};
for (int i=0;i<2;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);

8
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1643,14 +1643,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
pm->m_kLST = 1.0;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2,pm);
psb->m_cfg.piterations = 2;
psb->m_cfg.piterations = 15;
psb->m_cfg.kDF = 0.5;
psb->randomizeConstraints();
psb->rotate(btQuaternion(0.70711,0,0,0.70711));
psb->translate(btVector3(0,0,3.0));
psb->translate(btVector3(0,0,1.0));
psb->scale(btVector3(0.1,0.1,0.1));
psb->setTotalMass(1,true);
psb->getCollisionShape()->setMargin(0.01);
psb->setTotalMass(0.1,true);
psb->getCollisionShape()->setMargin(0.02);
m_data->m_dynamicsWorld->addSoftBody(psb);
#endif