Corrected Cartesian orientation control.

SemRoCo · Sep 2, 2016 · a45d2b3 · a45d2b3
1 parent 716a3b4
commit a45d2b3
Show file tree

Hide file tree

Showing 3 changed files with 103 additions and 56 deletions.
diff --git a/controller_specs/pr2_cart_cart_control.yaml b/controller_specs/pr2_cart_cart_control.yaml
@@ -127,10 +127,16 @@ scope:
       - r_gripper_offset
 
   # control params
-  - pos_p_gain: 6.0
-  - rot_p_gain: -2.0
+  - pos_p_gain: 1.0
+  - rot_p_gain: 1.0
   - pos_thresh: 0.05
-  - rot_thresh: 0.2
+  - rot_thresh: 0.1
+  - weight_arm_joints: 0.001
+  - weight_pos_control: 1
+  - weight_rot_control: 1
+  - weight_elbow_control: 0
+  - neg_vel_limit_arm_joints: -0.6
+  - pos_vel_limit_arm_joints: 0.6
 
   # definition EE goals and control laws
   # left arm position
@@ -147,8 +153,20 @@ scope:
         - {axis-angle: [unit_z, l_goal_rot_z]}
         - {axis-angle: [unit_y, l_goal_rot_y]}
         - {axis-angle: [unit_x, l_goal_rot_x]}
-  - l_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: {orientation-of: left_ee}}, l_goal_rot]}}}
-  - l_rot_control: {double-mul: [rot_p_gain, {min: [rot_thresh, l_rot_error]}]}
+  - l_rot: {orientation-of: left_ee}
+  - l_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: l_rot}, l_goal_rot]}}}
+  - l_rot_scaling: 
+      double-if:
+      - {double-sub: [rot_thresh, l_rot_error]}
+      - 1
+      - {double-div: [rot_thresh, l_rot_error]}
+  - l_intermediate_goal_rot:
+      slerp:
+      - l_rot
+      - l_goal_rot
+      - l_rot_scaling
+  - l_rot_control:
+      scale-vector: [rot_p_gain, {rotate-vector: [l_rot, {rot-vector: {rotation-mul: [{inverse-rotation: l_rot}, l_intermediate_goal_rot]}}]}]
   # right arm position
   - r_goal_trans: {vector3: [r_goal_x, r_goal_y, r_goal_z]}
   - r_trans: {origin-of: right_ee}
@@ -163,17 +181,20 @@ scope:
         - {axis-angle: [unit_z, r_goal_rot_z]}
         - {axis-angle: [unit_y, r_goal_rot_y]}
         - {axis-angle: [unit_x, r_goal_rot_x]}
-  - r_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: {orientation-of: right_ee}}, r_goal_rot]}}}
-  - r_rot_control: {double-mul: [rot_p_gain, {min: [rot_thresh, r_rot_error]}]}
-
-  # some constants
-  - weight_arm_joints: 1.0
-  - weight_pos_control: 20.0
-  - weight_rot_control: 5.0
-  - weight_elbow_control: 0.1
-  - neg_vel_limit_arm_joints: -0.6
-  - pos_vel_limit_arm_joints: 0.6
-
+  - r_rot: {orientation-of: right_ee}
+  - r_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: r_rot}, r_goal_rot]}}}
+  - r_rot_scaling: 
+      double-if:
+      - {double-sub: [rot_thresh, r_rot_error]}
+      - 1
+      - {double-div: [rot_thresh, r_rot_error]}
+  - r_intermediate_goal_rot:
+      slerp:
+      - r_rot
+      - r_goal_rot
+      - r_rot_scaling
+  - r_rot_control:
+      scale-vector: [rot_p_gain, {rotate-vector: [r_rot, {rot-vector: {rotation-mul: [{inverse-rotation: r_rot}, r_intermediate_goal_rot]}}]}]
 
 controllable-constraints:
   # torso joint
@@ -202,12 +223,16 @@ soft-constraints:
   - soft-constraint: [{x-coord: r_trans_control}, {x-coord: r_trans_control}, weight_pos_control, {x-coord: r_trans}, right EE x-pos control slack]
   - soft-constraint: [{y-coord: r_trans_control}, {y-coord: r_trans_control}, weight_pos_control, {y-coord: r_trans}, right EE y-pos control slack]
   - soft-constraint: [{z-coord: r_trans_control}, {z-coord: r_trans_control}, weight_pos_control, {z-coord: r_trans}, right EE z-pos control slack]
-  - soft-constraint: [l_rot_control, l_rot_control, weight_rot_control, l_rot_error, left EE rotation control slack]
-  - soft-constraint: [0.1, 0.1, weight_elbow_control, {y-coord: {origin-of: left_elbow}}, left elbow out control slack]
-  - soft-constraint: [0.1, 0.1, weight_elbow_control, {z-coord: {origin-of: left_elbow}}, left elbow up control slack]
-  - soft-constraint: [r_rot_control, r_rot_control, weight_rot_control, r_rot_error, right EE rotation control slack]
-  - soft-constraint: [-0.1, -0.1, weight_elbow_control, {y-coord: {origin-of: right_elbow}}, right elbow out control slack]
-  - soft-constraint: [0.1, 0.1, weight_elbow_control, {z-coord: {origin-of: right_elbow}}, right elbow up control slack]
+  - soft-constraint: [{x-coord: l_rot_control}, {x-coord: l_rot_control}, weight_rot_control, {x-coord: {rot-vector: l_rot}}, left EE x-rot control slack]
+  - soft-constraint: [{y-coord: l_rot_control}, {y-coord: l_rot_control}, weight_rot_control, {y-coord: {rot-vector: l_rot}}, left EE y-rot control slack]
+  - soft-constraint: [{z-coord: l_rot_control}, {z-coord: l_rot_control}, weight_rot_control, {z-coord: {rot-vector: l_rot}}, left EE z-rot control slack]
+  - soft-constraint: [{x-coord: r_rot_control}, {x-coord: r_rot_control}, weight_rot_control, {x-coord: {rot-vector: r_rot}}, right EE x-rot control slack]
+  - soft-constraint: [{y-coord: r_rot_control}, {y-coord: r_rot_control}, weight_rot_control, {y-coord: {rot-vector: r_rot}}, right EE y-rot control slack]
+  - soft-constraint: [{z-coord: r_rot_control}, {z-coord: r_rot_control}, weight_rot_control, {z-coord: {rot-vector: r_rot}}, right EE z-rot control slack]
+  - soft-constraint: [-0.01, -0.01, weight_elbow_control, {y-coord: {origin-of: right_elbow}}, right elbow out control slack]
+  - soft-constraint: [0.01, 0.01, weight_elbow_control, {z-coord: {origin-of: right_elbow}}, right elbow up control slack]
+  - soft-constraint: [0.01, 0.01, weight_elbow_control, {y-coord: {origin-of: left_elbow}}, left elbow out control slack]
+  - soft-constraint: [0.01, 0.01, weight_elbow_control, {z-coord: {origin-of: left_elbow}}, left elbow up control slack]
 
 hard-constraints:
   - hard-constraint:

diff --git a/controller_specs/pr2_cart_joint_control.yaml b/controller_specs/pr2_cart_joint_control.yaml
@@ -97,10 +97,16 @@ scope:
       - l_gripper_offset
 
   # control params
-  - pos_p_gain: 6.0
-  - rot_p_gain: -2.0
+  - pos_p_gain: 1.0
+  - rot_p_gain: 1.0
   - pos_thresh: 0.05
-  - rot_thresh: 0.2
+  - rot_thresh: 0.1
+  - weight_arm_joints: 0.001
+  - weight_pos_control: 1
+  - weight_rot_control: 1
+  - weight_elbow_control: 0
+  - neg_vel_limit_arm_joints: -0.6
+  - pos_vel_limit_arm_joints: 0.6
 
   # definition EE goals and control laws
   # left arm position
@@ -117,8 +123,20 @@ scope:
         - {axis-angle: [unit_z, l_goal_rot_z]}
         - {axis-angle: [unit_y, l_goal_rot_y]}
         - {axis-angle: [unit_x, l_goal_rot_x]}
-  - l_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: {orientation-of: left_ee}}, l_goal_rot]}}}
-  - l_rot_control: {double-mul: [rot_p_gain, {min: [rot_thresh, l_rot_error]}]}
+  - l_rot: {orientation-of: left_ee}
+  - l_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: l_rot}, l_goal_rot]}}}
+  - l_rot_scaling: 
+      double-if:
+      - {double-sub: [rot_thresh, l_rot_error]}
+      - 1
+      - {double-div: [rot_thresh, l_rot_error]}
+  - l_intermediate_goal_rot:
+      slerp:
+      - l_rot
+      - l_goal_rot
+      - l_rot_scaling
+  - l_rot_control:
+      scale-vector: [rot_p_gain, {rotate-vector: [l_rot, {rot-vector: {rotation-mul: [{inverse-rotation: l_rot}, l_intermediate_goal_rot]}}]}]
 
   # definition joint goals and control laws
   - r_shoulder_pan_error: {double-sub: [r_shoulder_pan_joint_goal, r_shoulder_pan_joint]}
@@ -147,15 +165,6 @@ scope:
         - {double-sub: [r_wrist_roll_error_normalized, two_pi]}
         - r_wrist_roll_error_normalized
 
-  # some constants
-  - weight_arm_joints: 1.0
-  - weight_pos_control: 20.0
-  - weight_rot_control: 5.0
-  - weight_elbow_control: 0.1
-  - neg_vel_limit_arm_joints: -0.6
-  - pos_vel_limit_arm_joints: 0.6
-
-
 controllable-constraints:
   # torso joint
   - controllable-constraint: [-0.02, 0.02, 100.0, 0, torso_lift_joint]
@@ -180,9 +189,11 @@ soft-constraints:
   - soft-constraint: [{x-coord: l_trans_control}, {x-coord: l_trans_control}, weight_pos_control, {x-coord: l_trans}, left EE x-pos control slack]
   - soft-constraint: [{y-coord: l_trans_control}, {y-coord: l_trans_control}, weight_pos_control, {y-coord: l_trans}, left EE y-pos control slack]
   - soft-constraint: [{z-coord: l_trans_control}, {z-coord: l_trans_control}, weight_pos_control, {z-coord: l_trans}, left EE z-pos control slack]
-  - soft-constraint: [l_rot_control, l_rot_control, weight_rot_control, l_rot_error, left EE rotation control slack]
-  - soft-constraint: [0.1, 0.1, weight_elbow_control, {y-coord: {origin-of: left_elbow}}, left elbow out control slack]
-  - soft-constraint: [0.1, 0.1, weight_elbow_control, {z-coord: {origin-of: left_elbow}}, left elbow up control slack]
+  - soft-constraint: [{x-coord: l_rot_control}, {x-coord: l_rot_control}, weight_rot_control, {x-coord: {rot-vector: l_rot}}, left EE x-rot control slack]
+  - soft-constraint: [{y-coord: l_rot_control}, {y-coord: l_rot_control}, weight_rot_control, {y-coord: {rot-vector: l_rot}}, left EE y-rot control slack]
+  - soft-constraint: [{z-coord: l_rot_control}, {z-coord: l_rot_control}, weight_rot_control, {z-coord: {rot-vector: l_rot}}, left EE z-rot control slack]
+  - soft-constraint: [0.01, 0.01, weight_elbow_control, {y-coord: {origin-of: left_elbow}}, left elbow out control slack]
+  - soft-constraint: [0.01, 0.01, weight_elbow_control, {z-coord: {origin-of: left_elbow}}, left elbow up control slack]
   - soft-constraint: [r_shoulder_pan_error, r_shoulder_pan_error , weight_pos_control, r_shoulder_pan_joint, r_shoulder_pan_joint control slack]
   - soft-constraint: [r_shoulder_lift_error, r_shoulder_lift_error , weight_pos_control, r_shoulder_lift_joint, r_shoulder_lift_joint control slack]
   - soft-constraint: [r_upper_arm_roll_error, r_upper_arm_roll_error , weight_pos_control, r_upper_arm_roll_joint, r_upper_arm_roll_joint control slack]

diff --git a/controller_specs/pr2_joint_cart_control.yaml b/controller_specs/pr2_joint_cart_control.yaml
@@ -98,10 +98,16 @@ scope:
       - r_gripper_offset
 
   # control params
-  - pos_p_gain: 6.0
-  - rot_p_gain: -2.0
+  - pos_p_gain: 1.0
+  - rot_p_gain: 1.0
   - pos_thresh: 0.05
-  - rot_thresh: 0.2
+  - rot_thresh: 0.1
+  - weight_arm_joints: 0.001
+  - weight_pos_control: 1
+  - weight_rot_control: 1
+  - weight_elbow_control: 0
+  - neg_vel_limit_arm_joints: -0.6
+  - pos_vel_limit_arm_joints: 0.6
 
   # definition EE goals and control laws
   # right arm position
@@ -118,8 +124,20 @@ scope:
         - {axis-angle: [unit_z, r_goal_rot_z]}
         - {axis-angle: [unit_y, r_goal_rot_y]}
         - {axis-angle: [unit_x, r_goal_rot_x]}
-  - r_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: {orientation-of: right_ee}}, r_goal_rot]}}}
-  - r_rot_control: {double-mul: [rot_p_gain, {min: [rot_thresh, r_rot_error]}]}
+  - r_rot: {orientation-of: right_ee}
+  - r_rot_error: {vector-norm: {rot-vector: {rotation-mul: [{inverse-rotation: r_rot}, r_goal_rot]}}}
+  - r_rot_scaling: 
+      double-if:
+      - {double-sub: [rot_thresh, r_rot_error]}
+      - 1
+      - {double-div: [rot_thresh, r_rot_error]}
+  - r_intermediate_goal_rot:
+      slerp:
+      - r_rot
+      - r_goal_rot
+      - r_rot_scaling
+  - r_rot_control:
+      scale-vector: [rot_p_gain, {rotate-vector: [r_rot, {rot-vector: {rotation-mul: [{inverse-rotation: r_rot}, r_intermediate_goal_rot]}}]}]
 
   # definition of joint goals and control laws
   - l_shoulder_pan_error: {double-sub: [l_shoulder_pan_joint_goal, l_shoulder_pan_joint]}
@@ -148,15 +166,6 @@ scope:
         - {double-sub: [l_wrist_roll_error_normalized, two_pi]}
         - l_wrist_roll_error_normalized
 
-  # some constants
-  - weight_arm_joints: 1.0
-  - weight_pos_control: 20.0
-  - weight_rot_control: 5.0
-  - weight_elbow_control: 0.1
-  - neg_vel_limit_arm_joints: -0.6
-  - pos_vel_limit_arm_joints: 0.6
-
-
 controllable-constraints:
   # torso joint
   - controllable-constraint: [-0.02, 0.02, 100.0, 0, torso_lift_joint]
@@ -181,9 +190,11 @@ soft-constraints:
   - soft-constraint: [{x-coord: r_trans_control}, {x-coord: r_trans_control}, weight_pos_control, {x-coord: r_trans}, right EE x-pos control slack]
   - soft-constraint: [{y-coord: r_trans_control}, {y-coord: r_trans_control}, weight_pos_control, {y-coord: r_trans}, right EE y-pos control slack]
   - soft-constraint: [{z-coord: r_trans_control}, {z-coord: r_trans_control}, weight_pos_control, {z-coord: r_trans}, right EE z-pos control slack]
-  - soft-constraint: [r_rot_control, r_rot_control, weight_rot_control, r_rot_error, right EE rotation control slack]
-  - soft-constraint: [-0.1, -0.1, weight_elbow_control, {y-coord: {origin-of: right_elbow}}, right elbow out control slack]
-  - soft-constraint: [0.1, 0.1, weight_elbow_control, {z-coord: {origin-of: right_elbow}}, right elbow up control slack]
+  - soft-constraint: [{x-coord: r_rot_control}, {x-coord: r_rot_control}, weight_rot_control, {x-coord: {rot-vector: r_rot}}, right EE x-rot control slack]
+  - soft-constraint: [{y-coord: r_rot_control}, {y-coord: r_rot_control}, weight_rot_control, {y-coord: {rot-vector: r_rot}}, right EE y-rot control slack]
+  - soft-constraint: [{z-coord: r_rot_control}, {z-coord: r_rot_control}, weight_rot_control, {z-coord: {rot-vector: r_rot}}, right EE z-rot control slack]
+  - soft-constraint: [-0.01, -0.01, weight_elbow_control, {y-coord: {origin-of: right_elbow}}, right elbow out control slack]
+  - soft-constraint: [0.01, 0.01, weight_elbow_control, {z-coord: {origin-of: right_elbow}}, right elbow up control slack]
   - soft-constraint: [l_shoulder_pan_error, l_shoulder_pan_error , weight_pos_control, l_shoulder_pan_joint, l_shoulder_pan_joint control slack]
   - soft-constraint: [l_shoulder_lift_error, l_shoulder_lift_error , weight_pos_control, l_shoulder_lift_joint, l_shoulder_lift_joint control slack]
   - soft-constraint: [l_upper_arm_roll_error, l_upper_arm_roll_error , weight_pos_control, l_upper_arm_roll_joint, l_upper_arm_roll_joint control slack]