Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 51 additions & 0 deletions data/grasping_simulation.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
<?xml version="1.0" encoding="UTF-8"?>
<world>
<robot name="Hand" file="robots/3fingerhand.rob"/>
<terrain file="terrains/plane.off"/>

<simulation>
<globals maxContacts="10" robotSelfCollisions="1" adaptiveTimeStepping="0.1"/>
<object index="0">
<geometry padding="0.005"/>
</object>
<robot index="0" body="5">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.01" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="6">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="7">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="8">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="9">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="10">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="11">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="12">s
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="13">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="14">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="15">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="16">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
<robot index="0" body="17">
<geometry kFriction="1.0" kRestitution="0.0" padding="0.001" stiffness="2000000" damping="1000"/>
</robot>
</simulation>
</world>
90 changes: 90 additions & 0 deletions data/robots/3fingerhand.rob
Original file line number Diff line number Diff line change
@@ -0,0 +1,90 @@
### Kinematics of the Robotiq gripper ###
TParent 1 0 0 0 1 0 0 0 1 0 0 0 \
1 0 0 0 1 0 0 0 1 0 0 0 \
1 0 0 0 1 0 0 0 1 0 0 0 \
1 0 0 0 1 0 0 0 1 0 0 0 \
1 0 0 0 1 0 0 0 1 0 0 0 \
1 0 0 0 1 0 0 0 1 0 0 0 \
0 0 -1 0 -1 0 -1 0 0 0.037 0.044 0.00 \
0 0 -1 0 -1 0 -1 0 0 -0.037 0.044 0.00 \
0 0 1 0 1 0 -1 0 0 0.0 -0.044 0.00

# 0 1 0 1 0 0 0 0 -1 0 0 0.05\

parents -1 0 1 2 3 4 5 5 5
axis 1 0 0 0 1 0 0 0 1 0 0 1 0 1 0 1 0 0 0 1 0 0 1 0 0 1 0
jointtype p p p r r r r r r
qMin -inf -inf -inf -inf -inf -inf 0 0 0
qMax inf inf inf inf inf inf 0 0 0
velMaxDeg inf inf inf inf inf inf 0 0 0
accMaxDeg inf inf inf inf inf inf 0 0 0
qDeg 0 0 0 0 0 0 0 0 0
geometry "" "" "" "" "" "RobotiQ/palm0.off" "RobotiQ/link0.off" "RobotiQ/link0.off" "RobotiQ/link0.off"


#geomscale 0.1 1 1 1
torqueMax 500 500 500 50 50 50 0 0 0

mass 0.1 0.1 0.1 0.1 0.1 1 0.15 0.15 0.15
automass
#inertia m.offx [7300 0 -650
# 0 8800 0
# -650 0 7000]kg*mm^2

#joint floating 5 -1
joint normal 0
joint normal 1
joint normal 2
joint spin 3
joint spin 4
joint spin 5

joint weld 8
joint weld 6
joint weld 7

mount 8 "box_finger/finger.rob" 1 0 0 0 1 0 0 0 1 0.018 0.0008 0 as "thumb"
mount 6 "box_finger/finger.rob" 1 0 0 0 1 0 0 0 1 0.018 0.0008 0 as "finger1"
mount 7 "box_finger/finger.rob" 1 0 0 0 1 0 0 0 1 0.018 0.0008 0 as "finger2"


#scissor 1 to scissor 2
noselfcollision 6 7
#scissor 1 to scissor 3
noselfcollision 6 8
#scissor 2 to scissor 3
noselfcollision 7 8

#finger 1
noselfcollision 5 12 #palm
noselfcollision 6 13 6 14 #scissor 1
noselfcollision 7 12 7 13 7 14 #scissor 2
noselfcollision 8 12 8 13 8 14 #scissor 3
#finger 2
noselfcollision 5 15 #palm
noselfcollision 5 17 #palm
noselfcollision 5 16 #palm
noselfcollision 7 16 7 17 #scissor 2
noselfcollision 6 15 6 16 6 17 #scissor 1
noselfcollision 8 15 8 16 8 17 #scissor 3
#finger 3
noselfcollision 5 9 #palm
noselfcollision 8 9 8 10 8 11 #scissor 3
noselfcollision 6 9 6 10 6 11 #scissor 1
noselfcollision 7 9 7 10 7 11 #scissor 2

#proximal to distal joints, all fingers
noselfcollision 9 11 12 14 15 17

driver normal 0
driver normal 1
driver normal 2
driver normal 3
driver normal 4
driver normal 5

servoP 5000 5000 5000 500 500 500
servoI 10 10 10 .5 .5 .5
servoD 100 100 100 10 10 10
viscousFriction 50 50 50 50 50 50
dryFriction 1 1 1 1 1 1
22 changes: 22 additions & 0 deletions data/robots/box_finger/box_finger.off
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
OFF
8 12 0
0.000000 -0.150000 -0.080000
0.000000 -0.150000 0.00000
0.000000 0.150000 -0.080000
0.000000 0.150000 0.00000
0.500000 -0.150000 -0.080000
0.500000 -0.150000 0.00000
0.500000 0.150000 -0.080000
0.500000 0.150000 0.00000
3 0 1 3
3 0 3 2
3 4 6 7
3 4 7 5
3 0 4 5
3 0 5 1
3 2 3 7
3 2 7 6
3 0 2 6
3 0 6 4
3 1 5 7
3 1 7 3
47 changes: 47 additions & 0 deletions data/robots/box_finger/finger.rob
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
### Kinematics of the Robotiq gripper fingerThumb ###

#Denavit-Hartenberg parameters. Alpha and theta are in degrees, d and a are in mm
#thetaDeg -29.61 0 29.61
#d 0 0.1 0.1
#a 0 0.057150 0.038940
#alphaDeg 0 0 0
TParent 0.8694087071436382 0.0 0.49409361455378836 -0.49409361455378836 0.0 0.8694087071436382 0.0 -1.0 0.0 0.000 0.0008 0.0 \
1.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 1.0 0.05 0.0 0.\
1.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 1.0 0.05 0.0 0. \
#0.8694087071436382 0.49409361455378836 0.0 -0.49409361455378836 0.8694087071436382 0.0 0.0 0.0 1.0 0.018 0.0008 0.0 \

#scaled 0.8694087071436382 -0.49409361455378836 0.0 0.49409361455378836 0.8694087071436382 0.0 0.0 0.0 1.0 0.1168200 0.0 -0.008
#geomtransform 1 1 0 0 0 0 1 0 0 0 0 1 0
#geomtransform 2 1 0 0 0 0 1 0 0 0 0 0.1 0
#rotational axes
geomscale 0.1 0.1 0.1
axis 0 -1 0 0 -1 0 0 -1 0

#index of parent link
parents -1 0 1

#min and max joint values, in degrees
qMinDeg -5.39 0 -54.61
qMaxDeg 64.61 90 43.39

#velocity maxima, in degrees
velMaxDeg 90 90 90

#acceration maxima, in degrees
accMaxDeg 180 180 180

#initial configuration, in degrees
qDeg -5.39 0 0.0

#geometry files
geometry "box_finger.off" "box_finger.off" "box_finger.off"

mass 0.3 0.3 0.3
automass
torqueMax 10 10 10

servoP 50 50 50
servoD 0 0 0
servoI 10 10 10
dryFriction 0.05 0.05 0.05
viscousFriction 2 2 2