diff --git a/launch/tutorial_6_launch.py b/launch/tutorial_6_launch.py index 30e81f5..ad1adf6 100644 --- a/launch/tutorial_6_launch.py +++ b/launch/tutorial_6_launch.py @@ -8,6 +8,7 @@ import xacro from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext, LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, @@ -17,8 +18,6 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch import LaunchContext, LaunchDescription - def get_robot_description_and_controllers(context: LaunchContext, robot_type): robot_type_str = context.perform_substitution(robot_type) diff --git a/launch/tutorial_7_launch.py b/launch/tutorial_7_launch.py index cbd1453..da60d40 100644 --- a/launch/tutorial_7_launch.py +++ b/launch/tutorial_7_launch.py @@ -8,6 +8,7 @@ import xacro from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext, LaunchDescription from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, @@ -17,8 +18,6 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch import LaunchContext, LaunchDescription - def get_robot_description_and_controllers(context: LaunchContext, robot_type): robot_type_str = context.perform_substitution(robot_type) @@ -45,6 +44,23 @@ def get_robot_description_and_controllers(context: LaunchContext, robot_type): }, ).toxml() + detachable_joint_plugin = f""" + + + {robot_type_str}_hand + box + base_link + /box/attach + /box/detach + true + + + +""" + robot_description_xml = robot_description_xml.replace( + "", detachable_joint_plugin + ) + # Replace the Franka default controllers YAML with our tutorial controllers franka_controllers = os.path.join( get_package_share_directory("franka_gazebo_bringup"), @@ -88,6 +104,9 @@ def get_robot_description_and_controllers(context: LaunchContext, robot_type): def generate_launch_description(): robot_type = LaunchConfiguration("robot_type") + tutorial_share = get_package_share_directory("hpp_tutorial") + box_urdf = os.path.join(tutorial_share, "urdf", "box.urdf") + ground_urdf = os.path.join(tutorial_share, "urdf", "ground.urdf") # Gazebo Sim os.environ["GZ_SIM_RESOURCE_PATH"] = os.path.dirname( @@ -111,10 +130,51 @@ def generate_launch_description(): output="screen", ) + spawn_box = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + "box", + "-file", + box_urdf, + "-x", + "0.4", + "-y", + "-0.2", + "-z", + "0.0251", + ], + output="screen", + ) + + spawn_ground = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-name", + "ground", + "-file", + ground_urdf, + "-x", + "0.0", + "-y", + "0.0", + "-z", + "0.0", + ], + output="screen", + ) + bridge = Node( package="ros_gz_bridge", executable="parameter_bridge", - arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + arguments=[ + "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", + "/box/attach@std_msgs/msg/Empty]gz.msgs.Empty", + "/box/detach@std_msgs/msg/Empty]gz.msgs.Empty", + "/world/empty/set_pose@ros_gz_interfaces/srv/SetEntityPose", + ], output="screen", ) @@ -126,6 +186,8 @@ def generate_launch_description(): function=get_robot_description_and_controllers, args=[robot_type] ), spawn, + spawn_ground, + spawn_box, bridge, ] ) diff --git a/pyproject.toml b/pyproject.toml index e14e5b7..2c41eb6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,5 +4,8 @@ extend-exclude = ["cmake"] [tool.ruff.lint] extend-select = ["I", "NPY", "RUF", "UP", "W"] +[tool.ruff.lint.isort] +known-third-party = ["launch", "launch_ros"] + [tool.tomlsort] all = true diff --git a/tutorial_6/Dockerfile b/tutorial_6/Dockerfile index 05ab7c1..4b315cf 100644 --- a/tutorial_6/Dockerfile +++ b/tutorial_6/Dockerfile @@ -1,11 +1,14 @@ # Extends the base tutorial image with ROS2 control + Gazebo + Franka packages # for executing planned trajectories on a simulated Panda/FR3 robot. # -# Build: docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp-tutorial-ros2 . +# Build: docker build --build-arg DOCKER_USER=`id -u` --build-arg DOCKER_GROUP=`id -g` -t hpp-ros2:tuto . # Run: See tutorial_6/README.md for docker run instructions. FROM hpp:tuto +ARG FRANKA_DESCRIPTION_VERSION=2.6.0 +ARG FRANKA_ROS2_VERSION=v3.2.2 + USER root # ROS2 control packages @@ -24,33 +27,30 @@ RUN apt-get update -y && DEBIAN_FRONTEND=noninteractive apt-get install -qqy \ ros-jazzy-sdformat-urdf \ ros-jazzy-gz-ros2-control \ ros-jazzy-ros-gz-sim \ + ros-jazzy-ros-gz-bridge \ ros-jazzy-joint-state-publisher-gui \ ros-jazzy-xacro \ && apt-get clean && rm -rf /var/lib/apt/lists/* -# Franka Panda packages (description + Gazebo bringup) +# Franka packages used by the tutorial. Pin the source versions because the +# upstream jazzy branch can add runtime dependencies for hardware controllers +# that are not needed for this Gazebo-only example. WORKDIR /opt/franka_ws RUN mkdir -p src \ && cd src \ - && git clone --depth 1 https://github.com/frankarobotics/franka_description.git \ - && git clone --branch jazzy --depth 1 https://github.com/frankarobotics/franka_ros2.git - -RUN apt-get update && apt-get install -y python3-rosdep \ - && (rosdep init 2>/dev/null || true) \ - && rosdep update \ - && . /opt/ros/jazzy/setup.sh \ - && rosdep install --from-paths src --ignore-src -r -y \ - && apt-get clean && rm -rf /var/lib/apt/lists/* + && git clone --branch ${FRANKA_DESCRIPTION_VERSION} --depth 1 https://github.com/frankarobotics/franka_description.git \ + && git clone --branch ${FRANKA_ROS2_VERSION} --depth 1 https://github.com/frankarobotics/franka_ros2.git RUN chown -R user:user /opt/franka_ws USER user RUN . /opt/ros/jazzy/setup.sh \ - && colcon build --packages-up-to \ + && colcon build --packages-select \ franka_description \ + franka_mobile_sensors \ franka_gazebo_bringup \ - --cmake-args -DCMAKE_BUILD_TYPE=Release + --cmake-args -DCMAKE_BUILD_TYPE=Release -DBUILD_TESTING=OFF RUN echo 'source /opt/franka_ws/install/setup.bash 2>/dev/null' >> /home/user/.bashrc diff --git a/tutorial_7/README.md b/tutorial_7/README.md index c86ba39..88955a7 100644 --- a/tutorial_7/README.md +++ b/tutorial_7/README.md @@ -6,19 +6,25 @@ Having completed [tutorial 6](../tutorial_6/README.md). ## Overview -This tutorial extends tutorial 6 to a full pick-and-place scenario. It loads the -FR3 robot with a gripper, a box, and a ground plane, builds a manipulation -constraint graph, solves the pick-and-place problem, and executes on Gazebo with -gripper open/close actions between trajectory segments. +Tutorial 6 executed one arm trajectory with `send_trajectory`. This tutorial +adds the gripper: the robot must open the fingers before approaching the box, +close them before transport, then open them again at the goal. + +The script plans a pick-and-place path with an HPP manipulation constraint +graph. We then use `hpp_exec` to split the path at grasp and release +transitions and execute each segment on Gazebo. + +For the full `hpp_exec` API, see the +[hpp-exec documentation](https://gepetto.github.io/doc/hpp-exec/doxygen-html/index.html). ## Setting up the simulation -Use the same Docker image as tutorial 6 (`hpp-tutorial-ros2`). If you haven't +Use the same Docker image as tutorial 6 (`hpp-ros2:tuto`). If you have not built it yet, see the [tutorial 6 instructions](../tutorial_6/README.md). ## Terminal 1: Launching the simulation -Launch Gazebo with the FR3 including its gripper: +Launch Gazebo with the FR3 and its gripper: ``` ros2 launch hpp_tutorial tutorial_7_launch.py @@ -26,11 +32,10 @@ ros2 launch hpp_tutorial tutorial_7_launch.py Wait until you see `Configured and activated gripper_controller` in the output. -Note: One gripper finger may appear loose in Gazebo. This is a simulation artifact -with mimic joints (finger2 copies finger1 via physics constraints). It does not -affect the real robot. +Note: one gripper finger may appear loose in Gazebo. This is a simulation +artefact with mimic joints. It does not affect the tutorial. -## Terminal 2: Planning and executing +## Terminal 2: Planning Open a second terminal: @@ -38,182 +43,87 @@ Open a second terminal: docker exec -it hpp bash ``` -Source the environments and run the tutorial script: +Run the tutorial script: ``` cd ~/devel/src/hpp_tutorial/tutorial_7 python -i init.py ``` -The script loads the FR3 robot with a box on a ground plane, builds a -manipulation constraint graph, and solves a pick-and-place problem (moving the -box from one position to another). The result is a list of `configs` and `times` -with grasp/release transitions embedded in the path. +The script loads the FR3, the ground, and a box. It solves a pick-and-place +problem that moves the box from `(0.4, -0.2)` to `(0.4, 0.2)`, optimizes the +path, and time-parameterizes it with `SimpleTimeParameterization`. + +You can visualize the planned path in the browser viewer: -You can visualize the path in the viewer: ```python v = display() v.loadPath(p_timed) ``` -## Understanding segments and actions +At this point the useful objects are: -The key difference with tutorial 6 is that the trajectory has multiple phases: -approach, grasp, transport, and release. The `hpp_exec` package detects these -transitions from the constraint graph and splits the trajectory into **segments** -with gripper actions between them. +- `p_timed`: the time-parameterized HPP path. +- `configs`: sampled HPP configurations along the timed path. +- `times`: timestamps in seconds, returned by `segments_from_graph`. +- `segments`: executable arm trajectory slices with gripper pre-actions. +- `graph`: the HPP manipulation constraint graph. +- `open_gripper`, `close_gripper`, `grasp_box`, and `release_box`: small + Gazebo actions for the gripper and simulated box attachment. -### How it works +## Building execution segments -HPP's constraint graph encodes the grasp state at each configuration. The -`segments_from_graph()` function queries the graph for each waypoint to detect -when a grasp is acquired or released: +The planned path contains the approach, transport, and retreat motion in one +path. To execute it, ask `hpp_exec` to sample the timed path, insert graph +transition boundaries, and split the arm motion where a gripper action is +needed: ```python -from hpp_exec import extract_grasp_transitions +from hpp_exec import segments_from_graph -transitions = extract_grasp_transitions(configs, times, graph) -for t in transitions: - print(f"Config {t.config_index} at t={t.time:.2f}s:") - print(f" Acquired: {t.acquired}") # New grasps (close gripper) - print(f" Released: {t.released}") # Lost grasps (open gripper) +configs, times, segments = segments_from_graph( + p_timed, graph, + on_grasp=grasp_box, + on_release=release_box, +) ``` -### Segments and pre_actions +For this problem, `segments` contains three phases: -A `Segment` represents a slice of the trajectory with optional actions to run -before or after sending that segment: +| # | Phase | What the arm does | Pre-action | +|---|-----------|-----------------------------|------------| +| 0 | approach | move above the box, descend | none | +| 1 | transport | carry the box to the goal | attach and close | +| 2 | retreat | lift and return | open and detach | + +Make the initial gripper state explicit before the approach: ```python -from hpp_exec import Segment - -# Example: 3 segments for approach → grasp → retreat -segments = [ - Segment(0, 50), # Approach (no action) - Segment(50, 150, pre_actions=[close_gripper]), # Transport (close first) - Segment(150, 200, pre_actions=[open_gripper]), # Retreat (open first) -] +segments[0].pre_actions.insert(0, open_gripper) ``` -The `execute_segments()` function iterates through segments: -1. Run all `pre_actions` (stop if any returns False) -2. Send the arm trajectory for this segment -3. Run all `post_actions` (stop if any returns False) +This matters if a previous run left the simulated gripper closed. -## Executing with gripper actions +## Executing the segments -Define gripper actions as functions returning `True` on success: +Send the arm segments to the arm controller and let the pre-actions command +the gripper controller: ```python -from hpp_exec import segments_from_graph, execute_segments, send_trajectory -import numpy as np - -# Only fr3_finger_joint1 is controllable (finger2 is a mimic joint) -def close_gripper(): - send_trajectory( - [np.array([0.035]), np.array([0.0])], - [0.0, 0.5], - joint_names=['fr3_finger_joint1'], - controller_topic='/gripper_controller/follow_joint_trajectory', - ) - return True - -def open_gripper(): - send_trajectory( - [np.array([0.0]), np.array([0.035])], - [0.0, 0.5], - joint_names=['fr3_finger_joint1'], - controller_topic='/gripper_controller/follow_joint_trajectory', - ) - return True - -# Build segments from the constraint graph (auto-detects grasp/release) -segments = segments_from_graph( - configs, times, graph, - on_grasp=close_gripper, - on_release=open_gripper, -) +from hpp_exec import execute_segments -# Execute all segments with gripper actions execute_segments( segments, configs, times, - joint_names=[f'fr3_joint{i}' for i in range(1, 8)], + joint_names=[f"fr3_joint{i}" for i in range(1, 8)], joint_indices=list(range(7)), ) ``` -You should see the robot approach the box, close its gripper, move the box to -the goal position, open the gripper, and retreat. +You should see the fingers open, the arm descend, the fingers close on the +box, the arm carry the box to the goal, the fingers open, and the arm retreat. -## Reference: Franka native gripper (real robot) - -On a real Franka robot, you can use the native gripper actions which provide -force-controlled grasping via `franka_msgs`: +`init.py` also defines this convenience wrapper: ```python -import rclpy -from rclpy.node import Node -from rclpy.action import ActionClient -from franka_msgs.action import Grasp, Move - -class FrankaGripper: - def __init__(self, arm_id="fr3"): - self.arm_id = arm_id - if not rclpy.ok(): - rclpy.init() - self._node = Node("franka_gripper") - self._grasp_client = ActionClient( - self._node, Grasp, f"/{arm_id}_gripper/grasp" - ) - self._move_client = ActionClient( - self._node, Move, f"/{arm_id}_gripper/move" - ) - - def open(self) -> bool: - """Open gripper using Move action (position control).""" - if not self._move_client.wait_for_server(timeout_sec=10.0): - return False - goal = Move.Goal() - goal.width = 0.08 # Fully open (meters) - goal.speed = 0.05 # m/s - future = self._move_client.send_goal_async(goal) - rclpy.spin_until_future_complete(self._node, future, timeout_sec=10.0) - goal_handle = future.result() - if not goal_handle or not goal_handle.accepted: - return False - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(self._node, result_future, timeout_sec=30.0) - return True - - def close(self) -> bool: - """Close gripper using Grasp action (force-controlled).""" - if not self._grasp_client.wait_for_server(timeout_sec=10.0): - return False - goal = Grasp.Goal() - goal.width = 0.02 # Target width (meters) - goal.speed = 0.05 # Closing speed (m/s) - goal.force = 50.0 # Grasping force (Newtons) - goal.epsilon.inner = 0.01 # Tolerance for grasp success - goal.epsilon.outer = 0.01 - future = self._grasp_client.send_goal_async(goal) - rclpy.spin_until_future_complete(self._node, future, timeout_sec=10.0) - goal_handle = future.result() - if not goal_handle or not goal_handle.accepted: - return False - result_future = goal_handle.get_result_async() - rclpy.spin_until_future_complete(self._node, result_future, timeout_sec=30.0) - return True - - def destroy(self): - self._node.destroy_node() - -# Usage with segments -gripper = FrankaGripper("fr3") -segments = segments_from_graph( - configs, times, graph, - on_grasp=gripper.close, - on_release=gripper.open, -) -execute_segments(segments, configs, times, ...) -gripper.destroy() +execute_on_gazebo() ``` diff --git a/tutorial_7/init.py b/tutorial_7/init.py index 42b2379..7d3b31b 100644 --- a/tutorial_7/init.py +++ b/tutorial_7/init.py @@ -1,3 +1,5 @@ +import time + import numpy as np from pinocchio import SE3 from pyhpp.constraints import ComparisonType, ComparisonTypes, LockedJoint @@ -6,6 +8,21 @@ from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory from pyhpp_viser import Viewer +try: + import rclpy + from geometry_msgs.msg import Pose + from hpp_exec import execute_segments, segments_from_graph, send_trajectory + from rclpy.node import Node + from ros_gz_interfaces.msg import Entity + from ros_gz_interfaces.srv import SetEntityPose + from std_msgs.msg import Empty + + ROS_EXECUTION_AVAILABLE = True + ROS_EXECUTION_IMPORT_ERROR = None +except ImportError as exc: + ROS_EXECUTION_AVAILABLE = False + ROS_EXECUTION_IMPORT_ERROR = exc + def display(): v = Viewer(robot) @@ -15,6 +32,12 @@ def display(): return v +ARM_JOINT_NAMES = [f"fr3_joint{i}" for i in range(1, 8)] +GRIPPER_JOINT_NAMES = ["fr3_finger_joint1"] +BOX_INITIAL_POSITION = (0.4, -0.2, 0.0251) +BOX_GOAL_POSITION = (0.4, 0.2, 0.0251) + + # Load FR3 robot with gripper robot = Device("tuto") @@ -98,9 +121,7 @@ def display(): 0.785, # arm (ready) 0.035, 0.035, # fingers (open) - 0.4, - -0.2, - 0.0251, + *BOX_INITIAL_POSITION, 0, 0, 0, @@ -120,9 +141,7 @@ def display(): 0.785, 0.035, 0.035, - 0.4, - 0.2, - 0.0251, + *BOX_GOAL_POSITION, 0, 0, 0, @@ -161,21 +180,196 @@ def display(): p_timed = stp.optimize(p_opt) print(f"Trajectory duration: {p_timed.length():.3f} s") -# Extract waypoints -n_samples = 200 -configs = [] -times = [] -for i in range(n_samples + 1): - t = (i / n_samples) * p_timed.length() - q, success = p_timed(t) - if success: - configs.append(np.array(q)) - times.append(t) - -print(f"Extracted {len(configs)} waypoints, ready to execute.") +print("Timed path ready for visualization and execution.") print() print("To visualize:") print(" v = display()") print(" v.loadPath(p_timed)") print() -print("To execute on Gazebo, see README.md for gripper setup and execution code.") + + +class _UnavailableGazeboExecution: + def __init__(self, import_error): + self._import_error = import_error + + def raise_unavailable(self): + raise RuntimeError( + "ROS2 / Gazebo execution helpers are unavailable in this Python " + "environment." + ) from self._import_error + + def attach(self): + self.raise_unavailable() + + def detach(self): + self.raise_unavailable() + + def reset_pose(self, *_args, **_kwargs): + self.raise_unavailable() + + +if ROS_EXECUTION_AVAILABLE: + + class GazeboBoxController: + def __init__( + self, + world_name="empty", + attach_topic="/box/attach", + detach_topic="/box/detach", + pose_service=None, + settle_time=0.2, + ): + self.attach_topic = attach_topic + self.detach_topic = detach_topic + self.pose_service = pose_service or f"/world/{world_name}/set_pose" + self.settle_time = settle_time + self._node = None + self._attach_pub = None + self._detach_pub = None + self._pose_client = None + self._attached = False + + def _ensure_node(self): + if self._node is not None: + return self._node + + if not rclpy.ok(): + rclpy.init() + + self._node = Node("tutorial_7_box_controller") + self._attach_pub = self._node.create_publisher(Empty, self.attach_topic, 1) + self._detach_pub = self._node.create_publisher(Empty, self.detach_topic, 1) + self._pose_client = self._node.create_client( + SetEntityPose, self.pose_service + ) + return self._node + + def _publish(self, publisher_attr, action_name): + node = self._ensure_node() + publisher = getattr(self, publisher_attr) + publisher.publish(Empty()) + rclpy.spin_once(node, timeout_sec=0.05) + time.sleep(self.settle_time) + node.get_logger().info(f"Published '{action_name}' on Gazebo topic") + return True + + def attach(self): + if self._attached: + return True + ok = self._publish("_attach_pub", self.attach_topic) + if ok: + self._attached = True + return ok + + def detach(self): + ok = self._publish("_detach_pub", self.detach_topic) + if ok: + self._attached = False + return ok + + def reset_pose(self, xyz=BOX_INITIAL_POSITION): + node = self._ensure_node() + + if not self.detach(): + return False + + if not self._pose_client.wait_for_service(timeout_sec=3.0): + node.get_logger().error( + f"Gazebo pose service '{self.pose_service}' is unavailable" + ) + return False + + req = SetEntityPose.Request() + req.entity.name = "box" + req.entity.type = Entity.MODEL + req.pose = Pose() + req.pose.position.x = float(xyz[0]) + req.pose.position.y = float(xyz[1]) + req.pose.position.z = float(xyz[2]) + req.pose.orientation.w = 1.0 + + future = self._pose_client.call_async(req) + rclpy.spin_until_future_complete(node, future, timeout_sec=3.0) + result = future.result() + if result is None or not result.success: + node.get_logger().error("Failed to reset the Gazebo box pose") + return False + + time.sleep(self.settle_time) + node.get_logger().info( + f"Reset box pose to x={xyz[0]:.3f}, y={xyz[1]:.3f}, z={xyz[2]:.3f}" + ) + return True + + box_controller = GazeboBoxController() + + def open_gripper(): + return send_trajectory( + [np.array([0.0]), np.array([0.035])], + [0.0, 0.5], + joint_names=GRIPPER_JOINT_NAMES, + controller_topic="/gripper_controller/follow_joint_trajectory", + ) + + def close_gripper(): + return send_trajectory( + [np.array([0.035]), np.array([0.0])], + [0.0, 0.5], + joint_names=GRIPPER_JOINT_NAMES, + controller_topic="/gripper_controller/follow_joint_trajectory", + ) + + def grasp_box(): + return box_controller.attach() and close_gripper() + + def release_box(): + return open_gripper() and box_controller.detach() + + def prepare_sim_for_run(): + return box_controller.reset_pose(BOX_INITIAL_POSITION) and open_gripper() + + configs, times, segments = segments_from_graph( + p_timed, + graph, + on_grasp=grasp_box, + on_release=release_box, + ) + print(f"Extracted {len(configs)} waypoints, ready to execute.") + if segments: + segments[0].pre_actions.insert(0, open_gripper) + + def execute_on_gazebo(): + return execute_segments( + segments, + configs, + times, + joint_names=ARM_JOINT_NAMES, + joint_indices=list(range(7)), + ) + + print("To execute on Gazebo:") + print(" prepare_sim_for_run()") + print(" execute_on_gazebo()") + print() + print("Useful helpers:") + print(" open_gripper()") + print(" close_gripper()") + print(" box_controller.attach()") + print(" box_controller.detach()") + print(" box_controller.reset_pose()") +else: + box_controller = _UnavailableGazeboExecution(ROS_EXECUTION_IMPORT_ERROR) + segments = None + + def _unavailable(*_args, **_kwargs): + box_controller.raise_unavailable() + + open_gripper = _unavailable + close_gripper = _unavailable + grasp_box = _unavailable + release_box = _unavailable + prepare_sim_for_run = _unavailable + execute_on_gazebo = _unavailable + + print("ROS2 / Gazebo execution helpers are unavailable in this Python env.") + print(f"Import error: {ROS_EXECUTION_IMPORT_ERROR}")