Tutorial fixes and additions: build deps, path optimization, Gazebo execution

CMakeLists.txt

@@ -72,6 +72,17 @@ set(CATKIN_PACKAGE_SHARE_DESTINATION
 install(DIRECTORY urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY srdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(
+    FILES tutorial_6/controllers.yaml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/tutorial_6
+)
+
+install(
+    FILES tutorial_7/controllers.yaml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/tutorial_7
+)
 
 install(
     FILES Media/models/meshes/glasses/__Color_A05_4.png

README.md

@@ -8,7 +8,8 @@ The various tutorials are:
 3. [tutorial 3](./tutorial_3/README.md) How to use HPP in manufacturing.
 4. [tutorial 4](./tutorial_4/README.md) How to control the trajectory of a tool.
 5. [tutorial 5](./tutorial_5/README.md) How to optimize and time-parameterize paths.
-6. [tutorial 6](./tutorial_6/README.md) How to execute motions on a real robot.
+6. [tutorial 6](./tutorial_6/README.md) How to execute motions on a simulated robot.
+7. [tutorial 7](./tutorial_7/README.md) Pick and place with gripper.
 
 [![Pipeline status](https://gitlab.laas.fr/humanoid-path-planner/hpp_tutorial/badges/master/pipeline.svg)](https://gitlab.laas.fr/humanoid-path-planner/hpp_tutorial/commits/master)
 [![Coverage report](https://gitlab.laas.fr/humanoid-path-planner/hpp_tutorial/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/humanoid-path-planner/hpp_tutorial/master/coverage/)

launch/tutorial_6_launch.py

@@ -0,0 +1,124 @@
+"""Launch Franka fr3 in Gazebo with a joint_trajectory_controller for HPP tutorial 6.
+
+Usage (inside the docker container):
+    ros2 launch hpp_tutorial tutorial_6/launch_sim.py
+"""
+
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchContext, LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def get_robot_description_and_controllers(context: LaunchContext, robot_type):
+    robot_type_str = context.perform_substitution(robot_type)
+
+    tutorial_dir = os.path.join(
+        get_package_share_directory("hpp_tutorial"), "tutorial_6"
+    )
+    controllers_yaml = os.path.join(tutorial_dir, "controllers.yaml")
+
+    franka_xacro = os.path.join(
+        get_package_share_directory("franka_gazebo_bringup"),
+        "urdf",
+        "franka_arm.gazebo.xacro",
+    )
+
+    robot_description_xml = xacro.process_file(
+        franka_xacro,
+        mappings={
+            "robot_type": robot_type_str,
+            "hand": "false",
+            "ros2_control": "true",
+            "gazebo": "true",
+            "ee_id": "franka_hand",
+        },
+    ).toxml()
+
+    # Replace the Franka default controllers YAML with our tutorial controllers
+    franka_controllers = os.path.join(
+        get_package_share_directory("franka_gazebo_bringup"),
+        "config",
+        "franka_gazebo_controllers.yaml",
+    )
+    robot_description_xml = robot_description_xml.replace(
+        franka_controllers, controllers_yaml
+    )
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[{"robot_description": robot_description_xml}],
+    )
+
+    # Spawn controllers using ros2_control spawner
+    spawn_jsb = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster"],
+        output="screen",
+    )
+
+    spawn_jtc = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_trajectory_controller"],
+        output="screen",
+    )
+
+    return [robot_state_publisher, spawn_jsb, spawn_jtc]
+
+
+def generate_launch_description():
+    robot_type = LaunchConfiguration("robot_type")
+
+    # Gazebo Sim
+    os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.dirname(
+        get_package_share_directory("franka_description")
+    )
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("ros_gz_sim"),
+                "launch",
+                "gz_sim.launch.py",
+            )
+        ),
+        launch_arguments={"gz_args": "empty.sdf -r"}.items(),
+    )
+
+    spawn = Node(
+        package="ros_gz_sim",
+        executable="create",
+        arguments=["-topic", "/robot_description"],
+        output="screen",
+    )
+
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
+        output="screen",
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("robot_type", default_value="fr3"),
+            gazebo,
+            OpaqueFunction(
+                function=get_robot_description_and_controllers, args=[robot_type]
+            ),
+            spawn,
+            bridge,
+        ]
+    )

launch/tutorial_7_launch.py

@@ -0,0 +1,130 @@
+"""Launch Franka FR3 with gripper in Gazebo for HPP tutorial 7 (pick and place).
+
+Usage (inside the docker container):
+    ros2 launch /home/user/devel/install/share/hpp_tutorial/tutorial_7/launch_sim.py
+"""
+
+import os
+
+import xacro
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchContext, LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def get_robot_description_and_controllers(context: LaunchContext, robot_type):
+    robot_type_str = context.perform_substitution(robot_type)
+
+    tutorial_dir = os.path.join(
+        get_package_share_directory("hpp_tutorial"), "tutorial_7"
+    )
+    controllers_yaml = os.path.join(tutorial_dir, "controllers.yaml")
+
+    franka_xacro = os.path.join(
+        get_package_share_directory("franka_gazebo_bringup"),
+        "urdf",
+        "franka_arm.gazebo.xacro",
+    )
+
+    robot_description_xml = xacro.process_file(
+        franka_xacro,
+        mappings={
+            "robot_type": robot_type_str,
+            "hand": "true",
+            "ros2_control": "true",
+            "gazebo": "true",
+            "ee_id": "franka_hand",
+        },
+    ).toxml()
+
+    # Replace the Franka default controllers YAML with our tutorial controllers
+    franka_controllers = os.path.join(
+        get_package_share_directory("franka_gazebo_bringup"),
+        "config",
+        "franka_gazebo_controllers.yaml",
+    )
+    robot_description_xml = robot_description_xml.replace(
+        franka_controllers, controllers_yaml
+    )
+
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[{"robot_description": robot_description_xml}],
+    )
+
+    spawn_jsb = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster"],
+        output="screen",
+    )
+
+    spawn_jtc = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_trajectory_controller"],
+        output="screen",
+    )
+
+    spawn_gripper = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gripper_controller"],
+        output="screen",
+    )
+
+    return [robot_state_publisher, spawn_jsb, spawn_jtc, spawn_gripper]
+
+
+def generate_launch_description():
+    robot_type = LaunchConfiguration("robot_type")
+
+    # Gazebo Sim
+    os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.dirname(
+        get_package_share_directory("franka_description")
+    )
+    gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                get_package_share_directory("ros_gz_sim"),
+                "launch",
+                "gz_sim.launch.py",
+            )
+        ),
+        launch_arguments={"gz_args": "empty.sdf -r"}.items(),
+    )
+
+    spawn = Node(
+        package="ros_gz_sim",
+        executable="create",
+        arguments=["-topic", "/robot_description"],
+        output="screen",
+    )
+
+    bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"],
+        output="screen",
+    )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("robot_type", default_value="fr3"),
+            gazebo,
+            OpaqueFunction(
+                function=get_robot_description_and_controllers, args=[robot_type]
+            ),
+            spawn,
+            bridge,
+        ]
+    )

package.xml

@@ -7,7 +7,7 @@
   </description>
   <author>Florent Lamiraux</author>
   <maintainer email="[email protected]">Florent Lamiraux</maintainer>
-  <license>LGPL v2</license>
+  <license>BSD 2-Clause</license>
   <build_depend>hpp-corbaserver</build_depend>
   <build_depend>hpp-manipulation-corba</build_depend>
 </package>

srdf/fr3.srdf

@@ -0,0 +1,92 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /home/psardin/devel/franka_ros2_ws/src/franka_description/robots/fr3/fr3.srdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="fr3">
+  <group name="fr3_arm">
+    <chain base_link="fr3_link0" tip_link="fr3_hand_tcp"/>
+  </group>
+  <group_state group="fr3_arm" name="ready">
+    <joint name="fr3_joint1" value="0"/>
+    <joint name="fr3_joint2" value="-0.7853981633974483"/>
+    <joint name="fr3_joint3" value="0"/>
+    <joint name="fr3_joint4" value="-2.356194490192345"/>
+    <joint name="fr3_joint5" value="0"/>
+    <joint name="fr3_joint6" value="1.5707963267948966"/>
+    <joint name="fr3_joint7" value="0.7853981633974483"/>
+  </group_state>
+  <group_state group="fr3_arm" name="extended">
+    <joint name="fr3_joint1" value="0"/>
+    <joint name="fr3_joint2" value="0"/>
+    <joint name="fr3_joint3" value="0"/>
+    <joint name="fr3_joint4" value="-0.1"/>
+    <joint name="fr3_joint5" value="0"/>
+    <joint name="fr3_joint6" value="1.5707963267948966"/>
+    <joint name="fr3_joint7" value="0.7853981633974483"/>
+  </group_state>
+  <virtual_joint child_link="base" name="virtual_joint" parent_frame="world" type="fixed"/>
+  <disable_collisions link1="fr3_link0" link2="fr3_link1" reason="Adjacent"/>
+  <disable_collisions link1="fr3_link0" link2="fr3_link2" reason="Never"/>
+  <disable_collisions link1="fr3_link0" link2="fr3_link3" reason="Never"/>
+  <disable_collisions link1="fr3_link0" link2="fr3_link4" reason="Never"/>
+  <disable_collisions link1="fr3_link1" link2="fr3_link2" reason="Adjacent"/>
+  <disable_collisions link1="fr3_link1" link2="fr3_link3" reason="Never"/>
+  <disable_collisions link1="fr3_link1" link2="fr3_link4" reason="Never"/>
+  <disable_collisions link1="fr3_link2" link2="fr3_link3" reason="Adjacent"/>
+  <disable_collisions link1="fr3_link2" link2="fr3_link4" reason="Never"/>
+  <disable_collisions link1="fr3_link2" link2="fr3_link6" reason="Never"/>
+  <disable_collisions link1="fr3_link3" link2="fr3_link4" reason="Adjacent"/>
+  <disable_collisions link1="fr3_link3" link2="fr3_link5" reason="Never"/>
+  <disable_collisions link1="fr3_link3" link2="fr3_link6" reason="Never"/>
+  <disable_collisions link1="fr3_link3" link2="fr3_link7" reason="Never"/>
+  <disable_collisions link1="fr3_link4" link2="fr3_link5" reason="Adjacent"/>
+  <disable_collisions link1="fr3_link4" link2="fr3_link6" reason="Never"/>
+  <disable_collisions link1="fr3_link4" link2="fr3_link7" reason="Never"/>
+  <disable_collisions link1="fr3_link4" link2="fr3_link8" reason="Never"/>
+  <disable_collisions link1="fr3_link5" link2="fr3_link6" reason="Adjacent"/>
+  <disable_collisions link1="fr3_link5" link2="fr3_link7" reason="Never"/>
+  <disable_collisions link1="fr3_link6" link2="fr3_link7" reason="Adjacent"/>
+  <disable_collisions link1="fr3_link6" link2="fr3_link8" reason="Default"/>
+  <disable_collisions link1="fr3_link7" link2="fr3_link8" reason="Adjacent"/>
+  <group name="fr3_hand">
+    <link name="fr3_hand"/>
+    <link name="fr3_leftfinger"/>
+    <link name="fr3_rightfinger"/>
+    <joint name="fr3_finger_joint1"/>
+    <passive_joint name="fr3_finger_joint2"/>
+  </group>
+  <group_state group="fr3_hand" name="open">
+    <joint name="fr3_finger_joint1" value="0.035"/>
+    <joint name="fr3_finger_joint2" value="0.035"/>
+  </group_state>
+  <group_state group="fr3_hand" name="close">
+    <joint name="fr3_finger_joint1" value="0"/>
+    <joint name="fr3_finger_joint2" value="0"/>
+  </group_state>
+  <end_effector group="fr3_hand" name="fr3_hand" parent_group="fr3_arm" parent_link="fr3_hand_tcp"/>
+  <disable_collisions link1="fr3_hand" link2="fr3_leftfinger" reason="Adjacent"/>
+  <disable_collisions link1="fr3_hand" link2="fr3_rightfinger" reason="Adjacent"/>
+  <disable_collisions link1="fr3_leftfinger" link2="fr3_rightfinger" reason="Default"/>
+  <disable_collisions link1="fr3_hand" link2="fr3_link3" reason="Never"/>
+  <disable_collisions link1="fr3_hand" link2="fr3_link4" reason="Never"/>
+  <disable_collisions link1="fr3_hand" link2="fr3_link6" reason="Never"/>
+  <disable_collisions link1="fr3_hand" link2="fr3_link7" reason="Adjacent"/>
+  <disable_collisions link1="fr3_hand" link2="fr3_link8" reason="Adjacent"/>
+  <disable_collisions link1="fr3_leftfinger" link2="fr3_link3" reason="Never"/>
+  <disable_collisions link1="fr3_leftfinger" link2="fr3_link4" reason="Never"/>
+  <disable_collisions link1="fr3_leftfinger" link2="fr3_link6" reason="Never"/>
+  <disable_collisions link1="fr3_leftfinger" link2="fr3_link7" reason="Never"/>
+  <disable_collisions link1="fr3_leftfinger" link2="fr3_link8" reason="Never"/>
+  <disable_collisions link1="fr3_link3" link2="fr3_rightfinger" reason="Never"/>
+  <disable_collisions link1="fr3_link4" link2="fr3_rightfinger" reason="Never"/>
+  <disable_collisions link1="fr3_link6" link2="fr3_rightfinger" reason="Never"/>
+  <disable_collisions link1="fr3_link7" link2="fr3_rightfinger" reason="Never"/>
+  <disable_collisions link1="fr3_link8" link2="fr3_rightfinger" reason="Never"/>
+
+  <!-- gripper for manipulation planning -->
+  <gripper name="gripper" clearance="0.05">
+    <position xyz="0 0 0" wxyz="1 0 0 0" />
+    <link name="fr3_hand_tcp"/>
+  </gripper>
+</robot>