Urdf update

README.md

@@ -84,6 +84,9 @@ Other dependencies needed (in a convenient one-liner)
     version=kinetic &&
     sudo apt install ros-$version-gazebo-* &&
     sudo apt install ros-$version-moveit-* &&
+    sudo apt install ros-$version-opencv* &&
+    sudo apt install ros-$version-image-proc && 
+    sudo apt install ros-$version-gazebo-ros-control &&
     sudo apt install ros-$version-move-base &&
     sudo apt install ros-$version-amcl &&
     sudo apt install ros-$version-joy &&
@@ -93,8 +96,6 @@ Other dependencies needed (in a convenient one-liner)
     sudo apt install ros-$version-position-controllers &&
     sudo apt install ros-$version-joint-trajectory-controller &&
     sudo apt install ros-$version-eband-local-planner &&
-    sudo apt install ros-$version-opencv* &&
-    sudo apt install ros-$version-gazebo-ros-control &&
     sudo apt install ros-$version-laser-filters &&
     sudo apt install libsuitesparse-dev &&
     sudo apt install ros-$version-fake-localization &&

vizzy_control/config/vizzy_control.yaml

@@ -14,6 +14,35 @@ vizzy:
     joint: waist_joint
     pid: {p: 100.0, i: 0.0, d: 0.0}
 
+  # Right Hand Position Controllers
+  #
+  # TODO: Implement controller after hand update
+  #
+  # r_min_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_min_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_med_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_med_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_ind_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_ind_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_thumb_phal_1_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_thumb_phal_1_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
+  # r_thumb_phal_2_position_controller:
+  #   type: velocity_controllers/JointPositionController
+  #   joint: r_thumb_phal_2_joint
+  #   pid: {p: 100.0, i: 0.0, d: 0.0, i_clamp: 0.0}
+
   # Head Position Controllers
   neck_pan_position_controller:
     type: velocity_controllers/JointPositionController

vizzy_control/launch/vizzy_control.launch

@@ -1,69 +1,76 @@
 <?xml version="1.0"?>
 <launch>
-	<arg name="use_yarp" default="false"/>
+    <arg name="use_yarp" default="false"/>
+
+    <group if="$(arg use_yarp)">
+        <!-- launch the joint state publisher to default values -->
+        <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
+            <rosparam param="source_list">["/vizzy_upper_body/joint_states","joint_states"]</rosparam>
+        <remap from="/joint_states" to="/vizzy/joint_states" />
+        </node>
+    </group>
 
-	<group if="$(arg use_yarp)">
- 		<!-- launch the joint state publisher to default values -->
-		<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
-			<rosparam param="source_list">["/vizzy_upper_body/joint_states","joint_states"]</rosparam>
-		<remap from="/joint_states" to="/vizzy/joint_states" />
-		</node>
-	</group>
     <!-- Load joint controller configurations from YAML file to parameter server -->
     <rosparam file="$(find vizzy_control)/config/vizzy_control.yaml" command="load"/>
-	<!-- bringup ROS controllers -->
- 	<group unless="$(arg use_yarp)">
+    <!-- bringup ROS controllers -->
+    <group unless="$(arg use_yarp)">
+
+      <!-- Load the controllers -->
+      <!--
+      <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/vizzy" args="
+        waist_position_controller 
+        neck_tilt_position_controller 
+        neck_pan_position_controller 
+        eyes_tilt_position_controller 
+        l_eye_position_controller
+        r_eye_position_controller
 
+        l_shoulder_scapula_position_controller
+        l_shoulder_flection_position_controller
+        l_shoulder_rotation_position_controller
+        l_elbow_flection_position_controller
+        l_forearm_pronation_position_controller
+        l_wrist_abduction_position_controller
+        l_wrist_flection_position_controller
 
-	  <!-- Load the controllers -->
-	  <!--<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-	    output="screen" ns="/vizzy" args="
-		waist_position_controller 
-		neck_tilt_position_controller 
-		neck_pan_position_controller 
-		eyes_tilt_position_controller 
-		l_eye_position_controller
-	        r_eye_position_controller
+        r_shoulder_scapula_position_controller
+        r_shoulder_flection_position_controller
+        r_shoulder_rotation_position_controller
+        r_elbow_flection_position_controller
+        r_forearm_pronation_position_controller
+        r_wrist_abduction_position_controller
+        r_wrist_flection_position_controller
+
+        joint_state_controller" />
+      -->
 
-		l_shoulder_scapula_position_controller
-	        l_shoulder_flection_position_controller
-	        l_shoulder_rotation_position_controller
-	        l_elbow_flection_position_controller
-	        l_forearm_pronation_position_controller
-	        l_wrist_abduction_position_controller
-	        l_wrist_flection_position_controller
+      <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/vizzy" args="
+        joint_state_controller
+        waist_position_controller
+        neck_pan_position_controller
+        neck_tilt_position_controller
+        eyes_tilt_position_controller
+        l_eye_position_controller
+        r_eye_position_controller
+        left_arm_trajectory_controller
+        right_arm_trajectory_controller"/>
 
-		r_shoulder_scapula_position_controller
-	        r_shoulder_flection_position_controller
-	        r_shoulder_rotation_position_controller
-	        r_elbow_flection_position_controller
-	        r_forearm_pronation_position_controller
-	        r_wrist_abduction_position_controller
-	        r_wrist_flection_position_controller
-                joint_state_controller" />-->
+    <!--
+        TODO: Implement controller after hand update
+    -->
+    <!--    
+        r_min_phal_1_position_controller
+        r_med_phal_1_position_controller
+        r_ind_phal_1_position_controller
+        r_thumb_phal_1_position_controller
+        r_thumb_phal_2_position_controller"/>
+    -->
 
-	  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-	    output="screen" ns="/vizzy" args="
-		joint_state_controller
-		waist_position_controller
-		neck_pan_position_controller
-        	neck_tilt_position_controller
-        	eyes_tilt_position_controller
-        	l_eye_position_controller
-        	r_eye_position_controller
-		left_arm_trajectory_controller
-		right_arm_trajectory_controller"/>
-		<!--
-		r_min_phal_1_position_controller
-		r_med_phal_1_position_controller
-		r_ind_phal_1_position_controller
-		r_thumb_phal_1_position_controller
-		r_thumb_phal_2_position_controller"/>
-		-->
   </group>
 
-	<!-- convert joint states to TF transforms for rviz, etc -->
+    <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
-		<remap from="/joint_states" to="/vizzy/joint_states" />
+        <remap from="/joint_states" to="/vizzy/joint_states" />
   </node>
+
 </launch>

vizzy_description/launch/display.launch

@@ -8,8 +8,8 @@
 
   <!-- Load the URDF into the ROS Parameter Server -->
   <param name="robot_description"
-		command="$(find xacro)/xacro --inorder $(find vizzy_description)/robots/vizzy.urdf.xacro use_yarp:=true"/>
-	 
+    command="$(find xacro)/xacro --inorder $(find vizzy_description)/robots/vizzy.urdf.xacro use_yarp:=true"/>
+   
   <!-- Use graphical interface to set the joints positions --> 
   <param
     name="use_gui"
@@ -31,4 +31,5 @@
     pkg="rviz"
     type="rviz"
     args="-d $(arg config_file)"/>
+
 </launch>

vizzy_description/launch/display_right_hand.launch

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Use graphical interface to set the joints positions --> 
+  <arg name="gui" default="True" />
+  <!-- Rviz configuration file -->
+  <arg name="config_file" default="$(find vizzy_description)/rviz/right_hand_urdf.rviz"/>
+
+  <!-- Load the URDF into the ROS Parameter Server -->
+  <param
+    name="robot_description"
+    command="$(find xacro)/xacro --inorder $(find vizzy_description)/robots/vizzy_hand_only.urdf.xacro use_full_gazebo_model:=false" />
+
+  <param
+    name="use_gui"
+    value="$(arg gui)" />
+
+  <node
+    name="joint_state_publisher"
+    pkg="joint_state_publisher"
+    type="joint_state_publisher" />
+
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="state_publisher" />
+
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(arg config_file)"/>
+
+</launch>

vizzy_description/robots/vizzy.urdf.xacro

@@ -4,12 +4,14 @@
   <!-- Load Yarp plugins for real robot execution -->
   <xacro:arg name="use_yarp" default="false"/>
 
+  <!-- Load full inertial and collision model or a simplified model -->
+  <xacro:arg name="use_full_gazebo_model" default="false"/>
   <!-- Load a fully detailed hand model with properly modeled links and joints -->
   <xacro:arg name="use_full_hand_model" default="false"/>
   <!-- Disable laser sensors -->
-  <xacro:arg name="disable_laser" default="false"/>
+  <xacro:arg name="disable_laser" default="true"/>
   <!-- Disable depth camera -->
-  <xacro:arg name="disable_3d_sensor" default="false"/>
+  <xacro:arg name="disable_3d_sensor" default="true"/>
 
   <!-- Includes -->
 
@@ -21,33 +23,41 @@
   <!-- Hokuyo URG-04LX laser scanner -->
   <xacro:include filename="$(find vizzy_description)/urdf/sensors/hokuyo_04lx_laser.urdf.xacro"/>
   <!-- TODO: Update Asus Xiton Live Pro -->
-  <xacro:include filename="$(find vizzy_description)/urdf/sensors/asus_camera.urdf.xacro" />
+  <xacro:include filename="$(find vizzy_description)/urdf/sensors/asus_camera.urdf.xacro"/>
 
   <!-- Robot body -->
 
   <!-- Base -->
-  <xacro:include filename="$(find vizzy_description)/urdf/base/base.urdf.xacro" />
+  <xacro:include filename="$(find vizzy_description)/urdf/base/base.urdf.xacro">
+    <xacro:arg name="use_full_gazebo_model" value="$(arg use_full_gazebo_model)"/>
+  </xacro:include>
   <!-- Torso -->
-  <xacro:include filename="$(find vizzy_description)/urdf/torso/torso.urdf.xacro" />
+  <xacro:include filename="$(find vizzy_description)/urdf/torso/torso.urdf.xacro">
+    <xacro:arg name="use_full_gazebo_model" value="$(arg use_full_gazebo_model)"/>
+  </xacro:include>
   <!-- Head -->
- <xacro:include filename="$(find vizzy_description)/urdf/head/head.urdf.xacro"/>
+ <xacro:include filename="$(find vizzy_description)/urdf/head/head.urdf.xacro" >
+    <xacro:arg name="use_full_gazebo_model" value="$(arg use_full_gazebo_model)"/>
+  </xacro:include>
   <!-- Arms -->
-  <xacro:include filename="$(find vizzy_description)/urdf/arm/arm.urdf.xacro" />
+  <xacro:include filename="$(find vizzy_description)/urdf/arm/arm.urdf.xacro" >
+    <xacro:arg name="use_full_gazebo_model" value="$(arg use_full_gazebo_model)"/>
+    <xacro:arg name="use_full_hand_model" value="$(arg use_full_hand_model)"/>
+  </xacro:include>
 
   <!-- Gazebo plugins -->
 
   <!-- ROS -->
-  <xacro:include filename="$(find vizzy_description)/plugins/ros.gazebo.xacro" />
+  <xacro:include filename="$(find vizzy_description)/plugins/ros.gazebo.xacro"/>
   <!-- Yarp -->
-  <xacro:include filename="$(find vizzy_description)/plugins/yarp.gazebo.xacro" />
+  <xacro:include filename="$(find vizzy_description)/plugins/yarp.gazebo.xacro"/>
 
   <!-- Instantiate base -->
   <xacro:base name="base"/>
   <!-- Instantiate the torso -->
   <xacro:torso name="torso" parent="base_link"/>
   <!-- Instantiate head -->
   <xacro:head name="head" parent="waist_link"/>
-
   <!-- Instantiate arms -->
   <xacro:arm name="left_arm" parent="waist_link"/>
   <xacro:arm name="right_arm" parent="waist_link"/>
@@ -108,4 +118,5 @@
   <xacro:unless value="$(arg use_yarp)">
     <xacro:ros_control_gazebo/>
   </xacro:unless>
+
 </robot>

vizzy_description/robots/vizzy_hand_only.urdf.xacro

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<robot  name="vizzy_hand" xmlns:xacro="http://ros.org/wiki/xacro" >
+
+  <!-- Includes -->
+
+  <!-- Misc common stuff such as constant definitions -->
+  <xacro:include filename="$(find vizzy_description)/urdf/common.urdf.xacro"/>
+  <!-- Hand -->
+  <xacro:include filename="$(find vizzy_description)/urdf/hand/right_hand.urdf.xacro">
+    <xacro:arg name="use_full_gazebo_model" value="$(arg use_full_gazebo_model)"/>
+  </xacro:include>
+
+  <!-- Root link, on the ground just below the robot origin -->
+  <link name="base_footprint">
+  </link>
+
+  <xacro:right_hand parent="base_footprint"/>
+
+</robot>