updating wam60 calibration, adding feedback pathway to cart servo

config/wam_60.yaml

@@ -1,10 +1,9 @@
 # These are the hardware calibrations for WAM#60
 # Zerocal
 home_position: [0.0, -1.58153, 0.0, 3.1415, -1.5708, -1.5708, -3.0]
-home_resolver_offset: [1.18423, -1.35297, 1.29775, -0.707165, 0.7455146629124219,
-  -0.15339807878856426, 2.147573103039898]
+home_resolver_offset: [1.2271846303085132, -1.2164467647933144, 1.4035924209153618,
+  -0.47553404424454904, 0.510815602365918, -0.27918450339518586, 2.1675148532824107]
 # Velocity filtering
 velocity_cutoff: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 1.0]
-velocity_smoothing_factor: 0.95
 # Torque constant scaling
 torque_scales: [1.4, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]

launch/hw/orocos_component_params.launch

@@ -64,10 +64,10 @@
       velocity_tolerance: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0]
     </rosparam>
     <rosparam if="$(arg ee_hand)" subst_value="true">
-      p_gains: [350.0, 350.0, 350.0, 300.0, 15.0, 15.0, 2.0]
-      d_gains: [20.0, 10.0, 10.0, 11.0, 0.4, 0.5, 0.01]
-      position_tolerance: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.5]
-      velocity_tolerance: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0]
+      p_gains: [350.0, 300.0, 300.0, 250.0, 15.0, 15.0, 2.0]
+      d_gains: [10.0, 5.0, 5.0, 2.0, 0.4, 0.5, 0.01]
+      position_tolerance: [0.1, 0.1, 0.1, 0.2, 0.3, 0.3, 0.5]
+      velocity_tolerance: [3.0, 3.0, 3.0, 3.0, 3.0, 3.0, 4.0]
     </rosparam>
 
     <node pkg="tf" type="static_transform_publisher" name="$(anon stp)"
@@ -107,13 +107,13 @@
       sampling_resolution: 0.002
       stop_on_violation: true
       verbose: false
-      stop_time: 0.5
+      stop_time: 1.0
       #note: absolute max joint velocity: 2.0 rad/s
       #note: safe joint velocity: 0.5 rad/s
       #max_velocities: [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]
       #max_accelerations: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
       #max_jerks: [2.5, 2.5, 2.5, 2.5, 5.5, 5.5, 5.0]
-      temporal_tolerance: 0.0
+      temporal_tolerance: 0.005
       goal_position_tolerance: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]
       goal_velocity_tolerance: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
     </rosparam>
@@ -153,15 +153,15 @@
       target_frame: ""#$(arg tf_prefix)/cmd
       singularity_avoidance_gain: 0.0
       joint_center_gain: 0.0
-      linear_p_gain: 1000.0
-      linear_d_gain: 10.0
-      angular_p_gain: 10.0
-      angular_d_gain: 0.005
+      linear_p_gain: 500.0
+      linear_d_gain: 5.0
+      angular_p_gain: 5.0
+      angular_d_gain: 0.002
       linear_effort_threshold: 50.0
       linear_position_threshold: 0.1
       angular_effort_threshold: 5.0
       angular_position_threshold: 1.5
-      nullspace_damping: 1.2
+      nullspace_damping: 0.8
       jointspace_damping: 1.2
       joint_d_gains: [0.8, 0.8, 0.8, 0.8, 0.1, 0.1, 0.2]
       projector_type: 3
@@ -187,6 +187,7 @@
       max_angular_error: 0.5
       linear_p_gain: 100.0
       angular_p_gain: 10.0
+      max_effort_norm_error: 0.5
     </rosparam>
 
     <rosparam if="$(arg ee_empty)" subst_value="true">

lua/load_controllers.lua

@@ -11,7 +11,8 @@ function load_controllers(depl, scheme, prefix)
 
   --[[ get required components --]]
   barrett_manager = depl:getPeer(prefix.."barrett_manager")
-  effort_sum = depl:getPeer(prefix.."effort_sum")
+  effort_sum_name = prefix.."effort_sum"
+  effort_sum = depl:getPeer(effort_sum_name)
   tf = depl:getPeer("tf")
 
   local wam = barrett_manager:getName()..".wam"
@@ -82,8 +83,10 @@ function load_controllers(depl, scheme, prefix)
   cart_servo_name = prefix.."cart_servo"
   depl:loadComponent(cart_servo_name,"lcsr_controllers::CartesianLogisticServo");
   cart_servo = depl:getPeer(cart_servo_name)
-  connect(wam, "position_out", cart_servo, "positions_in");
-  connect(                     cart_servo, "framevel_out", jtns, "framevel_in");
+  connect(effort_sum, "sum_out", cart_servo, "effort_cmd_in");
+  connect(wam, "effort_out",     cart_servo, "effort_in");
+  connect(wam, "position_out",   cart_servo, "positions_in");
+  connect(                       cart_servo, "framevel_out", jtns, "framevel_in");
   cart_servo:connectPeers(tf)
 
   --[[ Create a coulomb friction compensator --]]
@@ -152,6 +155,8 @@ function load_controllers(depl, scheme, prefix)
   scheme:addGroup(cart_imp_control);
   scheme:addToGroup(jtns_name, cart_imp_control);
   scheme:addToGroup(cart_servo_name, cart_imp_control);
+  --[[ latch connections from effort sum to the cart_servo --]]
+  scheme:latchConnections(effort_sum_name, cart_servo_name, true);
 
   --[[ Create an IK control group --]]
   ik_control = prefix.."ik_control"