Ascent

launch/sim/orocos_component_params.launch

@@ -39,10 +39,10 @@
       static_deadband: [0.001, 0.001, 0.001, 0.01, 0.01, 0.01, 0.01]
       static_effort: [4.0, 1.0, 2.0, 1.0, 0.2, 0.2, 0.05]
       static_eps: 0.020
-      #i_gains: [5.0, 5.0, 4.0, 3.0, 0.3, 0.2, 0.2]
-      #i_clamps: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+      i_gains: [5.0, 5.0, 5.0, 5.0, 5.0, 3.0, 3.0]
+      i_clamps: [0.5, 0.5, 0.5, 0.8, 0.8, 0.5, 0.5]
       compensate_friction: false
-      verbose: false
+      verbose: true
     </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]
@@ -57,10 +57,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, 25.0, 25.0, 2.0]
-      d_gains: [30.0, 30.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]
+      p_gains: [300.0, 250.0, 250.0, 200.0, 25.0, 25.0, 2.0]
+      d_gains: [18.0, 18.0, 15.0, 15.0, 10.5, 4.5, 3.5]
+      position_tolerance: [1.0, 1.0, 1.0, 1.0, 0.5, 0.5, 0.5]
+      velocity_tolerance: [4.0, 3.0, 4.0, 3.0, 4.0, 2.0, 2.0]
     </rosparam>
   </group>
 
@@ -93,12 +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]
-      velocity_tolerance: [3.2, 3.2, 3.2, 3.2, 3.0, 3.0, 3.0]
+      position_tolerance: [1.5, 1.5, 1.5, 1.5, 1.0, 0.5, 0.5]
+      velocity_tolerance: [5.2, 5.2, 5.2, 4.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_velocities: [2.5, 2.5, 2.5, 3.5, 3.5, 5.5, 5.5]
+      max_accelerations: [1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5]
       max_jerks: [2.5, 2.5, 2.5, 2.5, 5.5, 5.5, 5.0]
     </rosparam>
   </group>
@@ -109,15 +109,15 @@
       target_frame: "" #$(arg tf_prefix)/cmd
       singularity_avoidance_gain: 0.0
       joint_center_gain: 0.0
-      linear_p_gain: 500.0
+      linear_p_gain: 600.0
       linear_d_gain:  100.0
       angular_p_gain: 5.0
-      angular_d_gain:  1.5
+      angular_d_gain:  0.7
       #angular_p_gain: 5.0
       #angular_d_gain:  1.5
-      linear_effort_threshold: 50.0
+      linear_effort_threshold: 5000.0
       linear_position_threshold: 0.1
-      angular_effort_threshold: 5.0
+      angular_effort_threshold: 500.0
       angular_position_threshold: 1.5
       nullspace_damping: 0.0
       jointspace_damping: 0.0

launch/sim/wam7_sim.launch

@@ -10,6 +10,7 @@
    -->
 
   <arg name="log_level" default="5" doc="(int) Set the Orocos log level (0:none) (4:warn) (5:info) (6:debug)"/>
+  <arg name="server" default="true" doc="(bool) Launch gazebo server"/>
   <arg name="debug" default="false" doc="(bool) Launch in debug mode"/>
   <arg name="gui" default="true" doc="(bool) Launch the gazebo GUI"/>
   <arg name="planning" default="false" doc="(bool) Launch the motion planing pipeline"/>
@@ -33,7 +34,7 @@
   <param name="use_sim_time" value="true"/>
 
   <!-- Gazebo -->
-  <include file="$(find rtt_gazebo_examples)/test/empty_world.launch">
+  <include if="$(arg server)" file="$(find rtt_gazebo_examples)/test/empty_world.launch">
     <env name="ORO_LOGLEVEL" value="$(arg log_level)"/>
     <arg name="debug" value="$(arg debug)" />
     <arg name="gui" value="$(arg gui)" />
@@ -83,7 +84,7 @@
     command="$(find xacro)/xacro.py '$(find lcsr_barrett)/models/stage.urdf.xacro'" />
   <node if="false" name="$(anon urdf_spawner)" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
     args="-urdf -model stage -param stage_model"/>
-  <node name="stage_pose_broadcaster" pkg="tf" type="static_transform_publisher"
+  <node if="false" name="stage_pose_broadcaster" pkg="tf" type="static_transform_publisher"
     args="0 0 0 0 0 0 /world /stage_link 50"/>
 
   <!-- Create nodes in gazebo namespace -->
@@ -115,28 +116,29 @@
 
         # WAM Parameters
         wam:
-          velocity_cutoff_frequency: 300
+          velocity_cutoff_frequency: 20
 
         # Hand Parameters
         hand:
           admittance_control: false
           admittance_gain: 0.1
           trap_vel: 2.0
           trap_accel: 10.0
-          p_gain: 10.0
+          p_gain: 1.7
           i_gain: 0.0
           i_clamp: 0.0
-          d_gain: 0.02
-          max_torque: 3.0
+          d_gain: 0.0
+          max_torque: 5.0
           stop_torque: 3.0
           spread_p_gain: 5.0
           spread_d_gain: 0.0
           finger_acceleration: 1.0
+          outer_coupling_p_gain: 3.0
+          outer_coupling_d_gain: 0.0
+          # disabled essentially
           inner_breakaway_gain: 0.001
-          inner_breakaway_torque: 0.5
+          inner_breakaway_torque: 1000.0
           outer_recouple_velocity: 1.0
-          outer_coupling_p_gain: 5.0
-          outer_coupling_d_gain: 0.1
       </rosparam>
 
       <!-- If using the BHand -->

launch/sim/wam7_sim.urdf.xacro

@@ -12,10 +12,10 @@
       <luaScript><filename>$(arg lua_script)</filename></luaScript>
     </plugin>
     <plugin name="gazebo_ros_moveit_planning_scene" filename="libgazebo_ros_moveit_planning_scene.so">
-      <robotNamespace>~</robotNamespace>
+      <robotNamespace>/gazebo</robotNamespace>
       <sceneName>B08</sceneName>
       <robotName>wam</robotName>
-      <updatePeriod>0.033</updatePeriod>
+      <updatePeriod>0.33</updatePeriod>
       <topicName>raw_planning_scene</topicName>
     </plugin>
   </gazebo>
@@ -27,7 +27,7 @@
   <link name="world"/>
 
   <!-- r = R.RotZ(pi/2)*R.RotY(-3*pi/4); r.GetRPY() -->
-  <xacro:wam_7dof prefix="$(arg prefix)" parent_link="world" xyz="0.0 0.46 1.0" rpy="1.5707963267948966 6.123233995736766e-17 1.5707963267948966"/>
+  <xacro:wam_7dof prefix="$(arg prefix)" parent_link="world" xyz="0.3 0.56 1.0" rpy="1.5707963267948966 0 0.78539816339"/>
   <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 0" primitive="true"/>
   </xacro:if>

launch/wam7_planning.launch

@@ -1,4 +1,7 @@
 <launch>
+
+  <arg name="execution" default="true"/>
+
   <arg name="oro_prefix" default="" doc="This prefix is used for orocos component names"/>
   <arg name="tf_prefix" default="wam" doc="This prefix is used for TF frames"/>
   <arg name="joint_states_topic" default="joint_states"/>
@@ -11,10 +14,15 @@
   <arg name="ee_hand" default="false"/>
   <arg name="ee_ball" default="false"/>
 
+  <arg name="robot_description" default="/robot_description"/>
+
   <include file="$(find barrett_wam_moveit_config)/launch/move_group.launch">
     <arg name="debug" value="false"/>
-    <arg name="fake_execution" value="false"/>
-    <arg name="allow_trajectory_execution" value="true"/>
+    <arg if="$(arg execution)" name="fake_execution" value="false"/>
+    <arg if="$(arg execution)" name="allow_trajectory_execution" value="true"/>
+    <arg unless="$(arg execution)" name="fake_execution" value="true"/>
+    <arg unless="$(arg execution)" name="allow_trajectory_execution" value="false"/>
+
     <arg name="joint_states_topic" default="$(arg joint_states_topic)"/>
     <arg name="info" value="true"/>
     <!-- This publishes things like the robot state -->
@@ -24,7 +32,7 @@
 
   <include file="$(find barrett_wam_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="false"/>
-    <arg name="robot_description" value="/robot_description"/>
+    <arg name="robot_description" value="$(arg robot_description)"/>
     <arg name="ee_empty" value="$(arg ee_empty)"/>
     <arg name="ee_hand" value="$(arg ee_hand)"/>
     <arg name="ee_ball" value="$(arg ee_ball)"/>

lua/lcsr_barrett.lua

@@ -15,6 +15,9 @@ function lcsr_barrett(sim, prefix)
   --[[ set up TF component -]]
   depl:import("rtt_tf")
   depl:loadComponent("tf","rtt_tf::RTT_TF")
+  --depl:setActivity( "tf" 0.01,
+    --depl:getAttribute("HighestPriority"):get(),
+    --rtt.globals.ORO_SCHED_OTHER)
   tf = depl:getPeer("tf")
   tf:configure()
   tf:start()

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.material

@@ -0,0 +1,16 @@
+
+material lcsr_barrett/ManipulationPlatform
+{
+  receive_shadows on
+
+  technique
+  {
+    pass main
+    {
+      ambient 0.7 0.7 0.7 1.0
+      diffuse 0.2 0.2 0.2 1.0
+      specular 0.1 0.1 0.1 1.000000 0.00500
+    }
+  }
+}
+

models/manipulation_platform.sdf

@@ -0,0 +1,79 @@
+
+<?xml version='1.0'?>
+<sdf version="1.4">
+<model name="my_model">
+  <pose>0 0 0.5 0 0 0</pose>
+  <static>true</static>
+    <link name="link">
+      <inertial>
+        <mass>10.0</mass>
+        <inertia> <!-- interias are tricky to compute -->
+          <!-- http://answers.gazebosim.org/question/4372/the-inertia-matrix-explained/ -->
+          <ixx>1.0</ixx>       <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
+          <ixy>0.0</ixy>         <!-- for a box: ixy = 0 -->
+          <ixz>0.0</ixz>         <!-- for a box: ixz = 0 -->
+          <iyy>1.0</iyy>       <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
+          <iyz>0.0</iyz>         <!-- for a box: iyz = 0 -->
+          <izz>1.0</izz>       <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
+        </inertia>
+      </inertial>
+      <collision name="collision">
+        <geometry>
+          <box>
+            <size>1 1 1</size>
+          </box>
+        </geometry>
+      </collision>
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>1 1 1</size>
+          </box>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>
+
+
+
+<robot name="manipulation_platform"
+  xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <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>
+
+    <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/platform.stl"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0 " xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="package://lcsr_barrett/models/meshes/platform.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <gazebo reference="platform_link">
+      <kp>50000</kp>
+      <kd>8</kd>
+    </gazebo>
+
+</robot>

models/manipulation_platform.sdf.xacro

@@ -0,0 +1,61 @@
+<?xml version='1.0'?>
+<sdf version="1.4">
+<model name="manipulation_platform">
+
+  <static>true</static>
+
+  <static>true</static>
+    <link name="link">
+      <kp>50000</kp>
+      <kd>8</kd>
+
+      <inertial>
+        <mass>10.0</mass>
+        <inertia>
+          <ixx>1.0</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>1.0</iyy>
+          <iyz>0.0</iyz>
+          <izz>1.0</izz>
+        </inertia>
+      </inertial>
+
+      <collision name="collision">
+        <geometry>
+          <box><size>1 1 0.1</size></box>
+          <!--<mesh><uri>file://$(find lcsr_barrett)/models/meshes/platform.stl</uri></mesh>-->
+        </geometry>
+
+        <surface>
+          <friction>
+            <ode>
+              <mu>1.0</mu>
+            </ode>
+          </friction>
+        </surface>
+
+        <contact>
+          <ode>
+            <!--<kp>100</kp>-->
+            <!--<kd>0.5</kd>-->
+          </ode>
+        </contact>
+      </collision>
+
+      <visual name="visual5">
+        <cast_shadows>false</cast_shadows>
+        <material>
+          <script>
+            <uri>file://$(find lcsr_barrett)/models/manipulation_platform.material</uri>
+            <name>lcsr_barrett/ManipulationPlatform</name>
+          </script>
+        </material>
+        <geometry>
+          <box><size>1 1 0.1</size></box>
+          <!--<mesh><uri>file://$(find lcsr_barrett)/models/meshes/platform.stl</uri></mesh>-->
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>

models/manipulation_platform.urdf.xacro

@@ -21,13 +21,13 @@
       <visual>
         <origin rpy="0 0 0 " xyz="0 0 0"/>
         <geometry>
-          <mesh filename="package://lcsr_barrett/models/meshes/manipulation_platform.stl"/>
+          <mesh filename="package://lcsr_barrett/models/meshes/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"/>
+          <mesh filename="package://lcsr_barrett/models/meshes/platform.stl"/>
         </geometry>
       </collision>
     </link>