Merge pull request #24 from cpaxton/teleop_fixes

Teleop fixes

CMakeLists.txt

@@ -4,3 +4,5 @@ project(lcsr_barrett)
 
 find_package(catkin REQUIRED COMPONENTS)
 
+catkin_python_setup()
+

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,17 +64,25 @@
       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)"
-      args="0 0 0 0 0 0 /world /pid/world 50"/>
+      args="0 0 0 0 0 0 /world /pid/world 100"/>
+    <node pkg="joint_state_publisher" type="joint_state_publisher" name="$(anon joint_state_publisher)">
+      <rosparam param="source_list" subst_value="true">
+        ["joint_state_desired", "hand/joint_states"]
+      </rosparam>
+      <param name="rate" value="10"/>
+      <remap from="robot_description" to="/robot_description"/>
+      <remap from="joint_states" to="joint_state_desired_full"/>
+    </node>
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
-      <remap from="joint_states" to="joint_state_desired"/>
-      <param name="publish_frequency" value="50"/>
+      <remap from="joint_states" to="joint_state_desired_full"/>
+      <param name="publish_frequency" value="10"/>
       <param name="tf_prefix" value="pid"/>
       <remap from="robot_description" to="/robot_description"/>
     </node>
@@ -99,12 +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.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>
@@ -144,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
@@ -178,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">
@@ -243,10 +253,18 @@
     </rosparam>
 
     <node pkg="tf" type="static_transform_publisher" name="$(anon stp)"
-      args="0 0 0 0 0 0 /world /ik/world 50"/>
-    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
-      <remap from="joint_states" to="joint_state_desired"/>
-      <param name="publish_frequency" value="50"/>
+      args="0 0 0 0 0 0 /world /ik/world 100"/>
+    <node pkg="joint_state_publisher" type="joint_state_publisher" name="$(anon joint_state_publisher)">
+      <rosparam param="source_list" subst_value="true">
+        ["joint_state_desired", "hand/joint_states"]
+      </rosparam>
+      <param name="rate" value="10"/>
+      <remap from="robot_description" to="/robot_description"/>
+      <remap from="joint_states" to="joint_state_desired_full"/>
+    </node>
+    <node pkg="robot_state_publisher" type="robot_state_publisher" name="$(anon robot_state_publisher)">
+      <remap from="joint_states" to="joint_state_desired_full"/>
+      <param name="publish_frequency" value="10"/>
       <param name="tf_prefix" value="ik"/>
       <remap from="robot_description" to="/robot_description"/>
     </node>

launch/hw/wam7.launch

@@ -18,32 +18,29 @@
   <arg name="aruco" default="false" doc="(bool) True if the end-effector is an aruco target"/>
 
 
-  <include unless="$(arg safe)" file="$(find rtt_ros)/launch/ldeployer.launch">
+  <include unless="$(arg safe)" file="$(find rtt_ros)/launch/deployer.launch">
     <arg name="NAME" value="barrett"/>
-    <arg name="DEPLOYER_ARGS" value="-g -s $(find lcsr_barrett)/lua/wam_hw.lua"/>
+    <arg name="DEPLOYER_ARGS" value="-s $(find lcsr_barrett)/lua/wam_hw.lua"/>
     <arg name="LOG_LEVEL" value="$(arg log_level)"/>
     <arg name="DEBUG" value="$(arg debug)"/>
   </include>
 
+  <!-- TODO: update safe mode to use rttlua program -->
+  <!--
   <node
     if="$(arg safe)"
     name="barrett"
     pkg="rtt_ros" type="ldeployer"
-    args="$(find lcsr_barrett)/lua/wam_hw.lua --"
+    args="$(find lcsr_barrett)/lua/wam_hw.lua - -"
     output="log">
   </node>
+  -->
 
   <param name="robot_description"
     command="$(find xacro)/xacro.py '$(find lcsr_barrett)/launch/hw/wam7.urdf.xacro'
     prefix:=wam
     hand:=$(arg hand) ball:=$(arg ball) bumblebee:=$(arg bumblebee) aruco:=$(arg aruco)" />
 
-  <node pkg="robot_state_publisher" type="robot_state_publisher"
-    name="wam_state_publisher">
-    <param name="publish_frequency" value="50"/>
-    <remap from="joint_states" to="wam_joint_states"/>
-  </node>
-
   <node pkg="rosbag" type="rosbag" name="rosbag_record_diag"
     args="record -j -o /tmp/wam-last.bag -e '/barrett(.*)' '(.*)joint_state(.*)' '(.*)wrench(.*)' /tf"/>
 
@@ -59,11 +56,11 @@
         <rosparam param="source_list" subst_value="true">
           ["wam/joint_states", "hand/joint_states"]
         </rosparam>
-        <param name="rate" value="50"/>
+        <param name="rate" value="100"/>
         <remap from="robot_description" to="/robot_description"/>
       </node>
       <node pkg="robot_state_publisher" type="robot_state_publisher" name="$(anon robot_state_publisher)">
-        <param name="publish_frequency" value="50"/>
+        <param name="publish_frequency" value="100"/>
       </node>
 
       <rosparam subst_value="true">
@@ -108,7 +105,7 @@
     </rosparam>
 
     <!-- Moveit -->
-    <include if="$(arg planning)" file="$(find lcsr_barrett)/sim/wam7_planning.launch">
+    <include if="$(arg planning)" file="$(find lcsr_barrett)/launch/wam7_planning.launch">
       <arg name="joint_states_topic" value="barrett_manager/joint_states"/>
       <arg name="ee_empty" value="$(arg empty)"/>
       <arg name="ee_hand" value="$(arg hand)"/>
@@ -130,6 +127,6 @@
   <param name="stage_model"
     command="$(find xacro)/xacro.py '$(find lcsr_barrett)/models/stage.urdf.xacro'" />
   <node name="stage_pose_broadcaster" pkg="tf" type="static_transform_publisher"
-    args="0 0 0 0 0 0 /world /stage_link 50"/>
+    args="0 0 0 0 0 0 /world /stage_link 100"/>
 
 </launch>

launch/hw/wam7.urdf.xacro

@@ -11,7 +11,7 @@
 
   <xacro:wam_7dof prefix="$(arg prefix)" parent_link="world" xyz="0.04 0.46 1" rpy="${PI} ${-PI/2} 0"/>
   <xacro:if value="$(arg hand)">
-    <xacro:bhand prefix="$(arg prefix)/hand" parent_link="$(arg prefix)/wrist_palm_link" xyz="0.0 0.0 0.06" rpy="0 0 ${PI}"/>
+    <xacro:bhand prefix="$(arg prefix)/hand" parent_link="$(arg prefix)/wrist_palm_link" xyz="0.0 0.0 0.06" rpy="0 0 ${PI}" primitive="true"/>
   </xacro:if>
   <xacro:if value="$(arg ball)">
     <xacro:bball prefix="$(arg prefix)/ball" parent_link="$(arg prefix)/wrist_palm_link" xyz="0.0 0.0 0.06" rpy="0 0 0"/>

launch/sim/orocos_component_params.launch

@@ -12,9 +12,10 @@
       robot_description_param: /robot_description
       root_link: $(arg tf_prefix)/base_link
       end_effector_frame_link: $(arg tf_prefix)/wrist_palm_link
-      gravity: [-9.8, 0.0, 0.0]
+      gravity: [0.0, -9.81, 0.0]
       inertia_mass_rate: 0.3
       inertia_com_rate: 0.05
+      max_attached_mass: 3.0
     </rosparam>
     <rosparam if="$(arg ee_empty)" subst_value="true">
       tip_link: $(arg tf_prefix)/wrist_palm_link
@@ -45,7 +46,7 @@
     </rosparam>
     <rosparam if="$(arg ee_empty)" subst_value="true">
       p_gains: [350.0, 300.0, 200.0, 170.0, 12.0, 12.0, 1.0]
-      d_gains: [20.0, 5.0, 3.0, 5.0, 0.4, 0.4, 0.001]
+      d_gains: [40.0, 40.0, 30.0, 20.0, 6.5, 4.5, 1.1]
       position_tolerance: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
       velocity_tolerance: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0]
     </rosparam>
@@ -57,7 +58,7 @@
     </rosparam>
     <rosparam if="$(arg ee_hand)" subst_value="true">
       p_gains: [350.0, 350.0, 350.0, 300.0, 25.0, 25.0, 2.0]
-      d_gains: [40.0, 40.0, 30.0, 20.0, 6.5, 4.5, 1.1]
+      d_gains: [10.0, 20.0, 20.0, 10.0, 4.5, 3.5, 0.8]
       position_tolerance: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
       velocity_tolerance: [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0]
     </rosparam>
@@ -73,14 +74,15 @@
       stop_on_violation: true
       verbose: false
       stop_time: 0.25
+      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>
     <rosparam if="$(arg ee_empty)" subst_value="true">
-      position_tolerance: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0]
-      velocity_tolerance: [1.2, 1.2, 1.2, 1.2, 2.0, 2.0, 2.0]
-      max_velocities: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
-      max_accelerations: [1.5, 1.5, 1.5, 4.0, 4.0, 4.0, 4.0]
+      position_tolerance: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
+      velocity_tolerance: [3.2, 3.2, 3.2, 3.2, 3.0, 3.0, 3.0]
+      max_velocities: [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]
+      max_accelerations: [1.5, 1.5, 1.5, 4.0, 8.0, 8.0, 8.0]
       max_jerks: [2.5, 2.5, 2.5, 2.5, 5.5, 5.5, 5.0]
     </rosparam>
     <rosparam if="$(arg ee_ball)" subst_value="true">
@@ -93,6 +95,8 @@
     <rosparam if="$(arg ee_hand)" subst_value="true">
       position_tolerance: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
       velocity_tolerance: [3.2, 3.2, 3.2, 3.2, 3.0, 3.0, 3.0]
+      goal_position_tolerance: [0.05, 0.05, 0.05, 0.05, 0.1, 0.1, 0.1]
+      goal_velocity_tolerance: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
       max_velocities: [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]
       max_accelerations: [1.5, 1.5, 1.5, 4.0, 8.0, 8.0, 8.0]
       max_jerks: [2.5, 2.5, 2.5, 2.5, 5.5, 5.5, 5.0]
@@ -105,16 +109,18 @@
       target_frame: "" #$(arg tf_prefix)/cmd
       singularity_avoidance_gain: 0.0
       joint_center_gain: 0.0
-      linear_p_gain: 1000.0
-      linear_d_gain:  200.0
-      angular_p_gain: 10.0
-      angular_d_gain:  0.1
+      linear_p_gain: 500.0
+      linear_d_gain:  100.0
+      angular_p_gain: 5.0
+      angular_d_gain:  1.5
+      #angular_p_gain: 5.0
+      #angular_d_gain:  1.5
       linear_effort_threshold: 50.0
       linear_position_threshold: 0.1
       angular_effort_threshold: 5.0
       angular_position_threshold: 1.5
-      nullspace_damping: 0.8
-      jointspace_damping: 1.2
+      nullspace_damping: 0.0
+      jointspace_damping: 0.0
       joint_d_gains: [0.8, 0.8, 0.8, 0.8, 0.1, 0.1, 0.5]
       projector_type: 3
     </rosparam>
@@ -133,12 +139,12 @@
     <rosparam subst_value="true">
       root_link: $(arg tf_prefix)/base_link
       target_frame: $(arg tf_prefix)/cmd
-      max_linear_rate: 0.1
-      max_linear_error: 0.05
-      max_angular_rate: 0.5
-      max_angular_error: 0.5
-      linear_p_gain: 100.0
-      angular_p_gain: 10.0
+      max_linear_rate: 0.5
+      max_linear_error: 0.1
+      max_angular_rate: 1.0
+      max_angular_error: 0.7
+      linear_p_gain: 200.0
+      angular_p_gain: 50.0
     </rosparam>
 
     <rosparam if="$(arg ee_empty)" subst_value="true">
@@ -206,9 +212,17 @@
 
     <node pkg="tf" type="static_transform_publisher" name="$(anon stp)"
       args="0 0 0 0 0 0 /world /ik/world 50"/>
+    <node pkg="joint_state_publisher" type="joint_state_publisher" name="$(anon joint_state_publisher)">
+      <rosparam param="source_list" subst_value="true">
+        ["joint_state_desired", "hand/joint_states"]
+      </rosparam>
+      <param name="rate" value="10"/>
+      <remap from="robot_description" to="/robot_description"/>
+      <remap from="joint_states" to="joint_state_desired_full"/>
+    </node>
     <node pkg="robot_state_publisher" type="robot_state_publisher" name="$(anon robot_state_publisher)">
-      <remap from="joint_states" to="joint_state_desired"/>
-      <param name="publish_frequency" value="50"/>
+      <remap from="joint_states" to="joint_state_desired_full"/>
+      <param name="publish_frequency" value="10"/>
       <param name="tf_prefix" value="ik"/>
       <remap from="robot_description" to="/robot_description"/>
     </node>