Pouring controllers, as used on real PR2 today.

SemRoCo · Feb 26, 2016 · c912504 · c912504
1 parent d358101
commit c912504
Show file tree

Hide file tree

Showing 13 changed files with 1,378 additions and 12 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -40,6 +40,9 @@ target_link_libraries(pr2_controller ${catkin_LIBRARIES} yaml-cpp)
 add_executable(single_pose_controller src/${PROJECT_NAME}/single_pose_controller.cpp)
 target_link_libraries(single_pose_controller ${catkin_LIBRARIES} yaml-cpp)
 
+add_executable(yaml_controller src/${PROJECT_NAME}/yaml_controller.cpp)
+target_link_libraries(yaml_controller ${catkin_LIBRARIES} yaml-cpp)
+
 add_executable(whole_body_interactive_markers src/${PROJECT_NAME}/interactive_marker.cpp)
 target_link_libraries(whole_body_interactive_markers ${catkin_LIBRARIES})
 

diff --git a/controller_specs/moveabove.yaml b/controller_specs/moveabove.yaml
@@ -0,0 +1,188 @@
+#
+# Copyright (C) 2015-2016 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]}
+
+
+  # definition of joint input variables
+  - torso_lift_joint: {input-var: 0}
+  - l_shoulder_pan_joint: {input-var: 1}
+  - l_shoulder_lift_joint: {input-var: 2}
+  - l_upper_arm_roll_joint: {input-var: 3}
+  - l_elbow_flex_joint: {input-var: 4}
+  - l_forearm_roll_joint: {input-var: 5}
+  - l_wrist_flex_joint: {input-var: 6}
+  - l_wrist_roll_joint: {input-var: 7}
+  - r_shoulder_pan_joint: {input-var: 8}
+  - r_shoulder_lift_joint: {input-var: 9}
+  - r_upper_arm_roll_joint: {input-var: 10}
+  - r_elbow_flex_joint: {input-var: 11}
+  - r_forearm_roll_joint: {input-var: 12}
+  - r_wrist_flex_joint: {input-var: 13}
+  - r_wrist_roll_joint: {input-var: 14}
+
+  # definition of joint transforms
+  - torso_lift:
+      frame: [{axis-angle: [unit_x, 0]}, {vector3: [-0.05, 0, {double-add: [0.739675, torso_lift_joint]}]}]
+  - r_shoulder_pan:
+      frame: [{axis-angle: [unit_z, r_shoulder_pan_joint]}, {vector3: [0, -0.188, 0]}]
+  - r_shoulder_lift:
+      frame: [{axis-angle: [unit_y, r_shoulder_lift_joint]}, {vector3: [0.1, 0, 0]}]
+  - r_upper_arm_roll: 
+      frame: [{axis-angle: [unit_x, r_upper_arm_roll_joint]}, {vector3: [0, 0, 0]}]
+  - r_elbow_flex:
+      frame: [{axis-angle: [unit_y, r_elbow_flex_joint]}, {vector3: [0.4, 0, 0]}]
+  - r_forearm_roll:
+      frame: [{axis-angle: [unit_x, r_forearm_roll_joint]}, {vector3: [0, 0, 0]}]
+  - r_wrist_flex:
+      frame: [{axis-angle: [unit_y, r_wrist_flex_joint]}, {vector3: [0.321, 0, 0]}]
+  - r_wrist_roll:
+      frame: [{axis-angle: [unit_x, r_wrist_roll_joint]}, {vector3: [0, 0, 0]}]
+  - r_gripper_offset:
+      frame: [{axis-angle: [unit_x, 0]}, {vector3: [0.18, 0, 0]}]
+
+
+  # definition of elbow FK
+  - right_elbow:
+      frame-mul:
+      - torso_lift
+      - r_shoulder_pan
+      - r_shoulder_lift
+      - r_upper_arm_roll
+      - r_elbow_flex
+
+  # defintion of EE FK
+  - right_ee:
+      frame-mul:
+      - right_elbow
+      - r_forearm_roll
+      - r_wrist_flex
+      - r_wrist_roll
+      - r_gripper_offset
+
+  # definition of maker top
+  - maker_frame:
+      frame: [{quaternion: [0, 0, 0, 1]}, {vector3: [0.4, 0.0, 0.798]}]
+  - maker_top: {origin-of: maker_frame}
+  # definition of points on mug
+  - mug_bottom: {transform-vector: [right_ee, {vector3: [0, 0, -0.08]}]}
+  - mug_top: {transform-vector: [right_ee, {vector3: [0, 0, 0.06]}]}
+
+  # definition of feature points
+  - mug_bottom_to_maker_top:
+      cached-vector: {vector-sub: [mug_bottom, maker_top]}
+  - mug_top_to_mug_bottom:
+      cached-vector: {vector-sub: [mug_top, mug_bottom]}
+  - mug_bottom_behind_maker: {x-coord: mug_bottom_to_maker_top}
+  - mug_bottom_left_maker: {y-coord: mug_bottom_to_maker_top}
+  - mug_bottom_above_maker: {z-coord: mug_bottom_to_maker_top}
+  - mug_behind_itself: {x-coord: mug_top_to_mug_bottom}
+  - mug_left_itself: {y-coord: mug_top_to_mug_bottom}
+  - mug_above_itself: {z-coord: mug_top_to_mug_bottom}
+
+  # some constants
+  - weight_arm_joints: 0.001
+  - weight_pos_control: 1.0
+  - weight_elbow_control: 0.1
+  - neg_vel_limit_arm_joints: -0.5
+  - pos_vel_limit_arm_joints: 0.5
+
+
+controllable-constraints:
+  # torso joint
+  - controllable-constraint: [-0.05, 0.05, 50.0, 0]
+  # left arm joints
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 1]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 2]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 3]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 4]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 5]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 6]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 7]
+  # right arm joints
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 8]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 9]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 10]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 11]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 12]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 13]
+  - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 14]
+
+soft-constraints:
+  - soft-constraint: [-0.03, -0.01, weight_elbow_control, {y-coord: {origin-of: right_elbow}}]
+  #  - soft-constraint: [0.0, 0.03, weight_elbow_control, {z-coord: {origin-of: right_elbow}}]
+  - soft-constraint: [{double-sub: [0.2, torso_lift_joint]}, {double-sub: [0.3, torso_lift_joint]} , weight_pos_control, torso_lift_joint]
+  # TODO(YUEN): this where you fill the data from your model
+  - soft-constraint: [{double-sub: [0.0063, mug_bottom_behind_maker]}, {double-sub: [0.0703, mug_bottom_behind_maker]}, weight_pos_control, mug_bottom_behind_maker]
+  - soft-constraint: [{double-sub: [-0.1247, mug_bottom_left_maker]}, {double-sub: [-0.0842, mug_bottom_left_maker]}, weight_pos_control, mug_bottom_left_maker]
+  - soft-constraint: [{double-sub: [0.1797, mug_bottom_above_maker]}, {double-sub: [0.2383, mug_bottom_above_maker]}, weight_pos_control, mug_bottom_above_maker]
+  - soft-constraint: [{double-sub: [-0.0767, mug_behind_itself]}, {double-sub: [-0.034, mug_behind_itself]}, weight_pos_control, mug_behind_itself]
+  - soft-constraint: [{double-sub: [0.0696, mug_left_itself]}, {double-sub: [0.0965, mug_left_itself]}, weight_pos_control, mug_left_itself]
+  - soft-constraint: [{double-sub: [-0.0348, mug_above_itself]}, {double-sub: [-0.0241, mug_above_itself]}, weight_pos_control, mug_above_itself]
+
+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
+  - hard-constraint:
+      - {double-sub: [-2.1353, r_shoulder_pan_joint]}
+      - {double-sub: [0.5646, r_shoulder_pan_joint]}
+      - r_shoulder_pan_joint
+  - hard-constraint:
+      - {double-sub: [-0.3536, r_shoulder_lift_joint]}
+      - {double-sub: [1.2963, r_shoulder_lift_joint]}
+      -  r_shoulder_lift_joint
+  - hard-constraint:
+      - {double-sub: [-3.75, r_upper_arm_roll_joint]}
+      - {double-sub: [0.65, r_upper_arm_roll_joint]}
+      - r_upper_arm_roll_joint
+  - hard-constraint:
+      - {double-sub: [-2.1213, r_elbow_flex_joint]}
+      - {double-sub: [-0.15, r_elbow_flex_joint]}
+      - r_elbow_flex_joint
+  - hard-constraint:
+      - {double-sub: [-2.0, r_wrist_flex_joint]}
+      - {double-sub: [-0.1, r_wrist_flex_joint]}
+      - r_wrist_flex_joint
diff --git a/controller_specs/pr2_pouring_prepose_controller.yaml b/controller_specs/pr2_pouring_prepose_controller.yaml
@@ -103,6 +103,14 @@ scope:
   - mug_upright_error: {double-sub: [0.14, mug_upright]}
   - mug_upright_control: {double-mul: [-1.0, mug_upright_error]}
 
+  - mug_upright_x: {double-sub: [{x-coord: mug_top}, {x-coord: mug_bottom}]}
+  - mug_upright_error_x: {double-sub: [0.0, mug_upright]}
+  - mug_upright_control_x: {double-mul: [-1.0, mug_upright_error]}
+
+  - mug_upright_y: {double-sub: [{y-coord: mug_top}, {y-coord: mug_bottom}]}
+  - mug_upright_error_y: {double-sub: [0.0, mug_upright]}
+  - mug_upright_control_y: {double-mul: [-1.0, mug_upright_error]}
+
   # some constants
   - weight_arm_joints: 0.01
   - weight_pos_control: 1.0
@@ -135,6 +143,8 @@ soft-constraints:
   - soft-constraint: [mug_control, mug_control, weight_pos_control, mug_error]
   - soft-constraint: [-0.03, 0.0, weight_elbow_control, {y-coord: {origin-of: right_elbow}}]
   - soft-constraint: [0.0, 0.03, weight_elbow_control, {z-coord: {origin-of: right_elbow}}]
+  - soft-constraint: [mug_upright_control_x, mug_upright_control_x, weight_pos_control, mug_upright_error_x]
+  - soft-constraint: [mug_upright_control_y, mug_upright_control_y, weight_pos_control, mug_upright_error_y]
   - soft-constraint: [mug_upright_control, mug_upright_control, weight_pos_control, mug_upright_error]
   - soft-constraint: [{double-sub: [0.2, torso_lift_joint]}, {double-sub: [0.3, torso_lift_joint]} , weight_pos_control, torso_lift_joint]