Skip to content

Commit 37fca91

Browse files
fmessmerfmessmer
committed
fix mimic joints and quaternion normalization
1 parent d5ea3f3 commit 37fca91

File tree

6 files changed

+111
-72
lines changed

6 files changed

+111
-72
lines changed

‎cob_grasp_generation/files/database/pringles/sdhx_pringles.csv

+25-25
Original file line numberDiff line numberDiff line change
@@ -1,25 +1,25 @@
1-
id,object,gripper_left_finger_1_joint,gripper_left_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
2-
0,pringles,0.1,-0.9,SIDE,0.5,-0.5,-0.5,-0.5,0,-14,6,0.01,0.0005,0
3-
1,pringles,0.1,-0.9,SIDE,0.653,-0.271,-0.653,-0.271,9.956,-9.842,6,0.01,0.0005,1
4-
2,pringles,0.1,-0.9,SIDE,0.707,0,-0.707,0,14,0,6,0.01,0.0005,2
5-
3,pringles,0.1,-0.9,SIDE,0.653,0.271,-0.653,0.271,9.63,10.162,6,0.01,0.0005,3
6-
4,pringles,0.1,-0.9,SIDE,0.5,0.5,-0.5,0.5,0,14,6,0.01,0.0005,4
7-
5,pringles,0.1,-0.9,SIDE,0.271,0.653,-0.271,0.653,-10.106,9.688,6,0.01,0.0005,5
8-
6,pringles,0.1,-0.9,SIDE,0,0.707,0,0.707,-14,0,6,0.01,0.0005,6
9-
7,pringles,0.1,-0.9,SIDE,-0.271,0.653,0.271,0.653,-9.463,-10.317,6,0.01,0.0005,7
10-
8,pringles,0.1,-0.9,SIDE,0.5,-0.5,-0.5,-0.5,0,-14,0,0.01,0.0005,8
11-
9,pringles,0.1,-0.9,SIDE,0.653,-0.271,-0.653,-0.271,9.956,-9.842,0,0.01,0.0005,9
12-
10,pringles,0.1,-0.9,SIDE,0.707,0,-0.707,0,14,0,0,0.01,0.0005,10
13-
11,pringles,0.1,-0.9,SIDE,0.653,0.271,-0.653,0.271,9.63,10.162,0,0.01,0.0005,11
14-
12,pringles,0.1,-0.9,SIDE,0.5,0.5,-0.5,0.5,0,14,0,0.01,0.0005,12
15-
13,pringles,0.1,-0.9,SIDE,0.271,0.653,-0.271,0.653,-10.106,9.688,0,0.01,0.0005,13
16-
14,pringles,0.1,-0.9,SIDE,0,0.707,0,0.707,-14,0,0,0.01,0.0005,14
17-
15,pringles,0.1,-0.9,SIDE,-0.271,0.653,0.271,0.653,-9.463,-10.317,0,0.01,0.0005,15
18-
16,pringles,0.1,-0.9,SIDE,0.5,-0.5,-0.5,-0.5,0,-14,-6,0.01,0.0005,16
19-
17,pringles,0.1,-0.9,SIDE,0.653,-0.271,-0.653,-0.271,9.956,-9.842,-6,0.01,0.0005,17
20-
18,pringles,0.1,-0.9,SIDE,0.707,0,-0.707,0,14,0,-6,0.01,0.0005,18
21-
19,pringles,0.1,-0.9,SIDE,0.653,0.271,-0.653,0.271,9.63,10.162,-6,0.01,0.0005,19
22-
20,pringles,0.1,-0.9,SIDE,0.5,0.5,-0.5,0.5,0,14,-6,0.01,0.0005,20
23-
21,pringles,0.1,-0.9,SIDE,0.271,0.653,-0.271,0.653,-10.106,9.688,-6,0.01,0.0005,21
24-
22,pringles,0.1,-0.9,SIDE,0,0.707,0,0.707,-14,0,-6,0.01,0.0005,22
25-
23,pringles,0.1,-0.9,SIDE,-0.271,0.653,0.271,0.653,-9.463,-10.317,-6,0.01,0.0005,23
1+
id,object,gripper_finger_1_joint,gripper_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
2+
0,pringles,0.1,-0.9,SIDE,0.50000,-0.50000,-0.50000,-0.50000,0,-14,6,0.01,0.0005,0
3+
1,pringles,0.1,-0.9,SIDE,0.65328,-0.27060,-0.65328,-0.27060,10,-10,6,0.01,0.0005,1
4+
2,pringles,0.1,-0.9,SIDE,0.70711,0.00000,-0.70711,0.00000,14,0,6,0.01,0.0005,2
5+
3,pringles,0.1,-0.9,SIDE,0.65328,0.27060,-0.65328,0.27060,10,10,6,0.01,0.0005,3
6+
4,pringles,0.1,-0.9,SIDE,0.50000,0.50000,-0.50000,0.50000,0,14,6,0.01,0.0005,4
7+
5,pringles,0.1,-0.9,SIDE,0.27060,0.65328,-0.27060,0.65328,-10,10,6,0.01,0.0005,5
8+
6,pringles,0.1,-0.9,SIDE,0.00000,0.70711,0.00000,0.70711,-14,0,6,0.01,0.0005,6
9+
7,pringles,0.1,-0.9,SIDE,-0.27060,0.65328,0.27060,0.65328,-9,-10,6,0.01,0.0005,7
10+
8,pringles,0.1,-0.9,SIDE,0.50000,-0.50000,-0.50000,-0.50000,0,-14,0,0.01,0.0005,8
11+
9,pringles,0.1,-0.9,SIDE,0.65328,-0.27060,-0.65328,-0.27060,10,-10,0,0.01,0.0005,9
12+
10,pringles,0.1,-0.9,SIDE,0.70711,0.00000,-0.70711,0.00000,14,0,0,0.01,0.0005,10
13+
11,pringles,0.1,-0.9,SIDE,0.65328,0.27060,-0.65328,0.27060,10,10,0,0.01,0.0005,11
14+
12,pringles,0.1,-0.9,SIDE,0.50000,0.50000,-0.50000,0.50000,0,14,0,0.01,0.0005,12
15+
13,pringles,0.1,-0.9,SIDE,0.27060,0.65328,-0.27060,0.65328,-10,10,0,0.01,0.0005,13
16+
14,pringles,0.1,-0.9,SIDE,0.00000,0.70711,0.00000,0.70711,-14,0,0,0.01,0.0005,14
17+
15,pringles,0.1,-0.9,SIDE,-0.27060,0.65328,0.27060,0.65328,-9,-10,0,0.01,0.0005,15
18+
16,pringles,0.1,-0.9,SIDE,0.50000,-0.50000,-0.50000,-0.50000,0,-14,-6,0.01,0.0005,16
19+
17,pringles,0.1,-0.9,SIDE,0.65328,-0.27060,-0.65328,-0.27060,10,-10,-6,0.01,0.0005,17
20+
18,pringles,0.1,-0.9,SIDE,0.70711,0.00000,-0.70711,0.00000,14,0,-6,0.01,0.0005,18
21+
19,pringles,0.1,-0.9,SIDE,0.65328,0.27060,-0.65328,0.27060,10,10,-6,0.01,0.0005,19
22+
20,pringles,0.1,-0.9,SIDE,0.50000,0.50000,-0.50000,0.50000,0,14,-6,0.01,0.0005,20
23+
21,pringles,0.1,-0.9,SIDE,0.27060,0.65328,-0.27060,0.65328,-10,10,-6,0.01,0.0005,21
24+
22,pringles,0.1,-0.9,SIDE,0.00000,0.70711,0.00000,0.70711,-14,0,-6,0.01,0.0005,22
25+
23,pringles,0.1,-0.9,SIDE,-0.27060,0.65328,0.27060,0.65328,-9,-10,-6,0.01,0.0005,23

‎cob_grasp_generation/files/database/yellowsaltcube/sdhx_yellowsaltcube.csv

+9-9
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,9 @@
1-
id,object,gripper_left_finger_1_joint,gripper_left_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
2-
0,yellowsaltcube,0.1,-0.9,SIDE,0.707,0,0.707,0,-22,0,10,0.01,0.0005,0
3-
1,yellowsaltcube,0.1,-0.9,SIDE,0.653,-0.271,0.653,0.271,-14,-17,10,0.01,0.0005,1
4-
2,yellowsaltcube,0.1,-0.9,SIDE,0.5,-0.5,0.5,0.5,3.5,-21,10,0.01,0.0005,2
5-
3,yellowsaltcube,0.1,-0.9,SIDE,0.271,-0.653,0.271,0.653,19,-11,10,0.01,0.0005,3
6-
4,yellowsaltcube,0.1,-0.9,SIDE,0,-0.707,0,0.707,21.2,5.6,10,0.01,0.0005,4
7-
5,yellowsaltcube,0.1,-0.9,SIDE,0.271,0.653,0.271,-0.653,11,19,10,0.01,0.0005,5
8-
6,yellowsaltcube,0.1,-0.9,SIDE,0.5,0.5,0.5,-0.5,-5.6,21,10,0.01,0.0005,6
9-
7,yellowsaltcube,0.1,-0.9,SIDE,0.653,0.271,0.653,-0.271,-18.4,12,10,0.01,0.0005,7
1+
id,object,gripper_finger_1_joint,gripper_finger_2_joint,direction,qw,qx,qy,qz,pos-x,pos-y,pos-z,eps_l1,vol_l1,
2+
0,yellowsaltcube,0.1,-0.9,SIDE,0.70711,0.00000,0.70711,0.00000,-22,0,10,0.01,0.0005,0
3+
1,yellowsaltcube,0.1,-0.9,SIDE,0.65328,-0.27060,0.65328,0.27060,-14,-17,10,0.01,0.0005,1
4+
2,yellowsaltcube,0.1,-0.9,SIDE,0.50000,-0.50000,0.50000,0.50000,3.5,-21,10,0.01,0.0005,2
5+
3,yellowsaltcube,0.1,-0.9,SIDE,0.27060,-0.65328,0.27060,0.65328,19,-11,10,0.01,0.0005,3
6+
4,yellowsaltcube,0.1,-0.9,SIDE,0.00000,-0.70711,0.00000,0.70711,21.2,5.6,10,0.01,0.0005,4
7+
5,yellowsaltcube,0.1,-0.9,SIDE,0.27060,0.65328,0.27060,-0.65328,11,19,10,0.01,0.0005,5
8+
6,yellowsaltcube,0.1,-0.9,SIDE,0.50000,0.50000,0.50000,-0.50000,-5.6,21,10,0.01,0.0005,6
9+
7,yellowsaltcube,0.1,-0.9,SIDE,0.65328,0.27060,0.65328,-0.27060,-18.4,12,10,0.01,0.0005,7

‎cob_grasp_generation/launch/show_grasp.rviz

+53-23
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
11
Panels:
22
- Class: rviz/Displays
3-
Help Height: 78
3+
Help Height: 0
44
Name: Displays
55
Property Tree Widget:
66
Expanded:
77
- /Global Options1
88
Splitter Ratio: 0.5
9-
Tree Height: 595
9+
Tree Height: 437
1010
- Class: rviz/Selection
1111
Name: Selection
1212
- Class: rviz/Tool Properties
@@ -61,32 +61,62 @@ Visualization Manager:
6161
Alpha: 1
6262
Show Axes: false
6363
Show Trail: false
64-
gripper_base_link:
64+
sdh_finger_11_link:
6565
Alpha: 1
6666
Show Axes: false
6767
Show Trail: false
6868
Value: true
69-
gripper_camera_link:
69+
sdh_finger_12_link:
7070
Alpha: 1
7171
Show Axes: false
7272
Show Trail: false
7373
Value: true
74-
gripper_finger_1_link:
74+
sdh_finger_13_link:
7575
Alpha: 1
7676
Show Axes: false
7777
Show Trail: false
7878
Value: true
79-
gripper_finger_2_link:
79+
sdh_finger_21_link:
8080
Alpha: 1
8181
Show Axes: false
8282
Show Trail: false
8383
Value: true
84-
gripper_grasp_link:
84+
sdh_finger_22_link:
8585
Alpha: 1
8686
Show Axes: false
8787
Show Trail: false
8888
Value: true
89-
gripper_palm_link:
89+
sdh_finger_23_link:
90+
Alpha: 1
91+
Show Axes: false
92+
Show Trail: false
93+
Value: true
94+
sdh_grasp_link:
95+
Alpha: 1
96+
Show Axes: false
97+
Show Trail: false
98+
Value: true
99+
sdh_palm_link:
100+
Alpha: 1
101+
Show Axes: false
102+
Show Trail: false
103+
Value: true
104+
sdh_thumb_1_link:
105+
Alpha: 1
106+
Show Axes: false
107+
Show Trail: false
108+
Value: true
109+
sdh_thumb_2_link:
110+
Alpha: 1
111+
Show Axes: false
112+
Show Trail: false
113+
Value: true
114+
sdh_thumb_3_link:
115+
Alpha: 1
116+
Show Axes: false
117+
Show Trail: false
118+
Value: true
119+
sdh_tip_link:
90120
Alpha: 1
91121
Show Axes: false
92122
Show Trail: false
@@ -129,38 +159,38 @@ Visualization Manager:
129159
Views:
130160
Current:
131161
Class: rviz/Orbit
132-
Distance: 0.72402
162+
Distance: 0.910843
133163
Enable Stereo Rendering:
134164
Stereo Eye Separation: 0.06
135165
Stereo Focal Distance: 1
136166
Swap Stereo Eyes: false
137167
Value: false
138168
Focal Point:
139-
X: -0.00595993
140-
Y: 0.024299
141-
Z: 0.0573712
169+
X: 0.0172839
170+
Y: 0.0316376
171+
Z: 0.0972207
142172
Name: Current View
143173
Near Clip Distance: 0.01
144-
Pitch: 0.559796
174+
Pitch: 0.204797
145175
Target Frame: <Fixed Frame>
146176
Value: Orbit (rviz)
147-
Yaw: 6.18858
177+
Yaw: 4.16858
148178
Saved: ~
149179
Window Geometry:
150180
Displays:
151-
collapsed: false
152-
Height: 876
153-
Hide Left Dock: false
154-
Hide Right Dock: false
155-
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000037a000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
181+
collapsed: true
182+
Height: 521
183+
Hide Left Dock: true
184+
Hide Right Dock: true
185+
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000017ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000280000017f000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002e2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002e2000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003ba0000003efc0100000002fb0000000800540069006d00650000000000000003ba000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002c1000001c300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
156186
Selection:
157187
collapsed: false
158188
Time:
159189
collapsed: false
160190
Tool Properties:
161191
collapsed: false
162192
Views:
163-
collapsed: false
164-
Width: 1535
165-
X: 65
166-
Y: 24
193+
collapsed: true
194+
Width: 705
195+
X: 386
196+
Y: 292

‎cob_grasp_generation/launch/show_grasp_rviz.launch

+6-7
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,15 @@
11
<?xml version="1.0"?>
22
<launch>
33

4-
<arg name="gripper"/>
5-
<arg name="object"/>
4+
<arg name="gripper"/>
65

7-
<!-- upload robot description -->
8-
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find cob_grasp_generation)/files/urdf/$(arg gripper).urdf.xacro'"/>
9-
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>
6+
<!-- upload robot description -->
7+
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find cob_grasp_generation)/files/urdf/$(arg gripper).urdf.xacro'"/>
8+
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen"/>
109

1110
<node pkg="cob_grasp_generation" type="show_grasps_rviz_server.py" name="show_grasps_rviz_server" output="screen"/>
1211

13-
<!-- rviz visualization -->
14-
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find cob_grasp_generation)/launch/show_grasp.rviz" required="true" />
12+
<!-- rviz visualization -->
13+
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find cob_grasp_generation)/launch/show_grasp.rviz" required="true" />
1514

1615
</launch>

‎cob_grasp_generation/scripts/show_grasps_rviz_server.py

+12-6
Original file line numberDiff line numberDiff line change
@@ -34,8 +34,11 @@ class ShowGraspsRvizServer(object):
3434
def __init__(self):
3535
self.joint_names = []
3636
self.joint_positions = []
37+
self.joint_mimic = []
3738
for joint in URDF.from_parameter_server().joints:
38-
if joint.type == 'revolute':
39+
if joint.mimic:
40+
self.joint_mimic.append(joint)
41+
elif joint.type == 'revolute':
3942
self.joint_names.append(joint.name)
4043
if joint.limit.lower < 0.0 and 0.0 < joint.limit.upper:
4144
self.joint_positions.append(0.0)
@@ -75,7 +78,6 @@ def __init__(self):
7578

7679
print("ShowGraspsRvizServer: action started...")
7780

78-
7981
def show_cb(self, goal):
8082
success = False
8183

@@ -85,11 +87,15 @@ def show_cb(self, goal):
8587
self.marker.mesh_resource = "package://cob_grasp_generation/files/meshes/"+goal.object_name+".stl"
8688
grasp_list = grasp_query_utils.get_grasps(goal.object_name, goal.gripper_type, goal.grasp_id, 1)
8789
if grasp_list:
88-
print grasp_list
89-
#self.js.name = grasp_list[0].pre_grasp_posture.joint_names #TODO: joint names in urdf are "side-independend"
90-
self.js.position = grasp_list[0].grasp_posture.points[0].positions #TODO: has problems with sdh mimic joint
90+
#print grasp_list
91+
self.js.name = grasp_list[0].pre_grasp_posture.joint_names #TODO: joint names in urdf are "side-independend"
92+
self.js.position = grasp_list[0].grasp_posture.points[0].positions
93+
for joint in self.joint_mimic:
94+
idx = grasp_list[0].pre_grasp_posture.joint_names.index(joint.mimic.joint)
95+
self.js.name.append(joint.name)
96+
self.js.position.append(grasp_list[0].grasp_posture.points[0].positions[idx])
9197
self.t.transform.translation = grasp_list[0].grasp_pose.pose.position
92-
#self.t.transform.rotation = grasp_list[0].grasp_pose.pose.orientation #TODO: quaternions seem to be wrong, i.e. not normalized
98+
self.t.transform.rotation = grasp_list[0].grasp_pose.pose.orientation
9399
success = True
94100
else:
95101
success = False

‎cob_grasp_generation/src/cob_grasp_generation/grasp_query_utils.py

+6-2
Original file line numberDiff line numberDiff line change
@@ -99,10 +99,12 @@ def _fill_grasp_msg(gripper_type, grasp):
9999
joint_config = JointTrajectory()
100100
#joint_config.header.stamp = rospy.Time.now()
101101
#joint_config.header.frame_id = ""
102+
103+
#ToDo: get rid of this hardcoded-joint names
102104
if gripper_type == "sdh":
103105
joint_config.joint_names = ['sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint']
104106
elif gripper_type == "sdhx":
105-
joint_config.joint_names = ['gripper_left_finger_1_joint', 'gripper_left_finger_2_joint']
107+
joint_config.joint_names = ['gripper_finger_1_joint', 'gripper_finger_2_joint']
106108
else:
107109
rospy.logerr("Gripper not supported")
108110
return Grasp()
@@ -126,11 +128,13 @@ def _fill_grasp_msg(gripper_type, grasp):
126128
pre_joint_config = JointTrajectory()
127129
#pre_joint_config.header.stamp = rospy.Time.now()
128130
#pre_joint_config.header.frame_id = ""
131+
132+
#ToDo: get rid of this hardcoded-joint names and open_config
129133
if gripper_type == "sdh":
130134
pre_joint_config.joint_names = ['sdh_knuckle_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint']
131135
open_config = [0.0, -0.9854, 0.9472, -0.9854, 0.9472, -0.9854, 0.9472]
132136
elif gripper_type == "sdhx":
133-
pre_joint_config.joint_names = ['gripper_left_finger_1_joint', 'gripper_left_finger_2_joint']
137+
pre_joint_config.joint_names = ['gripper_finger_1_joint', 'gripper_finger_2_joint']
134138
open_config = [0.85, -1.4]
135139
else:
136140
rospy.logerr("Gripper not supported")

0 commit comments

Comments
 (0)