decrease some gjk/epa tolerance to improve collision detection using very small collision margins (0.001/1mm)

tweak pr2 gripper.
This commit is contained in:
erwin coumans
2016-09-20 12:37:13 -07:00
parent 8d4c99801e
commit 0a628f06cc
10 changed files with 214 additions and 40 deletions

16
data/jenga/jenga.mtl Normal file
View File

@@ -0,0 +1,16 @@
newmtl jenga
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka jenga.tga
map_Kd jenga.png

113
data/jenga/jenga.obj Normal file
View File

@@ -0,0 +1,113 @@
# jenga.obj
#
o jenga
mtllib jenga.mtl
v -0.5 -0.5 0.5
v 0.5 -0.5 0.5
v 0.5 0.5 0.5
v -0.5 0.5 0.5
v -0.5 -0.5 -0.5
v 0.5 -0.5 -0.5
v 0.5 0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 -0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 0.5 0.5
v -0.5 -0.5 0.5
v 0.5 -0.5 -0.5
v 0.5 0.5 -0.5
v 0.5 0.5 0.5
v 0.5 -0.5 0.5
v -0.5 -0.5 -0.5
v -0.5 -0.5 0.5
v 0.5 -0.5 0.5
v 0.5 -0.5 -0.5
v -0.5 0.5 -0.5
v -0.5 0.5 0.5
v 0.5 0.5 0.5
v 0.5 0.5 -0.5
vt 0 1
vt 0 0.75
vt 0.25 0.75
vt 0.25 1
vt 0.25 0.5
vt 0.25 0.75
vt 0.5 0.75
vt 0.5 0.5
vt 1 0.75
vt 0.75 0.75
vt 0.75 1
vt 1 1
vt 0.25 0.75
vt 0.5 0.75
vt 0.5 1
vt 0.25 1
vt 0 0.5
vt 0 0.75
vt 0.25 0.75
vt 0.25 0.5
vt 0.75 0.75
vt 0.75 1
vt 0.5 1
vt 0.5 0.75
vn 0 0 1
vn 0 0 1
vn 0 0 1
vn 0 0 1
vn 0 0 -1
vn 0 0 -1
vn 0 0 -1
vn 0 0 -1
vn -1 0 0
vn -1 0 0
vn -1 0 0
vn -1 0 0
vn 1 0 0
vn 1 0 0
vn 1 0 0
vn 1 0 0
vn 0 -1 0
vn 0 -1 0
vn 0 -1 0
vn 0 -1 0
vn 0 1 0
vn 0 1 0
vn 0 1 0
vn 0 1 0
g jenga
usemtl jenga
s 1
f 1/1/1 2/2/2 3/3/3
f 1/1/1 3/3/3 4/4/4
s 2
f 7/7/7 6/6/6 5/5/5
f 8/8/8 7/7/7 5/5/5
s 3
f 11/11/11 10/10/10 9/9/9
f 12/12/12 11/11/11 9/9/9
s 4
f 13/13/13 14/14/14 15/15/15
f 13/13/13 15/15/15 16/16/16
s 5
f 19/19/19 18/18/18 17/17/17
f 20/20/20 19/19/19 17/17/17
s 6
f 21/21/21 22/22/22 23/23/23
f 21/21/21 23/23/23 24/24/24

BIN
data/jenga/jenga.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 280 KiB

31
data/jenga/jenga.urdf Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="0.0" ?>
<robot name="jenga.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<spinning_friction value="0.5"/>
<inertia_scaling value="3.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="jenga.obj" scale="0.15 0.05 0.03"/>
</geometry>
<material name="blue">
<color rgba="0.5 0.5 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.15 0.05 0.03"/>
</geometry>
</collision>
</link>
</robot>

Binary file not shown.

View File

@@ -32,18 +32,23 @@
<link name="left_gripper">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</visual>
<collision>
<origin rpy="0.0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@@ -55,8 +60,8 @@
<link name="left_tip">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value="0.1"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
@@ -71,7 +76,7 @@
<origin rpy="0.0 0 0" xyz="0.105 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@@ -86,18 +91,23 @@
<link name="right_gripper">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value="0.3"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger.stl"/>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</visual>
<collision>
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="l_finger_collision.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
@@ -109,8 +119,8 @@
<link name="right_tip">
<contact>
<lateral_friction value="5.0"/>
<spinning_friction value=".2"/>
<lateral_friction value="1.0"/>
<spinning_friction value="1.5"/>
</contact>
<visual>
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
@@ -125,7 +135,7 @@
<origin rpy="-3.1415 0 0" xyz="0.105 0.00495 0"/>
</collision>
<inertial>
<mass value="0.05"/>
<mass value="0.1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>