Merge pull request #5 from code-iai/new-model-w-cablemount

New model w cablemount

README.md

@@ -1,2 +1,14 @@
 # iai_pr2
+
 Installation, configuration, and launch files specific to IAI's PR2 robot.
+
+## Explanation of different model versions
+
+* `pr2_calibrated_with_ft2.xml` is the current model of the PR2 with the force torque sensor. The model is similar to `/etc/ros/indigo/urdf/robot.xml` on *pr2a* except for additional mimic joints.
+* `pr2_with_ft2_cableguide.xacro` is the current model of the PR2 with the force torque sensor and the cable guide made of item profiles. This model is derived from `pr2_calibrated_with_ft2.xml`.
+
+The simulation launch files with `_cableguide` in the name also use the new model with the cable guide and are based on the similar named launch files with or without odom joints.
+
+The real robot uses its own model and launch file stored at `/etc/ros/indigo/` on *pr2a*. The launch file `iai_pr2_bringup/launch/robot.launch` is derived from the launch file `/etc/ros/indigo/robot.launch` on *pr2a*. Both include the force torque sensor but not the cable guide.
+
+The launch `iai_pr2_bringup/launch/robot_with_cableguide.launch` is derived from `iai_pr2_bringup/launch/robot.launch` and uses the new model with cable guide.

iai_pr2_bringup/launch/robot.launch

@@ -1,6 +1,7 @@
 <launch>
 
-    <param name="robot_description" command="$(find xacro)/xacro.py '$(find iai_pr2_description)/robots/pr2_with_odom_joints_calibrated.xacro'" />
+    <!-- Derived from PR2 launch file in /etc/ros/indigo/robot.launch. Uses newer model. -->
+    <param name="robot_description" command="$(find xacro)/xacro '$(find iai_pr2_description)/robots/pr2_with_odom_joints_calibrated.xacro'" />
 
     <!-- Robot Analyzer --> <rosparam command="load" file="$(find pr2_bringup)/config/pr2_analyzers.yaml" ns="diag_agg" />
 

iai_pr2_bringup/launch/robot_with_cableguide.launch

@@ -0,0 +1,19 @@
+<launch>
+
+    <!-- Derived from PR2 launch file in /etc/ros/indigo/robot.launch. Uses new model. -->
+    <param name="robot_description" command="$(find xacro)/xacro '$(find iai_pr2_description)/robots/pr2_with_ft2_cableguide.xacro'" />
+
+    <!-- Robot Analyzer --> <rosparam command="load" file="$(find pr2_bringup)/config/pr2_analyzers.yaml" ns="diag_agg" />
+
+    <arg name="c2" default="true"/>
+
+    <!-- Robot bringup -->
+    <include file="$(find pr2_bringup)/pr2.launch">
+        <arg name="c2" value="$(arg c2)"/>
+    </include>
+
+    <include file="/etc/ros/indigo/ft.launch" />
+
+    <rosparam file="/etc/ros/robot.yaml"/>
+
+</launch>

iai_pr2_description/launch/upload_pr2_with_odom_joints_calibrated.launch → iai_pr2_description/launch/upload_pr2_cableguide.launch

@@ -1,3 +1,3 @@
 <launch>
-  <param name="robot_description" command="$(find xacro)/xacro '$(find iai_pr2_description)/robots/pr2_with_odom_joints_calibrated.xacro'" />
+  <param name="robot_description" command="$(find xacro)/xacro '$(find iai_pr2_description)/robots/pr2_with_ft2_cableguide.xacro'"/>
 </launch>

iai_pr2_description/robots/pr2_calibrated_with_ft2.xml

@@ -1809,9 +1809,9 @@
       </material>
     </visual>
     <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <origin rpy="0 0 0" xyz="-0.041 0 0.0776"/>
       <geometry>
-        <sphere radius="0.0005"/>
+        <box size="0.106 0.2957 0.1552"/>
       </geometry>
     </collision>
   </link>

iai_pr2_description/robots/pr2_with_ft2_cableguide.xacro

@@ -0,0 +1,139 @@
+<?xml version="1.0" ?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="pr2">
+  <!-- based on pr2_calibrated_with_ft2.xml -->
+
+  <xacro:include filename="$(find iai_pr2_description)/robots/pr2_calibrated_with_ft2.xml"/>
+
+  <joint name="head_mount_kinect2_joint" type="fixed">
+    <parent link="head_plate_frame"/>
+    <child link="head_mount_kinect2_link"/>
+    <origin rpy="0 0 -1.5708" xyz="-0.024 0 0.091"/>
+  </joint>
+  <link name="head_mount_kinect2_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iai_pr2_description/meshes/sensors/microsoft_kinect2.dae" scale="1 1 1"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.5 0.5 0.5 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 -0.0147805 0.02515"/>
+      <geometry>
+        <box size="0.250000 0.117597 0.050301"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_torso_lift_side_item_profile_joint" type="fixed">
+    <parent link="l_torso_lift_side_plate_link"/>
+    <child link="l_torso_lift_side_item_profile_link"/>
+    <origin rpy="0 0 1.570796" xyz="-0.205 0.02 0.05"/>
+  </joint>
+  <link name="l_torso_lift_side_item_profile_link">
+    <inertial>
+      <mass value="0.7134"/>
+      <origin xyz="0.0 0.0 0.0"/>
+      <!-- Calculated using MeshLabInertiaToURDF -->
+      <inertia ixx="0.01008718" ixy="0.0" ixz="0.0" iyy="0.00018726" iyz="0.0" izz="0.01008718"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iai_pr2_description/meshes/cableguide/item_Profil_8_40x40_leicht_L410.dae"/>
+      </geometry>
+      <material name="Grey2"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.04 0.41 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_torso_lift_side_item_profile_joint" type="fixed">
+    <parent link="r_torso_lift_side_plate_link"/>
+    <child link="r_torso_lift_side_item_profile_link"/>
+    <origin rpy="0 0 1.570796" xyz="-0.205 -0.02 0.05"/>
+  </joint>
+  <link name="r_torso_lift_side_item_profile_link">
+    <inertial>
+      <mass value="0.7134"/>
+      <origin xyz="0.0 0.0 0.0"/>
+      <!-- Calculated using MeshLabInertiaToURDF -->
+      <inertia ixx="0.01008718" ixy="0.0" ixz="0.0" iyy="0.00018726" iyz="0.0" izz="0.01008718"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iai_pr2_description/meshes/cableguide/item_Profil_8_40x40_leicht_L410.dae"/>
+      </geometry>
+      <material name="Grey2"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.04 0.41 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="m_torso_lift_side_item_profile_joint" type="fixed">
+    <parent link="r_torso_lift_side_item_profile_link"/>
+    <child link="m_torso_lift_side_item_profile_link"/>
+    <origin rpy="0 0 1.570796" xyz="0.23 0.185 0"/>
+  </joint>
+  <link name="m_torso_lift_side_item_profile_link">
+    <inertial>
+      <mass value="0.7308"/>
+      <origin xyz="0.0 0.0 0.0"/>
+      <!-- Calculated using MeshLabInertiaToURDF -->
+      <inertia ixx="0.01083867" ixy="0.0" ixz="-0.0" iyy="0.00019183" iyz="0.0" izz="0.01083867"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iai_pr2_description/meshes/cableguide/item_Profil_8_40x40_leicht_L420.dae"/>
+      </geometry>
+      <material name="Grey2"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.04 0.42 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="t_torso_lift_side_item_profile_joint" type="fixed">
+    <parent link="m_torso_lift_side_item_profile_link"/>
+    <child link="t_torso_lift_side_item_profile_link"/>
+    <origin rpy="1.570796 0 0" xyz="0 0 0.52"/>
+  </joint>
+  <link name="t_torso_lift_side_item_profile_link">
+    <inertial>
+      <origin xyz="0.0 0.0 0.0"/>
+      <mass value="1.74"/>
+      <!-- Calculated using MeshLabInertiaToURDF -->
+      <inertia ixx="0.14522837" ixy="0.0" ixz="0.0" iyy="0.00045673" iyz="0.0" izz="0.14522837"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://iai_pr2_description/meshes/cableguide/item_Profil_8_40x40_leicht_L1000.dae"/>
+      </geometry>
+      <material name="Grey2"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.04 1.0 0.04"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>

iai_pr2_sim/launch/ros_control_sim_with_base_cableguide.launch

@@ -0,0 +1,76 @@
+<launch>
+
+  <arg name="tf_prefix" default=""/>
+
+  <include file="$(find iai_pr2_description)/launch/upload_pr2_cableguide.launch" />
+
+  <node pkg="iai_naive_kinematics_sim" type="map_odom_transform_publisher.py" name="map_odom_transform_publisher" output="screen">
+    <param name="parent_frame" value="map"/>
+    <param name="child_frame" value="$(arg tf_prefix)odom_combined"/>
+  </node>
+
+  <rosparam command="load" file="$(find iai_pr2_sim)/config/ros_control_sim.yaml" />
+
+  <node pkg="ros_control_boilerplate" type="sim_hw_main" name="simulator" output="screen">
+    <remap from="joint_states" to="body/joint_states" />
+  </node>
+
+  <node pkg="controller_manager" type="spawner" name="spawner" output="screen" args="joint_state_controller whole_body_controller/body --shutdown-timeout 0.5" />
+
+  <node pkg="iai_naive_kinematics_sim" type="simulator" name="base_simulator" output="screen">
+    <rosparam command="load" file="$(find iai_pr2_sim)/config/naive_kinematics_sim_base.yaml" />
+    <remap from="~joint_states" to="base/joint_states" />
+    <!-- Remapping to global topics will break ros' group namespacing -->
+    <remap from="~commands" to="commands" />
+  </node>
+
+  <node pkg="iai_naive_kinematics_sim" type="simulator" name="r_gripper_simulator" output="screen">
+    <rosparam command="load" file="$(find iai_pr2_sim)/config/naive_kinematics_sim_r_gripper.yaml" />
+    <remap from="~joint_states" to="r_gripper/joint_states" />
+    <remap from="~commands" to="r_gripper/commands" />
+  </node>
+  <node pkg="iai_naive_kinematics_sim" type="simulator" name="l_gripper_simulator" output="screen">
+    <rosparam command="load" file="$(find iai_pr2_sim)/config/naive_kinematics_sim_l_gripper.yaml" />
+    <remap from="~joint_states" to="l_gripper/joint_states" />
+    <!-- Remapping to global topics will break ros' group namespacing -->
+    <remap from="~commands" to="l_gripper/commands" />
+  </node>
+
+  <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher" output="screen">
+    <rosparam param="source_list">
+    - base/joint_states
+    - body/joint_states
+    - r_gripper/joint_states
+    - l_gripper/joint_states
+    </rosparam>
+    <rosparam param="zeros">
+    'odom_x_joint': 0.0
+    'odom_y_joint': 0.0
+    'odom_z_joint': 0.0
+    </rosparam>
+    <param name="rate" value="120"/>
+    <param name="use_gui" value="False"/>
+  </node>
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+    <param name="prefix_tf_with" value="$(arg tf_prefix)"/>
+  </node>
+
+  <node pkg="tf2_ros" type="buffer_server" name="tf2_buffer_server" />
+
+  <node pkg="iai_naive_kinematics_sim" type="fake_base_controller.py" name="base_controller" output="screen">
+    <param name="odom_x_joint" value="odom_x_joint"/>
+    <param name="odom_y_joint" value="odom_y_joint"/>
+    <param name="odom_z_joint" value="odom_z_joint"/>
+    <param name="odom" value="$(arg tf_prefix)odom_combined"/>
+    <param name="base_footprint" value="$(arg tf_prefix)base_footprint" />
+    <!-- Remapping to global topics will break ros' group namespacing -->
+    <remap from="~cmd_vel" to="cmd_vel" />
+    <remap from="~commands" to="commands" />
+  </node>
+
+  <include file="$(find omni_pose_follower)/launch/omni_pose_follower.launch"/>
+
+  <node pkg="iai_pr2_sim" type="set_initial_joint_state.py" name="set_initial_joint_state" />
+
+</launch>