-
Notifications
You must be signed in to change notification settings - Fork 9
/
Copy pathpr2_left_arm_position_control.yaml
119 lines (107 loc) · 4.27 KB
/
pr2_left_arm_position_control.yaml
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#
# Copyright (C) 2015 Georg Bartels <[email protected]>
#
# This file is part of giskard.
#
# giskard is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program; if not, write to the Free Software
# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#
scope:
# definition of some nice short-cuts
- unit_x: {vector3: [1, 0, 0]}
- unit_y: {vector3: [0, 1, 0]}
- unit_z: {vector3: [0, 0, 1]}
- identity_rotation: {axis-angle: [unit-x, 0]}
- zero_vec: {vector3: [0, 0, 0]}
# definition of joint input variables
# - torso_lift_joint: {input-var: 0}
- l_shoulder_pan_joint: {input-var: 0}
- l_shoulder_lift_joint: {input-var: 1}
- l_upper_arm_roll_joint: {input-var: 2}
- l_elbow_flex_joint: {input-var: 3}
- l_forearm_roll_joint: {input-var: 4}
- l_wrist_flex_joint: {input-var: 5}
- l_wrist_roll_joint: {input-var: 6}
- goal_x: {input-var: 7}
- goal_y: {input-var: 8}
- goal_z: {input-var: 9}
# definition of joint transforms
# - torso_lift:
# frame: [{axis-angle: [unit_x, 0]}, {vector3: [-0.05, 0, {double-add: [0.739675, torso_lift_joint]}]}]
- l_shoulder_pan:
frame: [{axis-angle: [unit_z, l_shoulder_pan_joint]}, {vector3: [0.0, 0.188, 0.0]}]
- l_shoulder_lift:
frame: [{axis-angle: [unit_y, l_shoulder_lift_joint]}, {vector3: [0.1, 0, 0]}]
- l_upper_arm_roll:
frame: [{axis-angle: [unit_x, l_upper_arm_roll_joint]}, {vector3: [0, 0, 0]}]
- l_elbow_flex:
frame: [{axis-angle: [unit_y, l_elbow_flex_joint]}, {vector3: [0.4, 0, 0]}]
- l_forearm_roll:
frame: [{axis-angle: [unit_x, l_forearm_roll_joint]}, {vector3: [0, 0, 0]}]
- l_wrist_flex:
frame: [{axis-angle: [unit_y, l_wrist_flex_joint]}, {vector3: [0.321, 0, 0]}]
- l_wrist_roll:
frame: [{axis-angle: [unit_x, l_wrist_roll_joint]}, {vector3: [0, 0, 0]}]
# defintion of entire FK, goal, and control law
- pr2_fk:
frame-mul:
# - torso_lift
- l_shoulder_pan
- l_shoulder_lift
- l_upper_arm_roll
- l_elbow_flex
- l_forearm_roll
- l_wrist_flex
- l_wrist_roll
- pr2_fk_pos: {origin-of: pr2_fk}
- pr2_fk_goal: {vector3: [goal_x, goal_y, goal_z]}
- pr2_fk_error_vector: {vector-sub: [pr2_fk_goal, pr2_fk_pos]}
- pr2_fk_error: {vector-norm: pr2_fk_error_vector}
- pr2_fk_control_law: {double-mul: [-1.0, pr2_fk_error]}
controllable-constraints:
# - controllable-constraint: [-0.1, 0.1, 10.0, 0]
- controllable-constraint: [-0.3, 0.3, 1.0, 0]
- controllable-constraint: [-0.3, 0.3, 1.0, 1]
- controllable-constraint: [-0.3, 0.3, 1.0, 2]
- controllable-constraint: [-0.3, 0.3, 1.0, 3]
- controllable-constraint: [-0.3, 0.3, 1.0, 4]
- controllable-constraint: [-0.3, 0.3, 1.0, 5]
- controllable-constraint: [-0.3, 0.3, 1.0, 6]
soft-constraints:
- soft-constraint: [pr2_fk_control_law, pr2_fk_control_law, 10.0, pr2_fk_error]
hard-constraints:
# - hard-constraint:
# - {double-sub: [0.0115, torso_lift_joint]}
# - {double-sub: [0.325, torso_lift_joint]}
# - torso_lift_joint
- hard-constraint:
- {double-sub: [-0.5646, l_shoulder_pan_joint]}
- {double-sub: [2.1353, l_shoulder_pan_joint]}
- l_shoulder_pan_joint
- hard-constraint:
- {double-sub: [-0.3536, l_shoulder_lift_joint]}
- {double-sub: [1.2963, l_shoulder_lift_joint]}
- l_shoulder_lift_joint
- hard-constraint:
- {double-sub: [-0.65, l_upper_arm_roll_joint]}
- {double-sub: [3.75, l_upper_arm_roll_joint]}
- l_upper_arm_roll_joint
- hard-constraint:
- {double-sub: [-2.1213, l_elbow_flex_joint]}
- {double-sub: [-0.15, l_elbow_flex_joint]}
- l_elbow_flex_joint
- hard-constraint:
- {double-sub: [-2.0, l_wrist_flex_joint]}
- {double-sub: [-0.1, l_wrist_flex_joint]}
- l_wrist_flex_joint