Tweaked gains for PR2 on real robot. Better, but not perfect.

SemRoCo · Sep 2, 2016 · 31cdb7e · 31cdb7e
1 parent c02e5e3
commit 31cdb7e
Show file tree

Hide file tree

Showing 6 changed files with 28 additions and 19 deletions.
diff --git a/config/pr2_controller_action_server.yaml b/config/pr2_controller_action_server.yaml
@@ -1,8 +1,8 @@
 thresholds:
   bodypart_moves:
-    left_arm: 0.03
-    right_arm: 0.03
-    torso: 0.005
+    left_arm: 0.01
+    right_arm: 0.01
+    torso: 0.002
   motion_old: 0.5
 body_controllables:
   right_arm:

diff --git a/controller_specs/iai_pr2_cart_cart_control.yaml b/controller_specs/iai_pr2_cart_cart_control.yaml
@@ -127,16 +127,19 @@ scope:
       - r_gripper_offset
 
   # control params
-  - pos_p_gain: 1.0
-  - rot_p_gain: 1.0
+  - pos_p_gain: 3.0
+  - rot_p_gain: 3.0
   - pos_thresh: 0.05
   - rot_thresh: 0.1
   - weight_arm_joints: 0.001
+  - weight_torso_joint: 0.01
   - 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
+  - neg_vel_limit_torso_joint: -0.02
+  - pos_vel_limit_torso_joint: 0.02
 
   # definition EE goals and control laws
   # left arm position
@@ -198,7 +201,7 @@ scope:
 
 controllable-constraints:
   # torso joint
-  - controllable-constraint: [-0.02, 0.02, 100.0, 0, torso_lift_joint]
+  - controllable-constraint: [neg_vel_limit_torso_joint, pos_vel_limit_torso_joint, weight_torso_joint, 0, torso_lift_joint]
   # left arm joints
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 1, l_shoulder_pan_joint]
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 2, l_shoulder_lift_joint]

diff --git a/controller_specs/iai_pr2_cart_joint_control.yaml b/controller_specs/iai_pr2_cart_joint_control.yaml
@@ -97,16 +97,19 @@ scope:
       - l_gripper_offset
 
   # control params
-  - pos_p_gain: 1.0
-  - rot_p_gain: 1.0
+  - pos_p_gain: 3.0
+  - rot_p_gain: 3.0
   - pos_thresh: 0.05
   - rot_thresh: 0.1
   - weight_arm_joints: 0.001
+  - weight_torso_joint: 0.01
   - 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
+  - neg_vel_limit_torso_joint: -0.02
+  - pos_vel_limit_torso_joint: 0.02
 
   # definition EE goals and control laws
   # left arm position
@@ -167,7 +170,7 @@ scope:
 
 controllable-constraints:
   # torso joint
-  - controllable-constraint: [-0.02, 0.02, 100.0, 0, torso_lift_joint]
+  - controllable-constraint: [neg_vel_limit_torso_joint, pos_vel_limit_torso_joint, weight_torso_joint, 0, torso_lift_joint]
   # left arm joints
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 1, l_shoulder_pan_joint]
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 2, l_shoulder_lift_joint]

diff --git a/controller_specs/pr2_cart_cart_control.yaml b/controller_specs/pr2_cart_cart_control.yaml
@@ -198,7 +198,7 @@ scope:
 
 controllable-constraints:
   # torso joint
-  - controllable-constraint: [-0.02, 0.02, 100.0, 0, torso_lift_joint]
+  - controllable-constraint: [neg_vel_limit_torso_joint, pos_vel_limit_torso_joint, weight_torso_joint, 0, torso_lift_joint]
   # left arm joints
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 1, l_shoulder_pan_joint]
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 2, l_shoulder_lift_joint]

diff --git a/controller_specs/pr2_joint_cart_control.yaml b/controller_specs/pr2_joint_cart_control.yaml
@@ -98,16 +98,19 @@ scope:
       - r_gripper_offset
 
   # control params
-  - pos_p_gain: 1.0
-  - rot_p_gain: 1.0
+  - pos_p_gain: 3.0
+  - rot_p_gain: 3.0
   - pos_thresh: 0.05
   - rot_thresh: 0.1
   - weight_arm_joints: 0.001
+  - weight_torso_joint: 0.01
   - 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
+  - neg_vel_limit_torso_joint: -0.02
+  - pos_vel_limit_torso_joint: 0.02
 
   # definition EE goals and control laws
   # right arm position
@@ -168,7 +171,7 @@ scope:
 
 controllable-constraints:
   # torso joint
-  - controllable-constraint: [-0.02, 0.02, 100.0, 0, torso_lift_joint]
+  - controllable-constraint: [neg_vel_limit_torso_joint, pos_vel_limit_torso_joint, weight_torso_joint, 0, torso_lift_joint]
   # left arm joints
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 1, l_shoulder_pan_joint]
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 2, l_shoulder_lift_joint]

diff --git a/controller_specs/pr2_joint_joint_control.yaml b/controller_specs/pr2_joint_joint_control.yaml
@@ -114,17 +114,17 @@ scope:
         - 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
+  - weight_arm_joints: 0.001
+  - weight_torso_joint: 0.01
+  - weight_pos_control: 1
   - neg_vel_limit_arm_joints: -0.6
   - pos_vel_limit_arm_joints: 0.6
-
+  - neg_vel_limit_torso_joint: -0.02
+  - pos_vel_limit_torso_joint: 0.02
 
 controllable-constraints:
   # torso joint
-  - controllable-constraint: [-0.02, 0.02, 100.0, 0, torso_lift_joint]
+  - controllable-constraint: [neg_vel_limit_torso_joint, pos_vel_limit_torso_joint, weight_torso_joint, 0, torso_lift_joint]
   # left arm joints
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 1, l_shoulder_pan_joint]
   - controllable-constraint: [neg_vel_limit_arm_joints, pos_vel_limit_arm_joints, weight_arm_joints, 2, l_shoulder_lift_joint]