Devel

.gitignore

@@ -0,0 +1 @@
+*.pyc

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

@@ -12,7 +12,7 @@
       robot_description_param: /robot_description
       root_link: $(arg tf_prefix)/base_link
       end_effector_frame_link: $(arg tf_prefix)/wrist_palm_link
-      gravity: [0.0, -9.8, 0.0]
+      gravity: [0.0, -9.81, 0.0]
       inertia_mass_rate: 0.3
       inertia_com_rate: 0.05
       max_attached_mass: 3.0
@@ -58,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: [30.0, 30.0, 20.0, 10.0, 4.5, 3.5, 0.8]
+      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>

launch/sim/wam.world

@@ -7,10 +7,47 @@
     <scene>
       <background>0 0 0 0</background>
     </scene>
-    <include>
-      <uri>model://ground_plane</uri>
+    <!--<include>-->
+      <!--<uri>model://ground_plane</uri>-->
+      <!--<pose>0 0 -1.0 0 0 0</pose>-->
+    <!--</include>-->
+  <model name="ground_plane">
       <pose>0 0 -1.0 0 0 0</pose>
-    </include>
+    <static>true</static>
+    <link name="link">
+      <collision name="collision">
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>100 100</size>
+          </plane>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>100</mu>
+              <mu2>50</mu2>
+            </ode>
+          </friction>
+        </surface>
+      </collision>
+      <visual name="visual">
+        <cast_shadows>false</cast_shadows>
+        <geometry>
+          <plane>
+            <normal>0 0 1</normal>
+            <size>100 100</size>
+          </plane>
+        </geometry>
+        <material>
+          <script>
+            <uri>file://media/materials/scripts/gazebo.material</uri>
+            <name>Gazebo/FlatBlack</name>
+          </script>
+        </material>
+      </visual>
+    </link>
+  </model>
     <!--<include>-->
       <!--<uri>model://sun</uri>-->
       <!--</include>-->

lua/load_controllers.lua

@@ -81,6 +81,7 @@ function load_controllers(depl, scheme, prefix)
   connect(wam, "position_out", ik, "positions_in");
   connect(                     ik, "trajectories_out", traj_rml, "joint_traj_cmd_in");
   ik:connectPeers(tf)
+  ik:connectServices(tf)
 
   --[[ Create a cartesian interpolator --]]
   cart_servo_name = prefix.."cart_servo"
@@ -91,6 +92,7 @@ function load_controllers(depl, scheme, prefix)
   connect(wam, "position_out",   cart_servo, "positions_in");
   connect(                       cart_servo, "framevel_out", jtns, "framevel_in");
   cart_servo:connectPeers(tf)
+  cart_servo:connectServices(tf)
 
   --[[ Create a coulomb friction compensator --]]
   coulomb_name = prefix.."coulomb"

models/manipulation_platform.urdf.xacro

@@ -3,38 +3,48 @@
 
   <link name="world"/>
 
-    <joint name="platform_link_fixed_joint"  type="fixed">
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <parent link="world"/>
-      <child link="platform_link"/>
-    </joint>
+  <joint name="platform_link_fixed_joint"  type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <parent link="world"/>
+    <child link="platform_link"/>
+  </joint>
 
-    <link name = "platform_link">
-      <inertial>
-        <origin xyz="0 0 0" />
-        <mass value="25" />
-        <inertia
-          ixx="1" ixy="0"   ixz="0"
-                  iyy="1"   iyz="0"
-                            izz="1" />
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0 " xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://lcsr_barrett/models/meshes/manipulation_platform.stl"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0 " xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://lcsr_barrett/models/meshes/manipulation_platform.stl"/>
-        </geometry>
-      </collision>
-    </link>
+  <link name = "platform_link">
+    <inertial>
+      <origin xyz="0 0 0" />
+      <mass value="25" />
+      <inertia
+        ixx="1" ixy="0"   ixz="0"
+        iyy="1"   iyz="0"
+        izz="1" />
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0 " xyz="0 0 0"/>
+      <material name="white">
+        <color rgba="0.8 0.8 0.8 1"/>
+      </material>
+      <geometry>
+        <mesh filename="package://lcsr_barrett/models/meshes/manipulation_platform.stl"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0 " xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://lcsr_barrett/models/meshes/manipulation_platform.stl"/>
+      </geometry>
+    </collision>
+  </link>
 
-    <gazebo reference="platform_link">
-      <kp>50000</kp>
-      <kd>8</kd>
-    </gazebo>
+  <gazebo reference="platform_link">
+    <kp>50000</kp>
+    <kd>8</kd>
+    <visual name="visual">
+      <material>
+        <ambient>0 0 0 0</ambient>
+        <specular>0.8 0.8 0.8 0.1</specular>
+        <diffuse>1.0 1.0 1.0 1.0</diffuse>
+      </material>
+    </visual>
+  </gazebo>
 
 </robot>

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:11:28</created>
-    <modified>2015-09-07T18:11:28</modified>
+    <created>2015-09-07T18:24:02</created>
+    <modified>2015-09-07T18:24:02</modified>
     <unit name="meter" meter="1"/>
     <up_axis>Z_UP</up_axis>
   </asset>
@@ -182,7 +182,7 @@
         <instance_light url="#Lamp-light"/>
       </node>
       <node id="Cube" name="Cube" type="NODE">
-        <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+        <matrix sid="transform">0.8208293 0 0 0 0 1.741765 0 0 0 0 1 0 0 0 0 1</matrix>
         <instance_geometry url="#Cube-mesh">
           <bind_material>
             <technique_common>

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()
 
@@ -250,7 +250,7 @@ def cmd_thread(self):
             self.publish_cmd_ring_markers(rospy.Time.now())
 
             # Broadcast the command if it's defined
-            self.publish_cmd(self.resync_pose, self.finger_ref, rospy.Time.now())
+            self.publish_cmd(self.resync_pose, False, self.finger_ref, rospy.Time.now())
 
             r.sleep()
 

src/lcsr_barrett/wam_teleop.py

@@ -64,6 +64,7 @@ def __init__(self, input_ref_frame_id, input_frame_id):
         # Command state
         self.cmd_frame = None
         self.deadman_engaged = False
+        self.engage_augmenter = False
         self.cmd_scaling = 0.0
 
         # Hand structures
@@ -84,12 +85,12 @@ def __init__(self, input_ref_frame_id, input_frame_id):
                 sensor_msgs.msg.JointState,
                 self.hand_state_cb)
 
-            self.hand_pub = rospy.Publisher('hand/cmd', oro_barrett_msgs.msg.BHandCmd)
+            self.hand_pub = rospy.Publisher('hand/cmd', oro_barrett_msgs.msg.BHandCmd, queue_size=1000)
 
         # ROS generic telemanip command
-        self.telemanip_cmd_pub = rospy.Publisher('telemanip_cmd_out', TelemanipCommand)
+        self.telemanip_cmd_pub = rospy.Publisher('telemanip_cmd_out', TelemanipCommand, queue_size=1000)
         # goal marker
-        self.marker_pub = rospy.Publisher('master_target_markers', MarkerArray)
+        self.marker_pub = rospy.Publisher('master_target_markers', MarkerArray, queue_size=1000)
 
         self.master_target_markers = MarkerArray()
 
@@ -214,7 +215,7 @@ def handle_cart_cmd(self, scaling):
                 # Update commanded TF frame
                 cmd_twist = kdl.diff(self.cmd_origin, input_frame)
                 cmd_twist.vel = self.scale*self.cart_scaling*cmd_twist.vel
-                rospy.logwarn(cmd_twist)
+                #rospy.logwarn(cmd_twist)
                 self.cmd_frame = kdl.addDelta(self.tip_origin, cmd_twist)
 
         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex: