Lfd modifications

README.md

@@ -55,6 +55,7 @@ sudo apt-get install libtar-dev protobuf-compiler libprotobuf-dev
 
 ```bash
 echo "export OROCOS_TARGET=xenomai" >> ~/.bashrc
+export XENOMAI_ROOT_DIR=/usr/xenomai
 wget http://web.barrett.com/support/WAM_Installer/libconfig-barrett_1.4.5-1_`dpkg --print-architecture`.deb
 sudo dpkg -i libconfig-barrett_1.4.5-1_`dpkg --print-architecture`.deb
 ```

config/wam_40.yaml

@@ -1,7 +1,8 @@
 # These are the hardware calibrations for WAM#40
 # Zerocal
-home_position: [0.0, -1.58153, 0.0, 3.1415, -1.5708, -1.5708, -3.0]
-home_resolver_offset: [-1.5109710760673565, -0.21935925266764755, -0.48166996739609047, -3.1109130378320806, -1.2011, -0.8851069146100148, -2.5065246074051366]
+home_position: [0.0, -1.5708, 0.0, 3.1415, -1.5708, -1.5708, -3.5112]
+home_resolver_offset: [0.3911651009108388, -0.7086991240031661, 0.25003886842535916,
+  -3.0786994412864814, -0.75318456685185, -0.9894176081862387, -2.4911847995262804]
 # Velocity filtering
 velocity_cutoff: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 1.0]
 velocity_smoothing_factor: 0.95

launch/sim/orocos_component_params.launch

@@ -93,13 +93,12 @@
       max_jerks: [2.5, 2.5, 2.5, 2.5, 5.5, 5.5, 5.0]
     </rosparam>
     <rosparam if="$(arg ee_hand)" subst_value="true">
-      position_tolerance: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
+      position_tolerance: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05]
       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]
+      #max_velocities: [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]
+      max_velocities: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+      max_accelerations: [1.0, 1.5, 1.0, 3.0, 6.0, 6.0, 6.0]
+      max_jerks: [2.0, 2.0, 2.0, 2.0, 4.0, 4.0, 4.0]
     </rosparam>
   </group>
 

launch/sim/wam7_sim.launch

@@ -20,6 +20,7 @@
   <arg name="initial_group" default="joint_control" doc="(string) Initial active controller group"/>
   <arg name="dash" default="true" doc="(bool) If true, display the barrett dashboard."/>
   <arg name="rescuer" default="false" doc="(bool) If true, use the singularity rescuer."/>
+  <arg name="bhand_action_server" default="false" doc="(bool) If true, launch bhand action server."/>
 
   <arg name="j0" default="0.0" doc="(float) Joint 0 initial position"/>
   <arg name="j1" default="0.0" doc="(float) Joint 1 initial position"/>
@@ -144,7 +145,7 @@
         <param name="hand_urdf_prefix" value="wam/hand"/>
 
         <!-- Mid-Level hand interface -->
-        <node if="0" name="bhand_action_server"
+        <node if="$(arg bhand_action_server)" name="bhand_action_server"
           pkg="oro_barrett_interface"
           type="bhand_action_server.py"
           output="screen"/>

lua/wam_sim.lua

@@ -10,7 +10,7 @@ ros = gs:provides("ros")
 ros:import("lcsr_barrett")
 
 --[ Set log-level ]--
-rtt.setLogLevel("Info")
+rtt.setLogLevel("Debug")
 
 --[[ add lua dir to the lua path --]]
 package.path = ros:find("lcsr_barrett") .. "/lua/?.lua" .. ";" .. package.path

models/manipulation_platform.urdf.xacro

@@ -23,6 +23,9 @@
         <geometry>
           <mesh filename="package://lcsr_barrett/models/meshes/manipulation_platform.stl"/>
         </geometry>
+        <material name="white">
+          <color rgba="0.75 0.80 1 1"/>
+        </material>
       </visual>
       <collision>
         <origin rpy="0 0 0 " xyz="0 0 0"/>

models/meshes/manipulation_platform.dae

@@ -5,8 +5,8 @@
       <author>Blender User</author>
       <authoring_tool>Blender 2.71.0 commit date:2014-06-12, commit time:18:39, hash:169c95b</authoring_tool>
     </contributor>
-    <created>2015-09-07T18:24:02</created>
-    <modified>2015-09-07T18:24:02</modified>
+    <created>2015-02-09T07:57:30</created>
+    <modified>2015-02-09T07:57:30</modified>
     <unit name="meter" meter="1"/>
     <up_axis>Z_UP</up_axis>
   </asset>
@@ -114,10 +114,10 @@
               <color sid="ambient">0 0 0 1</color>
             </ambient>
             <diffuse>
-              <color sid="diffuse">0.06400918 0.02817589 0.01199546 1</color>
+              <color sid="diffuse">0.64 0.64 0.64 1</color>
             </diffuse>
             <specular>
-              <color sid="specular">0.1185567 0.03223746 0.01195687 1</color>
+              <color sid="specular">0.5 0.5 0.5 1</color>
             </specular>
             <shininess>
               <float sid="shininess">50</float>
@@ -139,7 +139,7 @@
     <geometry id="Cube-mesh" name="Cube">
       <mesh>
         <source id="Cube-mesh-positions">
-          <float_array id="Cube-mesh-positions-array" count="24">0.4488365 0.2494305 0 0.4488365 -0.2494307 0 -0.4488366 -0.2494307 0 -0.4488363 0.2494306 0 0.4488366 0.2494304 0.3048 0.4488362 -0.2494308 0.3048 -0.4488367 -0.2494306 0.3048 -0.4488365 0.2494305 0.3048</float_array>
+          <float_array id="Cube-mesh-positions-array" count="24">0.5720641 0.3597956 0 0.5720641 -0.3597958 0 -0.5720642 -0.3597958 0 -0.5720639 0.3597958 0 0.5720643 0.3597955 0.3048 0.5720637 -0.3597961 0.3048 -0.5720643 -0.3597957 0.3048 -0.5720641 0.3597957 0.3048</float_array>
           <technique_common>
             <accessor source="#Cube-mesh-positions-array" count="8" stride="3">
               <param name="X" type="float"/>
@@ -149,7 +149,7 @@
           </technique_common>
         </source>
         <source id="Cube-mesh-normals">
-          <float_array id="Cube-mesh-normals-array" count="36">0 0 -1 0 0 1 1 -1.01559e-6 9.77766e-7 -2.65596e-7 -1 2.44442e-7 -1 3.58444e-7 -2.9333e-7 1.32798e-7 1 2.44442e-7 0 0 -1 0 0 1 1 0 -6.84436e-7 0 -1 -5.37771e-7 -1 4.77926e-7 -4.88883e-7 1.32798e-7 1 2.44442e-7</float_array>
+          <float_array id="Cube-mesh-normals-array" count="36">0 0 -1 0 0 1 1 -9.11143e-7 1.17332e-6 -3.12577e-7 -1 3.91106e-7 -1 3.31325e-7 -3.91107e-7 1.56288e-7 1 3.91107e-7 0 0 -1 0 0 1 1 0 -9.77766e-7 0 -1 -7.82213e-7 -1 4.14156e-7 -5.8666e-7 1.56288e-7 1 3.91106e-7</float_array>
           <technique_common>
             <accessor source="#Cube-mesh-normals-array" count="12" stride="3">
               <param name="X" type="float"/>
@@ -182,7 +182,7 @@
         <instance_light url="#Lamp-light"/>
       </node>
       <node id="Cube" name="Cube" type="NODE">
-        <matrix sid="transform">0.8208293 0 0 0 0 1.741765 0 0 0 0 1 0 0 0 0 1</matrix>
+        <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
         <instance_geometry url="#Cube-mesh">
           <bind_material>
             <technique_common>

scripts/hydra_teleop.py

@@ -51,6 +51,7 @@ def __init__(self):
         self.last_buttons = [0] * 16
         self.clutch_enabled = False
         self.clutch_enable_time = None
+        self.engage_augmenter = False
 
         self.cart_scale = None
         self.last_joy_cmd = rospy.Time.now()

scripts/marker_teleop.py

@@ -131,7 +131,7 @@ def menu_grasp_cb(self, msg):
         if not checked:
             self.menu_handler.setCheckState(handle, MenuHandler.CHECKED)
             rospy.loginfo('grasp')
-            self.finger_ref = 0.0
+            self.finger_ref = 1.0
         else:
             self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED)
             self.finger_ref = 0.25
@@ -147,10 +147,10 @@ def menu_release_cb(self, msg):
         if not checked:
             self.menu_handler.setCheckState(handle, MenuHandler.CHECKED)
             rospy.loginfo('release')
-            self.finger_ref = 1.0
+            self.finger_ref = 0.0
         else:
             self.menu_handler.setCheckState(handle, MenuHandler.UNCHECKED)
-            self.finger_ref = 0.25
+            self.finger_ref = 0.50
 
         self.transform.header.stamp = rospy.Time.now()