From 16ed03d7732a40bb59f35318e596ce993f87a454 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Wed, 16 Apr 2025 13:41:00 -0700 Subject: [PATCH 01/14] Joint Trajectory Action Server now accepts positions to move joints. Enabled position mode in Stretch Mujoco Driver because it works now. --- .../joint_trajectory_server.py | 195 ++++++++++++++++-- .../stretch_mujoco_driver.py | 47 +---- 2 files changed, 188 insertions(+), 54 deletions(-) diff --git a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py index bc3e98f4..aa4f26f1 100644 --- a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py +++ b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 +from functools import cache import time import copy import pickle @@ -17,7 +18,11 @@ from rclpy.duration import Duration from control_msgs.action import FollowJointTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint, MultiDOFJointTrajectory, JointTrajectory +from trajectory_msgs.msg import ( + JointTrajectoryPoint, + MultiDOFJointTrajectory, + JointTrajectory, +) import hello_helpers.hello_misc as hm @@ -26,20 +31,182 @@ if TYPE_CHECKING: from stretch_mujoco_driver.stretch_mujoco_driver import StretchMujocoDriver +from stretch_mujoco.enums.actuators import Actuators + +import rclpy +import rclpy.action +from rclpy.node import Node +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectoryPoint +from rclpy.action import ActionServer +from rclpy.executors import MultiThreadedExecutor + + class JointTrajectoryAction: - def __init__(self, node: "StretchMujocoDriver", action_server_rate_hz:int): + def __init__(self, node: "StretchMujocoDriver", action_server_rate_hz: int): self.node = node self._goal_handle = None - self._goal_lock = threading.Lock() - self.server = ActionServer(self.node, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory', - execute_callback=self.execute_cb, - cancel_callback=self.cancel_cb, - goal_callback=self.goal_cb, - handle_accepted_callback=self.handle_accepted_cb, - callback_group=node.main_group) - - def goal_cb(self, goal_request: FollowJointTrajectory.Goal): raise NotImplementedError() - def handle_accepted_cb(self, goal_handle:ServerGoalHandle):raise NotImplementedError() - def execute_cb(self, goal_handle:ServerGoalHandle): raise NotImplementedError() - def cancel_cb(self, goal_handle:ServerGoalHandle):raise NotImplementedError() \ No newline at end of file + self._action_server = ActionServer( + self.node, + FollowJointTrajectory, + "/stretch_controller/follow_joint_trajectory", + execute_callback=self.execute_callback, + goal_callback=self.goal_callback, + cancel_callback=self.cancel_callback, + handle_accepted_callback=self.handle_accepted_callback, + callback_group=node.main_group, + ) + self.timeout = 0.2 # seconds + self.last_goal_time = self.node.get_clock().now().to_msg() + + self.latest_goal_id = 0 + + def handle_accepted_callback(self, goal_handle: ServerGoalHandle): + # This server only allows one goal at a time + if self._goal_handle is not None and self._goal_handle.is_active: + self.node.get_logger().info("Aborting previous goal") + # Abort the existing goal + # self._goal_handle.abort() \TODO(@hello-atharva): This is causing state transition issues. + self._goal_handle = goal_handle + + # Increment goal ID + self.latest_goal_id += 1 + + # Launch an asynch coroutine to execute the goal + goal_handle.execute() + + def goal_callback(self, goal_request): + self.node.get_logger().info("Received goal request") + new_goal_time = self.node.get_clock().now().to_msg() + time_duration = (new_goal_time.sec + new_goal_time.nanosec * pow(10, -9)) - ( + self.last_goal_time.sec + self.last_goal_time.nanosec * pow(10, -9) + ) + + if ( + self._goal_handle is not None + and self._goal_handle.is_active + and (time_duration < self.timeout) + ): + return ( + GoalResponse.REJECT + ) # Reject goal if another goal is currently active + + self.last_goal_time = self.node.get_clock().now().to_msg() + return GoalResponse.ACCEPT + + def cancel_callback(self, goal_handle): + self.node.get_logger().info("Received cancel request") + return CancelResponse.ACCEPT + + def execute_callback(self, goal_handle): + self.node.get_logger().info("Executing trajectory...") + + trajectory = goal_handle.request.trajectory + joint_names = trajectory.joint_names + last_positions = {name: 0.0 for name in joint_names} + + for point in trajectory.points: + positions: list[float] = point.positions + velocities: list[float | None] = ( + point.velocities if point.velocities else [None] * len(joint_names) + ) + + for i, joint in enumerate(joint_names): + try: + actuator = get_actuator_by_joint_names_in_command_groups(joint) + except: + self.node.get_logger().error(f"No command group for joint '{joint}'") + continue + + target_position = positions[i] + delta = target_position - last_positions[joint] + last_positions[joint] = target_position + velocity = velocities[i] + + if (actuator == Actuators.left_wheel_vel or actuator == Actuators.right_wheel_vel) and velocity is not None: + self.node.sim.set_base_velocity(velocity, 0) + continue + + self.node.sim.move_to(actuator, target_position, timeout=None) + + # Simulate wait until point.time_from_start + self._wait_until( + point.time_from_start.sec + point.time_from_start.nanosec * 1e-9 + ) + + goal_handle.succeed() + result = FollowJointTrajectory.Result() + self.node.get_logger().info("Trajectory execution complete") + return result + + def _wait_until(self, seconds): + loop_rate = self.node.create_rate(10) + t_start = self.node.get_clock().now().seconds_nanoseconds()[0] + while ( + self.node.get_clock().now().seconds_nanoseconds()[0] - t_start + ) < seconds: + loop_rate.sleep() + + +def get_joint_names_in_mjcf(actuator: Actuators) -> list[str]: + # Temporary until stretch_mujoco PR#39 is merged in, then we can use Actuators.get_joint_names_in_mjcf: + """ + An actuator may have multiple joints. Return their names here. Useful for querying positions from Mujoco. + """ + if actuator == Actuators.left_wheel_vel: + return ["joint_left_wheel"] + if actuator == Actuators.right_wheel_vel: + return ["joint_right_wheel"] + if actuator == Actuators.lift: + return ["joint_lift"] + if actuator == Actuators.arm: + return ["joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3"] + if actuator == Actuators.wrist_yaw: + return ["joint_wrist_yaw"] + if actuator == Actuators.wrist_pitch: + return ["joint_wrist_pitch"] + if actuator == Actuators.wrist_roll: + return ["joint_wrist_roll"] + if actuator == Actuators.gripper: + return ["joint_gripper_slide"] + if actuator == Actuators.head_pan: + return ["joint_head_pan"] + if actuator == Actuators.head_tilt: + return ["joint_head_tilt"] + + raise NotImplementedError(f"Joint names for {actuator} are not defined.") + + +@cache +def get_actuator_by_joint_names_in_command_groups(joint_name: str) -> Actuators: + """ + Joint names defined by stretch_core command groups, return their Actuator here. + """ + if joint_name == "joint_left_wheel": + return Actuators.left_wheel_vel + if joint_name == "joint_right_wheel": + return Actuators.right_wheel_vel + if joint_name == 'translate_mobile_base' or joint_name == 'position': + return Actuators.base_translate + if joint_name == 'rotate_mobile_base': + return Actuators.base_rotate + + if joint_name == "joint_lift": + return Actuators.lift + if joint_name == "joint_arm": + return Actuators.arm + if joint_name == "joint_wrist_yaw": + return Actuators.wrist_yaw + if joint_name == "joint_wrist_pitch": + return Actuators.wrist_pitch + if joint_name == "joint_wrist_roll": + return Actuators.wrist_roll + if joint_name == "joint_gripper_slide" or joint_name == "joint_gripper_finger_left" or joint_name == "joint_gripper_finger_right" or joint_name == "gripper_aperture": + return Actuators.gripper + if joint_name == "joint_head_pan": + return Actuators.head_pan + if joint_name == "joint_head_tilt": + return Actuators.head_tilt + + raise NotImplementedError(f"Actuator for {joint_name} is not defined.") diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index b2d02d36..7b260a64 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -236,13 +236,13 @@ def move_to_position(self, qpos): "Received qpos does not match the number of joints in the robot" ) return - self.sim.move_to("arm", qpos[Idx.ARM]) - self.sim.move_to("lift", qpos[Idx.LIFT]) - self.sim.move_to("wrist_yaw", qpos[Idx.WRIST_YAW]) - self.sim.move_to("wrist_pitch", qpos[Idx.WRIST_PITCH]) - self.sim.move_to("wrist_roll", qpos[Idx.WRIST_ROLL]) - self.sim.move_to("head_pan", qpos[Idx.HEAD_PAN]) - self.sim.move_to("head_tilt", qpos[Idx.HEAD_TILT]) + self.sim.move_to(Actuators.arm, qpos[Idx.ARM]) + self.sim.move_to(Actuators.lift, qpos[Idx.LIFT]) + self.sim.move_to(Actuators.wrist_yaw, qpos[Idx.WRIST_YAW]) + self.sim.move_to(Actuators.wrist_pitch, qpos[Idx.WRIST_PITCH]) + self.sim.move_to(Actuators.wrist_roll, qpos[Idx.WRIST_ROLL]) + self.sim.move_to(Actuators.head_pan, qpos[Idx.HEAD_PAN]) + self.sim.move_to(Actuators.head_tilt, qpos[Idx.HEAD_TILT]) if ( abs(qpos[Idx.BASE_TRANSLATE]) > 0.0 and abs(qpos[Idx.BASE_ROTATE]) > 0.0 @@ -740,10 +740,6 @@ def turn_on_position_mode(self): # mobile base. It does not update the virtual prismatic # joint. The frames associated with 'floor_link' and # 'base_link' become identical in this mode. - raise NotImplementedError( - "Position Mode is not yet supported in StretchMujocoDriver." - ) - def code_to_run(): # self.sim.base.enable_pos_incr_mode() ... @@ -1496,35 +1492,6 @@ def get_camera_frame(camera: StretchCameras): raise NotImplementedError(f"Camera {camera} frame is not implemented") -def get_joint_names_in_mjcf(actuator): - # Temporary until stretch_mujoco PR#39 is merged in, then we can use Actuators.get_joint_names_in_mjcf: - """ - An actuator may have multiple joints. Return their names here. - """ - if actuator == Actuators.left_wheel_vel: - return ["joint_left_wheel"] - if actuator == Actuators.right_wheel_vel: - return ["joint_right_wheel"] - if actuator == Actuators.lift: - return ["joint_lift"] - if actuator == Actuators.arm: - return ["joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3"] - if actuator == Actuators.wrist_yaw: - return ["joint_wrist_yaw"] - if actuator == Actuators.wrist_pitch: - return ["joint_wrist_pitch"] - if actuator == Actuators.wrist_roll: - return ["joint_wrist_roll"] - if actuator == Actuators.gripper: - return ["joint_gripper_slide"] - if actuator == Actuators.head_pan: - return ["joint_head_pan"] - if actuator == Actuators.head_tilt: - return ["joint_head_tilt"] - - raise NotImplementedError(f"Joint names for {actuator} are not defined.") - - def main(): rclpy.init() From f3a850d3f2d31d6f79381fb90bbcdc172b2ea7e6 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Wed, 16 Apr 2025 15:40:59 -0700 Subject: [PATCH 02/14] Added instructions to the README and a launch file to use Stretch Web Teleop. --- stretch_simulation/README.md | 45 +++ ...stretch_simulation_web_interface.launch.py | 288 ++++++++++++++++++ .../joint_trajectory_server.py | 8 +- .../stretch_mujoco_driver.py | 66 ++-- 4 files changed, 370 insertions(+), 37 deletions(-) create mode 100644 stretch_simulation/launch/stretch_simulation_web_interface.launch.py diff --git a/stretch_simulation/README.md b/stretch_simulation/README.md index 394aadec..c820ce34 100644 --- a/stretch_simulation/README.md +++ b/stretch_simulation/README.md @@ -86,6 +86,51 @@ ros2 param set /global_costmap/global_costmap inflation_layer.inflation_radius 0 ros2 param set /local_costmap/local_costmap inflation_layer.inflation_radius 0.20 ``` +### Web Teleop + +You can use Stretch Web Teleop with the Stretch Simulation environment! + +First you should install `NodeJS` and `npm` if you don't already have them: +```shell +curl -sL https://deb.nodesource.com/setup_22.x | sudo -E bash - +sudo apt install -y nodejs +``` + +Install dependencies for Web Teleop: +```shell +cd ~/ament_ws/src/stretch_web_teleop +npm install --legacy-peer-deps +npx install playwright ║ +sudo npx playwright install-deps +openssl req -new -x509 -nodes -out certificates/server.crt -keyout certificates/server.key + +sudo add-apt-repository ppa:sweptlaser/python3-pcl +sudo apt update +sudo apt install python3-pcl +``` + +Use the following commands to start Stretch Mujoco with Web Teleop: +```shell +# Terminal 1 +ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true + +# Terminal 2 +ros2 launch stretch_simulation stretch_simulation_web_interface.launch.py + +# Terminal 3 +cd ~/ament_ws/src/stretch_web_teleop +npm run localstorage + +# Terminal 4 +cd ~/ament_ws/src/stretch_web_teleop +sudo keyfile="server.key" certfile="server.crt" node ./server.js + +# Terminal 4 +cd ~/ament_ws/src/stretch_web_teleop +node start_robot_browser.js + +``` + ## Cameras and PointClouds Please use the `use_cameras:=true` argument to enable cameras and pointclouds. e.g. `ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=true mode:=navigation use_cameras:=true` diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py new file mode 100644 index 00000000..9417119e --- /dev/null +++ b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py @@ -0,0 +1,288 @@ +import fnmatch +import os + +from ament_index_python import get_package_share_directory +from ament_index_python.packages import get_package_share_path +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + GroupAction, + IncludeLaunchDescription, +) +from launch.conditions import LaunchConfigurationNotEquals +from launch.launch_description_sources import ( + FrontendLaunchDescriptionSource, + PythonLaunchDescriptionSource, +) +from launch.substitutions import ( + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +def generate_launch_description(): + teleop_interface_package = str(get_package_share_path("stretch_web_teleop")) + core_package = str(get_package_share_path("stretch_core")) + rosbridge_package = str(get_package_share_path("rosbridge_server")) + stretch_navigation_path = str(get_package_share_directory("stretch_nav2")) + + # Declare launch arguments + params_file = DeclareLaunchArgument( + "params", + default_value=[ + PathJoinSubstitution( + [ + teleop_interface_package, + "config", + "configure_video_streams_params.yaml", + ] + ) + ], + ) + map_yaml = DeclareLaunchArgument( + "map_yaml", description="filepath to previously captured map", default_value="" + ) + tts_engine = DeclareLaunchArgument( + "tts_engine", + description="name of the TTS engine. Either pyttsx3 or gtts.", + default_value="gtts", + ) + certfile_arg = DeclareLaunchArgument( + "certfile", default_value="server.crt" + ) + keyfile_arg = DeclareLaunchArgument( + "keyfile", default_value="server.key" + ) + nav2_params_file_param = DeclareLaunchArgument( + "nav2_params_file", + default_value=os.path.join( + stretch_navigation_path, "config", "nav2_params.yaml" + ), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + # Start collecting nodes to launch + ld = LaunchDescription( + [ + map_yaml, + tts_engine, + nav2_params_file_param, + params_file, + certfile_arg, + keyfile_arg, + ] + ) + + ld.add_action( + Node( + package="image_publisher", + executable="image_publisher_node", + name="navigation_camera_node", + output="screen", + parameters=[{"publish_rate": 15.0}], + remappings=[("image_raw", "/camera/cam_nav_rgb_raw")], + arguments=[ + PathJoinSubstitution( + [teleop_interface_package, "nodes", "blank_image.png"] + ) + ], + ) + ) + + ld.add_action( + Node( + package="image_publisher", + executable="image_publisher_node", + name="gripper_camera_node", + output="screen", + parameters=[{"publish_rate": 15.0}], + remappings=[("image_raw", "/camera/camera_d405_rgb_raw")], + arguments=[ + PathJoinSubstitution( + [teleop_interface_package, "nodes", "blank_image.png"] + ) + ], + ) + ) + + tf2_web_republisher_node = Node( + package="tf2_web_republisher_py", + executable="tf2_web_republisher", + name="tf2_web_republisher_node", + ) + ld.add_action(tf2_web_republisher_node) + + # Rosbridge Websocket + rosbridge_launch = IncludeLaunchDescription( + FrontendLaunchDescriptionSource( + PathJoinSubstitution( + [rosbridge_package, "launch", "rosbridge_websocket_launch.xml"] + ) + ), + launch_arguments={ + "port": "9090", + "address": "localhost", + "ssl": "true", + "certfile": PathJoinSubstitution( + [ + teleop_interface_package, + "certificates", + LaunchConfiguration("certfile"), + ] + ), + "keyfile": PathJoinSubstitution( + [ + teleop_interface_package, + "certificates", + LaunchConfiguration("keyfile"), + ] + ), + "authenticate": "false", + "call_services_in_new_thread": "true", + }.items(), + ) + ld.add_action(rosbridge_launch) + + + + # Configure Video Streams + labels = ["overhead", "realsense", "gripper"] + for i in range(len(labels)): + bools = ["False", "False", "False"] + bools[i] = "True" + label = labels[i] + configure_video_streams_node = Node( + package="stretch_web_teleop", + executable="configure_video_streams.py", + name=f"configure_video_streams_{label}", + output="screen", + arguments=[ + LaunchConfiguration("params"), + str(False), + *bools, + ], + parameters=[ + { + "has_beta_teleop_kit": False, + "stretch_tool": "stretch_tool", + } + ], + ) + ld.add_action(configure_video_streams_node) + + + navigation_bringup_launch = GroupAction( + condition=LaunchConfigurationNotEquals("map_yaml", ""), + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [stretch_navigation_path, "/launch/bringup_launch.py"] + ), + launch_arguments={ + "use_sim_time": "false", + "autostart": "true", + "map": PathJoinSubstitution( + [ + teleop_interface_package, + "maps", + LaunchConfiguration("map_yaml"), + ] + ), + "params_file": LaunchConfiguration("nav2_params_file"), + "use_rviz": "false", + }.items(), + ), + ], + ) + ld.add_action(navigation_bringup_launch) + + ld.add_action( + ExecuteProcess( + cmd=[ + [ + FindExecutable(name="ros2"), + " service call ", + "/reinitialize_global_localization ", + "std_srvs/srv/Empty ", + '"{}"', + ] + ], + shell=True, + ), + ) + + ld.add_action( + ExecuteProcess( + cmd=[ + [ + FindExecutable(name="ros2"), + " param set ", + "/rosbridge_websocket ", + "std_msgs/msg/Bool ", + "true", + ] + ], + shell=True, + ) + ) + + # ld.add_action( + # ExecuteProcess( + # cmd=[ + # [ + # FindExecutable(name="ros2"), + # " param set ", + # "/gripper_camera ", + # "depth_module.enable_auto_exposure ", + # "true", + # ] + # ], + # shell=True, + # ) + # ) + + # Move To Pre-grasp Action Server + # move_to_pregrasp_node = Node( + # package="stretch_web_teleop", + # executable="move_to_pregrasp.py", + # output="screen", + # arguments=[LaunchConfiguration("params")], + # parameters=[], + # ) + # ld.add_action(move_to_pregrasp_node) + + # Text to speech + text_to_speech_node = Node( + package="stretch_web_teleop", + executable="text_to_speech.py", + output="screen", + arguments=[LaunchConfiguration("tts_engine")], + parameters=[], + ) + ld.add_action(text_to_speech_node) + + # if stretch_tool == "eoa_wrist_dw3_tool_tablet_12in": + # detect_body_landmarks_node = Node( + # package="stretch_show_tablet", + # executable="detect_body_landmarks", + # output="screen", + # ) + # ld.add_action(detect_body_landmarks_node) + + # plan_tablet_pose_node = Node( + # package="stretch_show_tablet", + # executable="plan_tablet_pose_service", + # output="screen", + # ) + # ld.add_action(plan_tablet_pose_node) + + # show_tablet_node = Node( + # package="stretch_show_tablet", + # executable="show_tablet_server", + # output="screen", + # ) + # ld.add_action(show_tablet_node) + + return ld diff --git a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py index aa4f26f1..f5af5e3f 100644 --- a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py +++ b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py @@ -77,7 +77,7 @@ def handle_accepted_callback(self, goal_handle: ServerGoalHandle): goal_handle.execute() def goal_callback(self, goal_request): - self.node.get_logger().info("Received goal request") + self.node.get_logger().info(f"Received goal request, {goal_request}") new_goal_time = self.node.get_clock().now().to_msg() time_duration = (new_goal_time.sec + new_goal_time.nanosec * pow(10, -9)) - ( self.last_goal_time.sec + self.last_goal_time.nanosec * pow(10, -9) @@ -131,9 +131,9 @@ def execute_callback(self, goal_handle): self.node.sim.move_to(actuator, target_position, timeout=None) # Simulate wait until point.time_from_start - self._wait_until( - point.time_from_start.sec + point.time_from_start.nanosec * 1e-9 - ) + # self._wait_until( + # point.time_from_start.sec + point.time_from_start.nanosec * 1e-9 + # ) goal_handle.succeed() result = FollowJointTrajectory.Result() diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index 7b260a64..779abd94 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -884,39 +884,39 @@ def deactivate_streaming_position_service_callback(self, request, response): def get_joint_states_callback(self, request, response): joint_limits = JointState() joint_limits.header.stamp = self.get_clock().now().to_msg() - cgs = list( - set(self.joint_trajectory_action.command_groups) - - set( - [ - self.joint_trajectory_action.mobile_base_cg, - self.joint_trajectory_action.gripper_cg, - ] - ) - ) - for cg in cgs: - lower_limit, upper_limit = cg.range - joint_limits.name.append(cg.name) - joint_limits.position.append( - lower_limit - ) # Misuse position array to mean lower limits - joint_limits.velocity.append( - upper_limit - ) # Misuse velocity array to mean upper limits - - gripper_cg = self.joint_trajectory_action.gripper_cg - if gripper_cg is not None: - lower_aperture_limit, upper_aperture_limit = gripper_cg.range_aperture_m - joint_limits.name.append("gripper_aperture") - joint_limits.position.append(lower_aperture_limit) - joint_limits.velocity.append(upper_aperture_limit) - - lower_finger_limit, upper_finger_limit = gripper_cg.range_finger_rad - joint_limits.name.append("joint_gripper_finger_left") - joint_limits.position.append(lower_finger_limit) - joint_limits.velocity.append(upper_finger_limit) - joint_limits.name.append("joint_gripper_finger_right") - joint_limits.position.append(lower_finger_limit) - joint_limits.velocity.append(upper_finger_limit) + # cgs = list( + # set(self.joint_trajectory_action.command_groups) + # - set( + # [ + # self.joint_trajectory_action.mobile_base_cg, + # self.joint_trajectory_action.gripper_cg, + # ] + # ) + # ) + # for cg in cgs: + # lower_limit, upper_limit = cg.range + # joint_limits.name.append(cg.name) + # joint_limits.position.append( + # lower_limit + # ) # Misuse position array to mean lower limits + # joint_limits.velocity.append( + # upper_limit + # ) # Misuse velocity array to mean upper limits + + # gripper_cg = self.joint_trajectory_action.gripper_cg + # if gripper_cg is not None: + # lower_aperture_limit, upper_aperture_limit = gripper_cg.range_aperture_m + # joint_limits.name.append("gripper_aperture") + # joint_limits.position.append(lower_aperture_limit) + # joint_limits.velocity.append(upper_aperture_limit) + + # lower_finger_limit, upper_finger_limit = gripper_cg.range_finger_rad + # joint_limits.name.append("joint_gripper_finger_left") + # joint_limits.position.append(lower_finger_limit) + # joint_limits.velocity.append(upper_finger_limit) + # joint_limits.name.append("joint_gripper_finger_right") + # joint_limits.position.append(lower_finger_limit) + # joint_limits.velocity.append(upper_finger_limit) self.joint_limits_pub.publish(joint_limits) response.success = True From a7ad51156be00721cb769b39cdf564c59d01cdac Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Thu, 17 Apr 2025 12:09:42 -0700 Subject: [PATCH 03/14] Joint limits are now published. is_homed is always published as True. --- .../joint_trajectory_server.py | 29 ---------- .../stretch_mujoco_driver.py | 57 +++++++------------ 2 files changed, 22 insertions(+), 64 deletions(-) diff --git a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py index f5af5e3f..95a88098 100644 --- a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py +++ b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py @@ -149,35 +149,6 @@ def _wait_until(self, seconds): loop_rate.sleep() -def get_joint_names_in_mjcf(actuator: Actuators) -> list[str]: - # Temporary until stretch_mujoco PR#39 is merged in, then we can use Actuators.get_joint_names_in_mjcf: - """ - An actuator may have multiple joints. Return their names here. Useful for querying positions from Mujoco. - """ - if actuator == Actuators.left_wheel_vel: - return ["joint_left_wheel"] - if actuator == Actuators.right_wheel_vel: - return ["joint_right_wheel"] - if actuator == Actuators.lift: - return ["joint_lift"] - if actuator == Actuators.arm: - return ["joint_arm_l0", "joint_arm_l1", "joint_arm_l2", "joint_arm_l3"] - if actuator == Actuators.wrist_yaw: - return ["joint_wrist_yaw"] - if actuator == Actuators.wrist_pitch: - return ["joint_wrist_pitch"] - if actuator == Actuators.wrist_roll: - return ["joint_wrist_roll"] - if actuator == Actuators.gripper: - return ["joint_gripper_slide"] - if actuator == Actuators.head_pan: - return ["joint_head_pan"] - if actuator == Actuators.head_tilt: - return ["joint_head_tilt"] - - raise NotImplementedError(f"Joint names for {actuator} are not defined.") - - @cache def get_actuator_by_joint_names_in_command_groups(joint_name: str) -> Actuators: """ diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index 779abd94..e01098a3 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -433,7 +433,7 @@ def command_mobile_base_velocity_and_publish_state(self): ################################################## # publish homed status homed_status = Bool() - # homed_status.data = bool(self.sim.is_homed()) + homed_status.data = True self.homed_pub.publish(homed_status) # publish runstop event @@ -665,7 +665,7 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): ... # Lidar is disabled, get_data() throws a ValueError camera_data = self.sim.pull_camera_data() - for camera, frame in camera_data.get_all(auto_rotate=True).items(): + for camera, frame in camera_data.get_all(auto_rotate=False, auto_correct_rgb=True).items(): header = Header() header.frame_id = get_camera_frame(camera) header.stamp = current_time @@ -884,39 +884,26 @@ def deactivate_streaming_position_service_callback(self, request, response): def get_joint_states_callback(self, request, response): joint_limits = JointState() joint_limits.header.stamp = self.get_clock().now().to_msg() - # cgs = list( - # set(self.joint_trajectory_action.command_groups) - # - set( - # [ - # self.joint_trajectory_action.mobile_base_cg, - # self.joint_trajectory_action.gripper_cg, - # ] - # ) - # ) - # for cg in cgs: - # lower_limit, upper_limit = cg.range - # joint_limits.name.append(cg.name) - # joint_limits.position.append( - # lower_limit - # ) # Misuse position array to mean lower limits - # joint_limits.velocity.append( - # upper_limit - # ) # Misuse velocity array to mean upper limits - - # gripper_cg = self.joint_trajectory_action.gripper_cg - # if gripper_cg is not None: - # lower_aperture_limit, upper_aperture_limit = gripper_cg.range_aperture_m - # joint_limits.name.append("gripper_aperture") - # joint_limits.position.append(lower_aperture_limit) - # joint_limits.velocity.append(upper_aperture_limit) - - # lower_finger_limit, upper_finger_limit = gripper_cg.range_finger_rad - # joint_limits.name.append("joint_gripper_finger_left") - # joint_limits.position.append(lower_finger_limit) - # joint_limits.velocity.append(upper_finger_limit) - # joint_limits.name.append("joint_gripper_finger_right") - # joint_limits.position.append(lower_finger_limit) - # joint_limits.velocity.append(upper_finger_limit) + + joint_limits_from_sim = self.sim.pull_joint_limits() + for actuator, min_max in joint_limits_from_sim.items(): + joint_name = actuator.get_joint_names_in_mjcf()[0] + if actuator == Actuators.arm: + joint_name = "joint_arm" # Instead of the telescoping names + if actuator == Actuators.gripper: + joint_name = "gripper_aperture" # A different mapping from stretch_core command_groups + if actuator in [Actuators.gripper_left_finger, Actuators.gripper_right_finger]: + joint_name = joint_name.replace("_open", "") # A different mapping from stretch_core command_groups + + joint_limits.name.append(joint_name) #type:ignore + joint_limits.position.append(min_max[0]) + joint_limits.velocity.append(min_max[1]) + + # add "wrist_extension" because it's expected downstream + arm_joint_limit = joint_limits_from_sim[Actuators.arm] + joint_limits.name.append("wrist_extension") #type:ignore + joint_limits.position.append(arm_joint_limit[0]) + joint_limits.velocity.append(arm_joint_limit[1]) self.joint_limits_pub.publish(joint_limits) response.success = True From bc7cdf76209399dc9bfe1d58549640586b984d04 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Thu, 17 Apr 2025 17:26:40 -0700 Subject: [PATCH 04/14] Runstop button now works. Fixed wrist extension limit being 1/4 shorter. All camera topics now use BEST_EFFORT to match WebTeleop. --- .../joint_trajectory_server.py | 2 +- .../stretch_mujoco_driver.py | 73 ++++++++++++++----- 2 files changed, 55 insertions(+), 20 deletions(-) diff --git a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py index 95a88098..d69800ba 100644 --- a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py +++ b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py @@ -165,7 +165,7 @@ def get_actuator_by_joint_names_in_command_groups(joint_name: str) -> Actuators: if joint_name == "joint_lift": return Actuators.lift - if joint_name == "joint_arm": + if joint_name == "joint_arm" or joint_name == "wrist_extension": return Actuators.arm if joint_name == "joint_wrist_yaw": return Actuators.wrist_yaw diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index e01098a3..1324cc39 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -4,6 +4,7 @@ import numpy as np import threading +from sensor_msgs.msg._compressed_image import CompressedImage from stretch_mujoco import StretchMujocoSimulator from stretch_mujoco.enums.actuators import Actuators from stretch_mujoco.enums.stretch_sensors import StretchSensors @@ -16,6 +17,7 @@ layouts, ) +from rclpy.qos import QoSProfile, ReliabilityPolicy from stretch_core.rwlock import RWLock from stretch_mujoco_driver.joint_trajectory_server import JointTrajectoryAction import tf2_ros @@ -438,7 +440,7 @@ def command_mobile_base_velocity_and_publish_state(self): # publish runstop event runstop_event = Bool() - # runstop_event.data = robot_status.pimu.runstop_event + runstop_event.data = self.is_runstopped() self.runstop_event_pub.publish(runstop_event) # publish stretch_driver operation mode @@ -665,7 +667,9 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): ... # Lidar is disabled, get_data() throws a ValueError camera_data = self.sim.pull_camera_data() - for camera, frame in camera_data.get_all(auto_rotate=False, auto_correct_rgb=True).items(): + for camera, frame in camera_data.get_all( + auto_rotate=True, auto_correct_rgb=True + ).items(): header = Header() header.frame_id = get_camera_frame(camera) header.stamp = current_time @@ -687,6 +691,14 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): ) self.camera_info_pub.publish(camera_info) + if not camera.is_depth: + ros_image_compressed: CompressedImage = ( + self.bridge.cv2_to_compressed_imgmsg(frame) + ) + self.camera_compressed_publishers[camera.name].publish( + ros_image_compressed + ) + if camera.is_depth: if camera == StretchCameras.cam_d405_depth: pointcloud_msg = create_pointcloud_rgb_msg( @@ -888,22 +900,29 @@ def get_joint_states_callback(self, request, response): joint_limits_from_sim = self.sim.pull_joint_limits() for actuator, min_max in joint_limits_from_sim.items(): joint_name = actuator.get_joint_names_in_mjcf()[0] + min_limit, max_limit = min_max if actuator == Actuators.arm: joint_name = "joint_arm" # Instead of the telescoping names + max_limit *= 4 # 4x the telescoping limit if actuator == Actuators.gripper: - joint_name = "gripper_aperture" # A different mapping from stretch_core command_groups - if actuator in [Actuators.gripper_left_finger, Actuators.gripper_right_finger]: - joint_name = joint_name.replace("_open", "") # A different mapping from stretch_core command_groups - - joint_limits.name.append(joint_name) #type:ignore - joint_limits.position.append(min_max[0]) - joint_limits.velocity.append(min_max[1]) - + joint_name = "gripper_aperture" # A different mapping from stretch_core command_groups + if actuator in [ + Actuators.gripper_left_finger, + Actuators.gripper_right_finger, + ]: + joint_name = joint_name.replace( + "_open", "" + ) # A different mapping from stretch_core command_groups + + joint_limits.name.append(joint_name) # type:ignore + joint_limits.position.append(min_limit) + joint_limits.velocity.append(max_limit) + # add "wrist_extension" because it's expected downstream arm_joint_limit = joint_limits_from_sim[Actuators.arm] - joint_limits.name.append("wrist_extension") #type:ignore + joint_limits.name.append("wrist_extension") # type:ignore joint_limits.position.append(arm_joint_limit[0]) - joint_limits.velocity.append(arm_joint_limit[1]) + joint_limits.velocity.append(arm_joint_limit[1] * 4) # 4x the telescoping limit self.joint_limits_pub.publish(joint_limits) response.success = True @@ -966,6 +985,8 @@ def stow_the_robot(self): self.change_mode(last_robot_mode, lambda: None) return True, "Stowed." + def is_runstopped(self): + return self.robot_mode == "runstopped" def runstop_the_robot(self, runstopped, just_change_mode=False): if runstopped: self.robot_mode_rwlock.acquire_read() @@ -976,8 +997,6 @@ def runstop_the_robot(self, runstopped, just_change_mode=False): if already_runstopped: return self.change_mode("runstopped", lambda: None) - if not just_change_mode: - self.sim.pimu.runstop_event_trigger() else: self.robot_mode_rwlock.acquire_read() already_not_runstopped = self.robot_mode != "runstopped" @@ -985,8 +1004,6 @@ def runstop_the_robot(self, runstopped, just_change_mode=False): if already_not_runstopped: return self.change_mode(self.prerunstop_mode, lambda: None) - if not just_change_mode: - self.sim.pimu.runstop_event_reset() # ROS Setup ################# def ros_setup(self): @@ -1101,19 +1118,37 @@ def ros_setup(self): self.camera_publishers = { camera.name: self.create_publisher( - Image, f"/camera/{camera.name}_raw", qos_profile=5 + Image, f"/camera/{camera.name}", qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), + ) + for camera in self.sim._cameras_to_use + } + self.camera_compressed_publishers = { + camera.name: self.create_publisher( + CompressedImage, + f"/camera/{camera.name}/compressed", + qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), ) for camera in self.sim._cameras_to_use + if not camera.is_depth } self.pointcloud_publishers = { camera.name: self.create_publisher( - PointCloud2, f"/pointcloud/{camera.name}", qos_profile=5 + PointCloud2, f"/pointcloud/{camera.name}", + qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), ) for camera in self.sim._cameras_to_use if camera.is_depth } self.camera_info_pub = self.create_publisher( - CameraInfo, f"/camera/camera_info", qos_profile=5 + CameraInfo, f"/camera/camera_info", qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ), ) self.clock_pub = self.create_publisher( From 4c16af8230137480337c15b29325dd7cc4acd5ce Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Thu, 17 Apr 2025 17:28:45 -0700 Subject: [PATCH 05/14] Added topic remappings for Stretch Web Teleop. Added README instructions on how to launch Web Teleop. --- stretch_simulation/README.md | 39 ++++-- ...stretch_simulation_web_interface.launch.py | 128 ++++++++---------- 2 files changed, 85 insertions(+), 82 deletions(-) diff --git a/stretch_simulation/README.md b/stretch_simulation/README.md index c820ce34..3d799f9e 100644 --- a/stretch_simulation/README.md +++ b/stretch_simulation/README.md @@ -109,26 +109,45 @@ sudo apt update sudo apt install python3-pcl ``` +Some more steps to get IK for gripper working: +```shell +cd stretch_description/urdf +cp ./stretch_uncalibrated.urdf stretch.urdf +sudo apt install rpl +./export_urdf.sh # It's okay if it fails on calibrated params +mkdir -p $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf +cp -r ./exported_urdf/* $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf + +# Install Pinocchio for IK (https://stack-of-tasks.github.io/pinocchio/download.html) +sudo apt install -qqy lsb-release curl +sudo mkdir -p /etc/apt/keyrings +curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc \ + | sudo tee /etc/apt/keyrings/robotpkg.asc + echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \ + | sudo tee /etc/apt/sources.list.d/robotpkg.list +sudo apt update +sudo apt install -qqy robotpkg-py3*-pinocchio +echo "export PATH=/opt/openrobots/bin:$PATH;export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH;export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH;export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH;export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH" >> ~/.bashrc +``` + Use the following commands to start Stretch Mujoco with Web Teleop: ```shell +parallel_terminal="gnome-terminal --tab -- /bin/bash -c " # or "xterm -e" + # Terminal 1 -ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true +$parallel_terminal "ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true" & # Terminal 2 -ros2 launch stretch_simulation stretch_simulation_web_interface.launch.py +$parallel_terminal "ros2 launch stretch_simulation stretch_simulation_web_interface.launch.py" & # Terminal 3 -cd ~/ament_ws/src/stretch_web_teleop -npm run localstorage +$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; npm run localstorage" & # Terminal 4 -cd ~/ament_ws/src/stretch_web_teleop -sudo keyfile="server.key" certfile="server.crt" node ./server.js +$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; sudo keyfile="server.key" certfile="server.crt" node ./server.js" & # Terminal 4 -cd ~/ament_ws/src/stretch_web_teleop -node start_robot_browser.js - +$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; node start_robot_browser.js" & ``` ## Cameras and PointClouds @@ -263,7 +282,7 @@ python3 -m pip install -U hello-robot-stretch-urdf git clone https://github.com/hello-robot/stretch_urdf.git --depth 1 /tmp/stretch_urdf -python3 /tmp/stretch_urdf/tools/stretch_urdf_ros_update.py --model SE3 --tool eoa_wrist_dw3_tool_sg3 +python3 /tmp/stretch_urdf/tools/stretch_urdf_ros_update.py cd ~/ament_ws diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py index 9417119e..efb87300 100644 --- a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py +++ b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py @@ -1,4 +1,3 @@ -import fnmatch import os from ament_index_python import get_package_share_directory @@ -22,6 +21,8 @@ LaunchConfiguration, PathJoinSubstitution, ) + + def generate_launch_description(): teleop_interface_package = str(get_package_share_path("stretch_web_teleop")) core_package = str(get_package_share_path("stretch_core")) @@ -49,12 +50,8 @@ def generate_launch_description(): description="name of the TTS engine. Either pyttsx3 or gtts.", default_value="gtts", ) - certfile_arg = DeclareLaunchArgument( - "certfile", default_value="server.crt" - ) - keyfile_arg = DeclareLaunchArgument( - "keyfile", default_value="server.key" - ) + certfile_arg = DeclareLaunchArgument("certfile", default_value="server.crt") + keyfile_arg = DeclareLaunchArgument("keyfile", default_value="server.key") nav2_params_file_param = DeclareLaunchArgument( "nav2_params_file", default_value=os.path.join( @@ -74,39 +71,7 @@ def generate_launch_description(): keyfile_arg, ] ) - - ld.add_action( - Node( - package="image_publisher", - executable="image_publisher_node", - name="navigation_camera_node", - output="screen", - parameters=[{"publish_rate": 15.0}], - remappings=[("image_raw", "/camera/cam_nav_rgb_raw")], - arguments=[ - PathJoinSubstitution( - [teleop_interface_package, "nodes", "blank_image.png"] - ) - ], - ) - ) - - ld.add_action( - Node( - package="image_publisher", - executable="image_publisher_node", - name="gripper_camera_node", - output="screen", - parameters=[{"publish_rate": 15.0}], - remappings=[("image_raw", "/camera/camera_d405_rgb_raw")], - arguments=[ - PathJoinSubstitution( - [teleop_interface_package, "nodes", "blank_image.png"] - ) - ], - ) - ) - + tf2_web_republisher_node = Node( package="tf2_web_republisher_py", executable="tf2_web_republisher", @@ -145,33 +110,51 @@ def generate_launch_description(): ) ld.add_action(rosbridge_launch) + camera_topic_remappings = [ + # cam_nav_rgb_raw + ("/navigation_camera/image_raw", "/camera/cam_nav_rgb"), + # cam_d405_rgb_raw + ("/gripper_camera/image_raw", "/camera/cam_d405_rgb"), + ("/gripper_camera/image_raw/compressed", "/camera/cam_d405_rgb/compressed"), - # Configure Video Streams - labels = ["overhead", "realsense", "gripper"] - for i in range(len(labels)): - bools = ["False", "False", "False"] - bools[i] = "True" - label = labels[i] - configure_video_streams_node = Node( - package="stretch_web_teleop", - executable="configure_video_streams.py", - name=f"configure_video_streams_{label}", - output="screen", - arguments=[ - LaunchConfiguration("params"), - str(False), - *bools, - ], - parameters=[ - { - "has_beta_teleop_kit": False, - "stretch_tool": "stretch_tool", - } - ], - ) - ld.add_action(configure_video_streams_node) + # cam_d435i_rgb_raw + ("/camera/color/image_raw", "/camera/cam_d435i_rgb"), + ("/camera/color/image_raw/compressed", "/camera/cam_d435i_rgb/compressed"), + + # cam_d405_depth + ("/gripper_camera/depth/color/points", "/pointcloud/cam_d405_depth"), + + + # cam_d435i_depth + ("/camera/depth/color/points", "/pointcloud/cam_d435i_depth"), + # camera_info + ("/gripper_camera/color/camera_info", "/camera/camera_info"), + ("/camera/color/camera_info", "/camera/camera_info"), + ] + + # Configure Video Streams + configure_video_streams_node = Node( + package="stretch_web_teleop", + executable="configure_video_streams.py", + output="screen", + arguments=[ + LaunchConfiguration("params"), + str(False), + "True", # overhead" + "True", # "realsense" + "True", # "gripper" + ], + parameters=[ + { + "has_beta_teleop_kit": False, + "stretch_tool": "eoa_wrist_dw3_tool_sg3", + } + ], + remappings=camera_topic_remappings, + ) + ld.add_action(configure_video_streams_node) navigation_bringup_launch = GroupAction( condition=LaunchConfigurationNotEquals("map_yaml", ""), @@ -244,14 +227,15 @@ def generate_launch_description(): # ) # Move To Pre-grasp Action Server - # move_to_pregrasp_node = Node( - # package="stretch_web_teleop", - # executable="move_to_pregrasp.py", - # output="screen", - # arguments=[LaunchConfiguration("params")], - # parameters=[], - # ) - # ld.add_action(move_to_pregrasp_node) + move_to_pregrasp_node = Node( + package="stretch_web_teleop", + executable="move_to_pregrasp.py", + output="screen", + arguments=[LaunchConfiguration("params")], + parameters=[], + remappings=camera_topic_remappings, + ) + ld.add_action(move_to_pregrasp_node) # Text to speech text_to_speech_node = Node( From 48ea81ca7dc44e8e47a241a2eb6c5791f03b0cd6 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Fri, 18 Apr 2025 14:00:50 -0700 Subject: [PATCH 06/14] The Stretch Mujoco Driver now publishes camera topics matching what Stretch Web Teleop expects, without re-mapping --- ...stretch_simulation_web_interface.launch.py | 26 ------ stretch_simulation/rviz/stretch_sim.rviz | 67 ++++++++------- .../stretch_mujoco_driver.py | 83 +++++++++++++++++-- 3 files changed, 109 insertions(+), 67 deletions(-) diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py index efb87300..2aab0ac3 100644 --- a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py +++ b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py @@ -110,30 +110,6 @@ def generate_launch_description(): ) ld.add_action(rosbridge_launch) - camera_topic_remappings = [ - # cam_nav_rgb_raw - ("/navigation_camera/image_raw", "/camera/cam_nav_rgb"), - - # cam_d405_rgb_raw - ("/gripper_camera/image_raw", "/camera/cam_d405_rgb"), - ("/gripper_camera/image_raw/compressed", "/camera/cam_d405_rgb/compressed"), - - # cam_d435i_rgb_raw - ("/camera/color/image_raw", "/camera/cam_d435i_rgb"), - ("/camera/color/image_raw/compressed", "/camera/cam_d435i_rgb/compressed"), - - # cam_d405_depth - ("/gripper_camera/depth/color/points", "/pointcloud/cam_d405_depth"), - - - # cam_d435i_depth - ("/camera/depth/color/points", "/pointcloud/cam_d435i_depth"), - - # camera_info - ("/gripper_camera/color/camera_info", "/camera/camera_info"), - ("/camera/color/camera_info", "/camera/camera_info"), - ] - # Configure Video Streams configure_video_streams_node = Node( package="stretch_web_teleop", @@ -152,7 +128,6 @@ def generate_launch_description(): "stretch_tool": "eoa_wrist_dw3_tool_sg3", } ], - remappings=camera_topic_remappings, ) ld.add_action(configure_video_streams_node) @@ -233,7 +208,6 @@ def generate_launch_description(): output="screen", arguments=[LaunchConfiguration("params")], parameters=[], - remappings=camera_topic_remappings, ) ld.add_action(move_to_pregrasp_node) diff --git a/stretch_simulation/rviz/stretch_sim.rviz b/stretch_simulation/rviz/stretch_sim.rviz index 86405948..1f282fa8 100644 --- a/stretch_simulation/rviz/stretch_sim.rviz +++ b/stretch_simulation/rviz/stretch_sim.rviz @@ -6,9 +6,12 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /Camera1 - /Camera1/Topic1 + - /PointCloud21 + - /PointCloud21/Topic1 Splitter Ratio: 0.5 - Tree Height: 444 + Tree Height: 239 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -389,28 +392,6 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz_default_plugins/Camera - Enabled: true - Far Plane Distance: 100 - Image Rendering: background and overlay - Name: Camera - Overlay Alpha: 0.5 - Topic: - Depth: 20 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /camera/cam_d435i_rgb_raw - Value: true - Visibility: - Grid: true - LaserScan: true - Map: true - PointCloud2: false - RobotModel: true - TF: true - Value: false - Zoom Factor: 1 - Class: rviz_default_plugins/TF Enabled: false Frame Timeout: 15 @@ -446,6 +427,28 @@ Visualization Manager: Value: /map_updates Use Timestamp: false Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /camera/color/image_raw + Value: true + Visibility: + Grid: true + LaserScan: true + Map: true + PointCloud2: true + RobotModel: true + TF: true + Value: false + Zoom Factor: 1 - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -475,8 +478,8 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable - Value: /pointcloud/cam_d435i_depth + Reliability Policy: Best Effort + Value: /camera/depth/color/points Use Fixed Frame: true Use rainbow: true Value: true @@ -541,20 +544,20 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4653986096382141 + Pitch: 1.0253984928131104 Target Frame: Value: Orbit (rviz) - Yaw: 5.638580799102783 + Yaw: 6.02358341217041 Saved: ~ Window Geometry: Camera: collapsed: false Displays: collapsed: false - Height: 1043 + Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f700000359fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000023f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000282000001140000002800ffffff000000010000015f00000359fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000359000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000005afc0100000002fb0000000800540069006d0065010000000000000780000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000041e0000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000172000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002700000010b0000000000000000fb0000000c00430061006d00650072006101000001b5000001c60000002800ffffff000000010000015f0000033efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000005afc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003d80000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -563,6 +566,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1920 - X: 1920 - Y: 0 + Width: 1850 + X: 70 + Y: 27 diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index 1324cc39..70d6bf04 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -1,6 +1,7 @@ #! /usr/bin/env python3 import copy +from functools import cache import numpy as np import threading @@ -123,6 +124,7 @@ def __init__(self): sim = StretchMujocoSimulator( model=model, + camera_hz=10, cameras_to_use=( StretchCameras.all() if use_cameras else StretchCameras.none() ), @@ -668,7 +670,7 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): camera_data = self.sim.pull_camera_data() for camera, frame in camera_data.get_all( - auto_rotate=True, auto_correct_rgb=True + auto_rotate=False, auto_correct_rgb=True ).items(): header = Header() header.frame_id = get_camera_frame(camera) @@ -689,7 +691,7 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): frame_id=header.frame_id, timestamp=current_time, ) - self.camera_info_pub.publish(camera_info) + self.camera_info_publishers[camera.name].publish(camera_info) if not camera.is_depth: ros_image_compressed: CompressedImage = ( @@ -1118,7 +1120,7 @@ def ros_setup(self): self.camera_publishers = { camera.name: self.create_publisher( - Image, f"/camera/{camera.name}", qos_profile=QoSProfile( + Image, get_camera_topic_name(camera), qos_profile=QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT ), ) @@ -1127,7 +1129,7 @@ def ros_setup(self): self.camera_compressed_publishers = { camera.name: self.create_publisher( CompressedImage, - f"/camera/{camera.name}/compressed", + f"{get_camera_topic_name(camera)}/compressed", qos_profile=QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT ), @@ -1137,7 +1139,7 @@ def ros_setup(self): } self.pointcloud_publishers = { camera.name: self.create_publisher( - PointCloud2, f"/pointcloud/{camera.name}", + PointCloud2, get_camera_pointcloud_topic_name(camera), qos_profile=QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT ), @@ -1145,12 +1147,16 @@ def ros_setup(self): for camera in self.sim._cameras_to_use if camera.is_depth } - self.camera_info_pub = self.create_publisher( - CameraInfo, f"/camera/camera_info", qos_profile=QoSProfile( + self.camera_info_publishers = { + camera.name: self.create_publisher( + CameraInfo, get_camera_info_topic_name(camera), + qos_profile=QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT ), - ) - + ) + for camera in self.sim._cameras_to_use + } + self.clock_pub = self.create_publisher( msg_type=Clock, topic="/clock", qos_profile=5 ) @@ -1499,7 +1505,66 @@ def create_camera_info( return camera_info_msg +@cache +def get_camera_topic_name(camera: StretchCameras): + """ + Topic names to match the camera topics published by the real Stretch robot. + """ + if camera == StretchCameras.cam_d405_rgb: + return "/gripper_camera/image_raw" + if camera == StretchCameras.cam_d405_depth: + return "/gripper_camera/depth/image_rect_raw" + if camera == StretchCameras.cam_d435i_rgb: + return "/camera/color/image_raw" + if camera == StretchCameras.cam_d435i_depth: + return "/camera/depth/image_rect_raw" + if camera == StretchCameras.cam_nav_rgb: + return "/navigation_camera/image_raw" + + raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + +@cache +def get_camera_info_topic_name(camera: StretchCameras): + """ + Topic names to match the camera_info topics published by the real Stretch robot. + """ + if camera == StretchCameras.cam_d405_rgb: + return "/gripper_camera/camera_info" + if camera == StretchCameras.cam_d405_depth: + return "/gripper_camera/depth/camera_info" + if camera == StretchCameras.cam_d435i_rgb: + return "/camera/color/camera_info" + if camera == StretchCameras.cam_d435i_depth: + return "/camera/depth/camera_info" + if camera == StretchCameras.cam_nav_rgb: + return "/navigation_camera/camera_info" + + raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + +@cache +def get_camera_pointcloud_topic_name(camera: StretchCameras): + """ + Topic names to match the pointcloud2 topics published by the real Stretch robot. + """ + if camera == StretchCameras.cam_d405_rgb: + raise KeyError(f"{camera} camera does not have a pointcloud.") + if camera == StretchCameras.cam_d405_depth: + return "/gripper_camera/depth/color/points" + if camera == StretchCameras.cam_d435i_rgb: + raise KeyError(f"{camera} camera does not have a pointcloud.") + if camera == StretchCameras.cam_d435i_depth: + return "/camera/depth/color/points" + if camera == StretchCameras.cam_nav_rgb: + raise KeyError(f"{camera} camera does not have a pointcloud.") + + raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + + +@cache def get_camera_frame(camera: StretchCameras): + """ + Matches the simulation camera with the optical frame on the robot urdf. + """ if camera == StretchCameras.cam_d405_rgb: return "gripper_camera_color_optical_frame" if camera == StretchCameras.cam_d405_depth: From 24839d1261c54732c4576c676ebd70aa8e36ec2c Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Mon, 21 Apr 2025 10:19:18 -0700 Subject: [PATCH 07/14] LIDAR now uses best effort --- stretch_simulation/README.md | 2 +- .../launch/stretch_simulation_web_interface.launch.py | 2 +- stretch_simulation/rviz/stretch_sim.rviz | 8 ++++---- .../stretch_mujoco_driver/stretch_mujoco_driver.py | 4 +++- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/stretch_simulation/README.md b/stretch_simulation/README.md index 3d799f9e..30a87eec 100644 --- a/stretch_simulation/README.md +++ b/stretch_simulation/README.md @@ -135,7 +135,7 @@ Use the following commands to start Stretch Mujoco with Web Teleop: parallel_terminal="gnome-terminal --tab -- /bin/bash -c " # or "xterm -e" # Terminal 1 -$parallel_terminal "ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true" & +$parallel_terminal "ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true map_yaml:=${HELLO_FLEET_PATH}/maps/gshaped_modern1_robocasa.yaml" & # Terminal 2 $parallel_terminal "ros2 launch stretch_simulation stretch_simulation_web_interface.launch.py" & diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py index 2aab0ac3..36d3d854 100644 --- a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py +++ b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py @@ -139,7 +139,7 @@ def generate_launch_description(): [stretch_navigation_path, "/launch/bringup_launch.py"] ), launch_arguments={ - "use_sim_time": "false", + "use_sim_time": "true", "autostart": "true", "map": PathJoinSubstitution( [ diff --git a/stretch_simulation/rviz/stretch_sim.rviz b/stretch_simulation/rviz/stretch_sim.rviz index 1f282fa8..db5edefa 100644 --- a/stretch_simulation/rviz/stretch_sim.rviz +++ b/stretch_simulation/rviz/stretch_sim.rviz @@ -11,7 +11,7 @@ Panels: - /PointCloud21 - /PointCloud21/Topic1 Splitter Ratio: 0.5 - Tree Height: 239 + Tree Height: 348 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -80,7 +80,7 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable + Reliability Policy: Best Effort Value: /scan_filtered Use Fixed Frame: true Use rainbow: true @@ -529,7 +529,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.2111217975616455 + Distance: 4.92409086227417 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -557,7 +557,7 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000172000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002700000010b0000000000000000fb0000000c00430061006d00650072006101000001b5000001c60000002800ffffff000000010000015f0000033efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000005afc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003d80000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001f70000033efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001df000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000222000001590000002800fffffffb0000000c00430061006d00650072006101000001b5000001c60000000000000000000000010000015f0000033efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000033e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000005afc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003d80000033e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index 70d6bf04..cfa54676 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -1115,7 +1115,9 @@ def ros_setup(self): self.odom_pub = self.create_publisher(Odometry, "odom", 1) self.laser_scan_pub = self.create_publisher( - LaserScan, "/scan_filtered", qos_profile=5 + LaserScan, "/scan_filtered", qos_profile=QoSProfile( + depth=1, reliability=ReliabilityPolicy.BEST_EFFORT + ) ) self.camera_publishers = { From 6fff0297df51cb7349441a26b9fb86004406c4a0 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Wed, 23 Apr 2025 13:30:39 -0700 Subject: [PATCH 08/14] Camera matrices are now returned from stretch_mujoco's CameraSettings --- .../stretch_mujoco_driver.py | 53 ++++++------------- 1 file changed, 17 insertions(+), 36 deletions(-) diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index cfa54676..c086f395 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -9,7 +9,7 @@ from stretch_mujoco import StretchMujocoSimulator from stretch_mujoco.enums.actuators import Actuators from stretch_mujoco.enums.stretch_sensors import StretchSensors -from stretch_mujoco.enums.stretch_cameras import StretchCameras +from stretch_mujoco.enums.stretch_cameras import CameraSettings, StretchCameras from stretch_mujoco.robocasa_gen import ( layout_from_str, style_from_str, @@ -452,7 +452,7 @@ def command_mobile_base_velocity_and_publish_state(self): # publish end of arm tool tool_msg = String() - # tool_msg.data = self.sim.end_of_arm.name + tool_msg.data = "eoa_wrist_dw3_tool_sg3" self.tool_pub.publish(tool_msg) # publish streaming position status @@ -683,11 +683,9 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): ) self.camera_publishers[camera.name].publish(ros_image) - settings = camera.initial_camera_settings + settings: CameraSettings = camera.initial_camera_settings camera_info = create_camera_info( - fovy=settings.field_of_view_vertical_in_degrees, - width=settings.width, - height=settings.height, + camera_settings=settings, frame_id=header.frame_id, timestamp=current_time, ) @@ -1466,43 +1464,26 @@ def create_pointcloud_rgb_msg( def create_camera_info( - fovy: float, width: int, height: int, frame_id: str, timestamp: TimeMsg + camera_settings:CameraSettings, frame_id: str, timestamp: TimeMsg ): camera_info_msg = CameraInfo() camera_info_msg.header = Header() camera_info_msg.header.stamp = timestamp camera_info_msg.header.frame_id = frame_id - camera_info_msg.width = width - camera_info_msg.height = height + camera_info_msg.width = camera_settings.width + camera_info_msg.height = camera_settings.height camera_info_msg.distortion_model = "plumb_bob" - focal_scaling = (0.5 * height) / np.tan((fovy * np.pi / 360.0)) - camera_info_msg.d = [0.0, 0.0, 0.0, 0.0, 0.0] - camera_info_msg.k = [ - focal_scaling, - 0.0, - width / 2, - 0.0, - focal_scaling, - height / 2, - 0.0, - 0.0, - 1.0, - ] - camera_info_msg.p = [ - focal_scaling, - 0.0, - width / 2, - 0.0, - 0.0, - focal_scaling, - height / 2, - 0.0, - 0.0, - 0.0, - 1.0, - 0.0, - ] + + camera_info_msg.d = camera_settings.get_distortion_params_d() + camera_info_msg.k = camera_settings.get_intrinsic_params_k() + camera_info_msg.p = camera_settings.get_projection_matrix_p() + + if camera_settings.crop is not None: + camera_info_msg.roi.x_offset = camera_settings.crop.x_offset + camera_info_msg.roi.y_offset = camera_settings.crop.y_offset + camera_info_msg.roi.width = camera_settings.crop.width + camera_info_msg.roi.height = camera_settings.crop.height return camera_info_msg From 6680bd9d038a64af26ed569241f5a49262030de8 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Wed, 23 Apr 2025 15:27:48 -0700 Subject: [PATCH 09/14] Added a name to the configure_video_streams_node so that WebTeleop picks up the tool name correctly --- .../launch/stretch_simulation_web_interface.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py index 36d3d854..f26d4dcc 100644 --- a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py +++ b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py @@ -25,7 +25,6 @@ def generate_launch_description(): teleop_interface_package = str(get_package_share_path("stretch_web_teleop")) - core_package = str(get_package_share_path("stretch_core")) rosbridge_package = str(get_package_share_path("rosbridge_server")) stretch_navigation_path = str(get_package_share_directory("stretch_nav2")) @@ -114,6 +113,7 @@ def generate_launch_description(): configure_video_streams_node = Node( package="stretch_web_teleop", executable="configure_video_streams.py", + name=f"configure_video_streams_gripper", output="screen", arguments=[ LaunchConfiguration("params"), @@ -126,6 +126,7 @@ def generate_launch_description(): { "has_beta_teleop_kit": False, "stretch_tool": "eoa_wrist_dw3_tool_sg3", + "use_sim_time": True } ], ) From 7ef3b412a5d5db22bbd73c63a835e095ee7f6c69 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Thu, 24 Apr 2025 17:01:44 -0700 Subject: [PATCH 10/14] MoveToGrasp now works. The static transform was causing joints to not be broadcast correctly, making transform lookups fail in MoveToGrasp - this has been replaced with a wait_for_transform call to a tf buffer. A U16C1 (unsigned int, 1 channel) compressed depth map in millimeters is now published on the depth/compressed topics. --- ...stretch_simulation_web_interface.launch.py | 24 ++++---- .../stretch_mujoco_driver.py | 61 +++++++++++++------ 2 files changed, 56 insertions(+), 29 deletions(-) diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py index f26d4dcc..3a671aae 100644 --- a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py +++ b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py @@ -41,8 +41,8 @@ def generate_launch_description(): ) ], ) - map_yaml = DeclareLaunchArgument( - "map_yaml", description="filepath to previously captured map", default_value="" + map = DeclareLaunchArgument( + "map", description="filepath to previously captured map", default_value="" ) tts_engine = DeclareLaunchArgument( "tts_engine", @@ -62,7 +62,7 @@ def generate_launch_description(): # Start collecting nodes to launch ld = LaunchDescription( [ - map_yaml, + map, tts_engine, nav2_params_file_param, params_file, @@ -129,11 +129,12 @@ def generate_launch_description(): "use_sim_time": True } ], + remappings=[("/gripper_camera/color/camera_info","/gripper_camera/camera_info")] ) ld.add_action(configure_video_streams_node) navigation_bringup_launch = GroupAction( - condition=LaunchConfigurationNotEquals("map_yaml", ""), + condition=LaunchConfigurationNotEquals("map", ""), actions=[ IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -142,13 +143,7 @@ def generate_launch_description(): launch_arguments={ "use_sim_time": "true", "autostart": "true", - "map": PathJoinSubstitution( - [ - teleop_interface_package, - "maps", - LaunchConfiguration("map_yaml"), - ] - ), + "map": LaunchConfiguration("map"), "params_file": LaunchConfiguration("nav2_params_file"), "use_rviz": "false", }.items(), @@ -208,7 +203,12 @@ def generate_launch_description(): executable="move_to_pregrasp.py", output="screen", arguments=[LaunchConfiguration("params")], - parameters=[], + parameters=[{ + "use_sim_time": True}], + remappings=[ + ("/camera/aligned_depth_to_color/camera_info", "/camera/depth/camera_info"), + ("/camera/aligned_depth_to_color/image_raw/compressedDepth", "/camera/depth/image_rect_raw/compressed") + ] ) ld.add_action(move_to_pregrasp_node) diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index c086f395..6060d22f 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -1,7 +1,9 @@ #! /usr/bin/env python3 +import array import copy from functools import cache +import cv2 import numpy as np import threading @@ -691,13 +693,14 @@ def publish_camera_and_lidar(self, current_time: TimeMsg | None = None): ) self.camera_info_publishers[camera.name].publish(camera_info) - if not camera.is_depth: + if camera.is_depth: + ros_image_compressed = compress_depth_image(frame) + else: ros_image_compressed: CompressedImage = ( - self.bridge.cv2_to_compressed_imgmsg(frame) - ) - self.camera_compressed_publishers[camera.name].publish( - ros_image_compressed + self.bridge.cv2_to_compressed_imgmsg(frame, "png") ) + ros_image_compressed.header.frame_id = get_camera_frame(camera) + self.camera_compressed_publishers[camera.name].publish(ros_image_compressed) if camera.is_depth: if camera == StretchCameras.cam_d405_depth: @@ -903,7 +906,7 @@ def get_joint_states_callback(self, request, response): min_limit, max_limit = min_max if actuator == Actuators.arm: joint_name = "joint_arm" # Instead of the telescoping names - max_limit *= 4 # 4x the telescoping limit + max_limit *= 4 # 4x the telescoping limit if actuator == Actuators.gripper: joint_name = "gripper_aperture" # A different mapping from stretch_core command_groups if actuator in [ @@ -987,6 +990,7 @@ def stow_the_robot(self): def is_runstopped(self): return self.robot_mode == "runstopped" + def runstop_the_robot(self, runstopped, just_change_mode=False): if runstopped: self.robot_mode_rwlock.acquire_read() @@ -1054,7 +1058,6 @@ def ros_setup(self): "controller_calibration_file", str(stretch_core_path / "config" / "controller_calibration_head.yaml"), ) - # large_ang = np.radians(45.0) # filename = self.get_parameter('controller_calibration_file').value # self.get_logger().debug('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename)) @@ -1113,14 +1116,16 @@ def ros_setup(self): self.odom_pub = self.create_publisher(Odometry, "odom", 1) self.laser_scan_pub = self.create_publisher( - LaserScan, "/scan_filtered", qos_profile=QoSProfile( - depth=1, reliability=ReliabilityPolicy.BEST_EFFORT - ) + LaserScan, + "/scan_filtered", + qos_profile=QoSProfile(depth=1, reliability=ReliabilityPolicy.BEST_EFFORT), ) self.camera_publishers = { camera.name: self.create_publisher( - Image, get_camera_topic_name(camera), qos_profile=QoSProfile( + Image, + get_camera_topic_name(camera), + qos_profile=QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT ), ) @@ -1135,11 +1140,11 @@ def ros_setup(self): ), ) for camera in self.sim._cameras_to_use - if not camera.is_depth } self.pointcloud_publishers = { camera.name: self.create_publisher( - PointCloud2, get_camera_pointcloud_topic_name(camera), + PointCloud2, + get_camera_pointcloud_topic_name(camera), qos_profile=QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT ), @@ -1149,14 +1154,15 @@ def ros_setup(self): } self.camera_info_publishers = { camera.name: self.create_publisher( - CameraInfo, get_camera_info_topic_name(camera), + CameraInfo, + get_camera_info_topic_name(camera), qos_profile=QoSProfile( depth=1, reliability=ReliabilityPolicy.BEST_EFFORT ), ) for camera in self.sim._cameras_to_use } - + self.clock_pub = self.create_publisher( msg_type=Clock, topic="/clock", qos_profile=5 ) @@ -1463,8 +1469,28 @@ def create_pointcloud_rgb_msg( return cloud_msg +__COMPRESSED_DEPTH_16UC1_HEADER = array.array("B", [0] * 12) + + +def compress_depth_image(frame: np.ndarray): + """ + Converts a F32 depth map in meters to a U16 map in millimeters + """ + normalized_array = (frame * 1000).astype(np.uint16) + + _, encoded_image = cv2.imencode(".png", normalized_array) + + ros_image_compressed = CompressedImage() + ros_image_compressed.format = "16uc1; compressedDepth" + ros_image_compressed.data = __COMPRESSED_DEPTH_16UC1_HEADER + array.array( + "B", encoded_image.tobytes() + ) + + return ros_image_compressed + + def create_camera_info( - camera_settings:CameraSettings, frame_id: str, timestamp: TimeMsg + camera_settings: CameraSettings, frame_id: str, timestamp: TimeMsg ): camera_info_msg = CameraInfo() camera_info_msg.header = Header() @@ -1474,7 +1500,6 @@ def create_camera_info( camera_info_msg.height = camera_settings.height camera_info_msg.distortion_model = "plumb_bob" - camera_info_msg.d = camera_settings.get_distortion_params_d() camera_info_msg.k = camera_settings.get_intrinsic_params_k() camera_info_msg.p = camera_settings.get_projection_matrix_p() @@ -1506,6 +1531,7 @@ def get_camera_topic_name(camera: StretchCameras): raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + @cache def get_camera_info_topic_name(camera: StretchCameras): """ @@ -1524,6 +1550,7 @@ def get_camera_info_topic_name(camera: StretchCameras): raise NotImplementedError(f"Camera {camera} topic mapping is not implemented") + @cache def get_camera_pointcloud_topic_name(camera: StretchCameras): """ From 381b37bbf5c2794fdd62c8dbc061b5e09e9d54b3 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Fri, 25 Apr 2025 13:44:24 -0700 Subject: [PATCH 11/14] Simplified install instructions for Stretch Web Teleop. Added the map for the default mujoco open world scene --- stretch_simulation/README.md | 80 +++++++++--------- .../ubuntu2204_ament_ws_files.zip | Bin 395180 -> 361237 bytes 2 files changed, 41 insertions(+), 39 deletions(-) diff --git a/stretch_simulation/README.md b/stretch_simulation/README.md index 30a87eec..be534490 100644 --- a/stretch_simulation/README.md +++ b/stretch_simulation/README.md @@ -67,7 +67,7 @@ ros2 param set /local_costmap/local_costmap inflation_layer.inflation_radius 0. #### Pre-mapped scene -There is a map included in the [ubuntu2204_ament_ws_files.zip](ubuntu2204_ament_ws_files.zip) file that you can use with navigation out of the box. +There are maps included in the [ubuntu2204_ament_ws_files.zip](ubuntu2204_ament_ws_files.zip) file that you can use with navigation out of the box. If you set up your environment using the [Getting Started](#getting-started) section below, you should already have this map in your environment. @@ -90,45 +90,9 @@ ros2 param set /local_costmap/local_costmap inflation_layer.inflation_radius 0. You can use Stretch Web Teleop with the Stretch Simulation environment! -First you should install `NodeJS` and `npm` if you don't already have them: -```shell -curl -sL https://deb.nodesource.com/setup_22.x | sudo -E bash - -sudo apt install -y nodejs -``` - -Install dependencies for Web Teleop: -```shell -cd ~/ament_ws/src/stretch_web_teleop -npm install --legacy-peer-deps -npx install playwright ║ -sudo npx playwright install-deps -openssl req -new -x509 -nodes -out certificates/server.crt -keyout certificates/server.key - -sudo add-apt-repository ppa:sweptlaser/python3-pcl -sudo apt update -sudo apt install python3-pcl -``` -Some more steps to get IK for gripper working: -```shell -cd stretch_description/urdf -cp ./stretch_uncalibrated.urdf stretch.urdf -sudo apt install rpl -./export_urdf.sh # It's okay if it fails on calibrated params -mkdir -p $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf -cp -r ./exported_urdf/* $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf +Before you start, install the dependencies for Stretch Web Teleop by following these [instructions](#setting-up-stretch-web-teleop). -# Install Pinocchio for IK (https://stack-of-tasks.github.io/pinocchio/download.html) -sudo apt install -qqy lsb-release curl -sudo mkdir -p /etc/apt/keyrings -curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc \ - | sudo tee /etc/apt/keyrings/robotpkg.asc - echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" \ - | sudo tee /etc/apt/sources.list.d/robotpkg.list -sudo apt update -sudo apt install -qqy robotpkg-py3*-pinocchio -echo "export PATH=/opt/openrobots/bin:$PATH;export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH;export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH;export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH;export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH" >> ~/.bashrc -``` Use the following commands to start Stretch Mujoco with Web Teleop: ```shell @@ -245,6 +209,12 @@ source /opt/ros/humble/setup.bash If you are not running this package on a robot NUC (which is _not_ [recommended](#system-requirements)), you will need to set up a ROS2 environment similar to the environment that ships with Stretch. +First you should install `NodeJS>=21.x` and `npm` if you don't already have them: +```shell +curl -sL https://deb.nodesource.com/setup_22.x | sudo -E bash - +sudo apt install -y nodejs +``` + Please run these commands to install the environment. This will delete the existing `~/ament_ws` directory, so please proceed with caution. ```sh @@ -257,7 +227,7 @@ unzip ubuntu2204_ament_ws_files.zip sudo cp -r ./ubuntu2204_ament_ws_files/etc/* /etc/ cp -r ./ubuntu2204_ament_ws_files/stretch_user ~/ -bash ./ubuntu2204_ament_ws_files/env_install.sh +bash ./ubuntu2204_ament_ws_files/stretch_create_ament_workspace.sh rm -rf ./ubuntu2204_ament_ws_files @@ -270,6 +240,38 @@ colcon build source ./install/setup.bash ``` +### Setting up Stretch Web Teleop + +Make sure you've already completed everything under [Setting up `ament_ws`](#setting-up-ament_ws) above. + +Then run the following: + +```shell +cd ~/ament_ws/src/stretch_web_teleop + +# Install both npm and pip dependencies: +npm install --legacy-peer-deps +pip install -r ./requirements.txt + +# Install Playwright: +npx install playwright +sudo npx playwright install-deps + +# Create a certificate for SSL/HTTPS: +openssl req -new -x509 -nodes -out certificates/server.crt -keyout certificates/server.key +``` + +Some more steps to get IK for gripper working: +```shell +cd stretch_description/urdf +cp ./stretch_uncalibrated.urdf stretch.urdf + +sudo apt install rpl +./export_urdf.sh # It's okay if it fails on calibrated params + +mkdir -p $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf +cp -r ./exported_urdf/* $HELLO_FLEET_PATH/$HELLO_FLEET_ID/exported_urdf +``` ### Setting up URDF diff --git a/stretch_simulation/ubuntu2204_ament_ws_files.zip b/stretch_simulation/ubuntu2204_ament_ws_files.zip index f5bdc012923214e345c8bbf3618e44068999a4ea..2cc490c3f77985de14ad79923a6068240afb6e0f 100644 GIT binary patch delta 5965 zcmb7I3tUav8b4?6vrey5oyeopyOEU2$wZWk5+M{tDN55zk-YM_(kbK>C0ka>ctyF! zqa&sm#v_9fBPIz^9!*{~MGqz1wf528`PI&%-98$YRLvtF4Y<25tT|F1MC#u^xV6k%_8h%oLpy73cX zjVLF6q~@jf3UJXL{w+Q;qLz?*43Jn57RQ&a7LVseH4E*i(Ik+?CB(*u$A`u*j9d`9 zJbv-~h|r~B-_4J7To@fU-Cvidfi+-a{7hJ(!{ZBHkvty#M@L#WSney8*A5;$q^@(6 z_P9#}`hvVUhULAbwM5?Q4XK@$T0CEG4b8mH+6&fSH7nfK86;YXHj{no3 zm0@x7|H-1s>1EoNrQ)&e9h>>ZLr-~Vw*`25NNn^c47Qw7e%s}jt+_vy=>N9$!HQ~& zcAJ@Z{pBxRPPdfLZ(Db=(_ZrPfP{4aq0_VLj`F7LyBKx2YTs4;{_YN)!=61lb-m0e z$@Oe|-cLQht+U-9URZG>Fq72NeBeK|+}KP%%Du8pvS?rF==J1}c26bG_7=^$KL|VS z>N_ka4LMju2?RmRjub@+W)jQ@Z#7jd4B<{#Q_&g`%tUegdm@^kf;B|cIBgbJbX0r4 zfC0hwP9mTBsK1z*DVB5*JrrV8k2%aV^|41SGbm2~LUi$|nqDiIsiM%{_c+!in|r{$ zo_sNtZp3mF9|5E`vE3w`kwRYBQaem$u@lMWGpH%10qg`uRGP9R6hgqV=fprbx(XOj z;`s_Y+lR`Fw0zvSat&Iky^6RMV44o#g;o5M6tCV*|3coU6jvE6*U$`Z(~Nt z5ZcE)W~O-JSpJIb?C{2Y{xlX}4E(_W0#1r0IA$QbV0|pXiAZV#$YHh`hOr%_=fS>l=} z7h89IO8Whcu7>pO#VZL{YxUfo6#P;3p9^NItUQhwb%h7}9kDVReZAFwN_OPTftjv} z9|{eI&3j!kpyWwabI|M$RZD+*-n%5-#c1_FyN^~S73B$?pZ3?ha!aqz*cH;gzxlLV z*W;D)tet_G*KhZtAkp%0-0J$;LrFoW3)^MOpa0O2c*bl0It^!e!J=iq&5^zB{PZ-h zX8GrZo%-Y+r;?bWn31CjoIA_j=xGGLagT3$Ir3auy%BsH;`F_ax4q?WJMt$G@-^GE zekJTj=d(&%>R_?x-0pvW#OIR;Kh+%D_tjN&mOm+ zU;E@4|91HGaC*`3@%>8{jat{RGg*4I))_kajXhD|vx4xvxXiTJM`w4#+PD(G`em2v zZT6<-zqZ+%@4wqQx?n`arX$Dl5^@~Z+yaJofJrV0>z5lRw2jGJyX(dny%e5R^ak>neAT!6!j2T@rcN96EKPL1uOw>x<34|St!r~p{JdWgKj;C5s3!CXJ_nJeDyra!In$0aw`4ibW8{fz0!yQ)O*gh29(~oL=kLu?l3MD3tSOvKsEjzVW@%YOsh62! zXx!P8XYM5&w6n^wYP^@8d~M}{CpSzF=ypz@l5ivULvOQVjvYf9vt=#G##2{TE&j6B ztN*_|jaq)#9ed3XZn?XtsehJ^OH;s^Ax6b+@lSmmOCLnSpm+XrYa&{0D^DzqJgKpy z@?G|!t{ivyytELnb1m6VM+E1FG;Z2EBza&#wUM((YphqAwQ---_JZ>PI{lY=+#(Kd z>0!_>Yjb%?qiEIRu8@^-hb=NnmUi4UeBSAv{tM$r%`P1l@aF2T{*Jf4(<^aKB=_X_ z=Zi~nb)zTts$6UPV$?5$V@6_i$Its*A0}Mt>lV1{-sg>hO%XT!vj;9-JJU0J)!euI z@F(&6Zbdn5_bF}DaRTG_iw4~k*+^HvGuiNAmF|phGdrv+#;i79VtOIs!iV7PvdZoG z6VA`rT2NN8Jayo@Ee?tAz3ldUaT#w5%MQHJ4|CLcA>H_Jy}$plsDR@S|Evif_2sNQ zzRKqPz^Jl1s)zHOott^DCZB)dV*Jp1`(xuZHJ<@)v$j{4#tsnhc(~a3dh-cGl4-1& zt{1nN5|~^8lpAg6bPpKA`xRgs(`(aHBnR!?82p!hzp$DOYI|I>P7HO3a0nCU4I-FPl>$|V8Jybo+Kx5M9YtL>s` za`?Uym=Vu_f@6}P2kR6aBe$18EFQ*D*x>G_iNRyk0I4RS0P0i$Z9-0CT;ZHA1c2LG z01)|va6ZQF4^cqZR{;rOuZ78Ipb(n30*OdJLKSH900DJG3=mLh+HBVdX|l^0BNsCm zAvAjkj9D8IXgG1xfL*61i!GFv%^~>+aReH&7#NCXSWwC%#NpbDS+`PS>zHc9QP_vU z07}~1hNYlw4S5Dc@Iy7wBMwMdAk66idX&_N)7*zWMhg9m8H^BCKjs?5><}e35kQA0 zK%0LD@@s(`Rc@kZK!0=p8g^Ry`6o#GbTgV6fjN<2IAKr?!swgB%okuBZm9ItxFPoA zq{gWf$fAYtM3<^REcdKGfk@EcKHWmWzE_*nQj;OHskTp%8IQgKWluqG)(n>KItD>Y znvE*75L1p8oW`p=>Jq}%svBbUR~j>-nO^q6eVe+BJ@_1LZsIqXVpdj$&{56)C%ixq zgVPwoo<$TXN_!Jn2lU9Iv3z*xIhX>agivM6@6>_eaXTzwxGmwNcLXO{8FS#s7L?TaTx54S z<-m~{!-;-NIdlj_mr^q}J7`-YP<17Q%axJ1miAIQXh0fFv?1_`Rmxzxs45>z6zDvR zzLltgMkUrn4fTX}1fC^R0pnWRTMhaZM!P2~qx-A4mhl0B&aP1g;nMpJ6n6klMWuZU z1ws+@Le-Wyk*Xqsi!djLX!zX@L>|6g8A+F76?@s8OX@`AFE+9wY(f5O4qUpI zDrMd#B~g_qvU%|EZLzTf3!v*v)5*g|`GkJ;EfN ziNsKqvo|(0%TOpGjr91rPO z#1Lrt2RT{}!PVPQ4Z{dhC?WfM3bLGpvmvVDL)BpV2oUB}g#f0sf)Tii+p%^kJ5A2M zFMwfPU_2Z&kI=#;T)8-yLu@hmhJ#Y~1`CA>*Bc0N2R#w}JMDm;(*YX&7ucSjs{jB1 delta 39232 zcmZsC18`;CwsmZylXPs`PRF)w+ul*fHafO#TOD`oq+_$={Qccm@7`Dcd%NnKv*w&* zj9}c5Evl;T$dqX35ZMxOvd*XYv123e*JwV)Zio-i^BX5 z6I{wOHeo^-;q-5mF8bZ#pU7O2HUIzeH0FR#f&SY{askgtI){{lS!{mz z7j5G=C@nCU?f*=Y)Cr>v^AFuWll~W-0RcjiFs$i+a%y1b!2V-}A>>2++e%~h1uIF? z!$nBiLpA@mw^0$z|34XNn*f-7n!h+GLXslp{{Z!1y8Z{$$b-H3A1f^&8caCN!43>1 z$qLu-KfH6ec3}T&wYL9`|C1)3WI%+HL`|sizi=o?^@RW7`fse+|ABiT(*Hj=>xqs2 z53C4@_5Xo|C;h*wnn;@bZ)~eU9N0yCTDREWvQRc>{>T4L1b!d)kKg6rzWEHfVg9h-qgnx0QfvtjO{U=bo3aT9cAM?`+Eb)IA4kc|$3{)~raRmY^?b;ObfBdTd zU4Ey3Q=~DkLexY5vsvYVkhDB|ux|*Mrq>q#y_5d6b5Ns$LbU&Z{Bs?^gM*l*^}B%K zC&VLy+d5dJ%_>3?0W3@`>`ZJ-Y%DD7Tn5f&rdG~oCaz|t46dH8YO07J(A-wn7XQ6j z-8|tzKp{@RK|ucXcHX^SbHLPhGDmiKKJ^15jkYm16T!)pM@y$3B_9pyk?U+Gf)p zb0@wDM4q4C#-8jx%Hv6tztpXYHinfu1IRD#rTIIRNA8nH=kWQ)#~=EjOKKuiXJLF1 z#=sB^T;zG`RiZF&$*@}RmxPv?#-tXhnodNaH6}}3eY=>&rJKR+QjLon?`{l2&MZD} zeL|2oJ0IFa-?Z%7@#hXG?TW`)iW!d5vNj4YSSDKi`_c1c+q>Uh?_aZHJp$TeB7f`G zG1T_e-F4?3v|gM!YmPI6!bMGer2f{BKLI}BfnIAhJ?wcduZ(qYyDGs|gnPz)&3(mB zljPCaTS1NXuD1Tsg?p$aI(H_{KmEJh3@;ngz6^=DM!PS&F2BIh z9+j)z-MFt2BrUZrbIoMt28`X$exzC)9i|Z$a9IJ*C#C9fxeG4y-!43PCA{tyV1e+M z^@nM#R=XVhxMUUEhmkdgHGYaVc}^-CjSX2J>lH%do~p31Cx-aXbL(;q`u zwm1#kqVToP$0GH#&&MPAwa+Ib{Q$Za1T80_sE8siCd+zqT8bu3aZST{V-0oNIb$3n z(1T3Ou}JD=+kXx1CjJ>x8ztMc*h+#;u5SI_k+0eYcDAjY=~kG(wmcFhlG z8^7qW3*K&SF8yKWkfGsL(?!EB;e+5di$%9nWaitJPg|~EZ1ws|m{`|V%%o|uhC$S} zZ;2uAch=WHKgbkhNI3kOeUuS+22q1U$-8~q;>OM={Y<_ngT}?o5(|p4z~NsT6dyLvD5+Y> zAD&+-TcJ~|NHkWpO|_ehRI1lPO2C9}T1ms;Eo-M+PQWbWHuOrZ!kz-1lw&|gy zEzyN9@*`cg6&X<-Egohm;TdDkl6)1fSuKe!M{=HO0Ze7x5Vh${Dx!27?N$Ri#@N>r zm{YJOnLHB8TWI&&Z5E8Ft&_`hqK_ENDj4_MWiq}>zLv-$7NwJ_;ERm7AZMJS;LPv~ zN15$k5HI7Y{PnaSYO>_`jIocVK3>ck6JF6;hX=qY5o=s!qyGudMOc$
  • 24wo8q9L6-)k%-%8X-f zrvLb9^-O-jI~OY_cz>&*G@*G2(pC6Zs&ctxHxXw%bBg!39O8fceC4V|`tcI~O0tkn zpV+{o`l_Pq{Y`Gur&|wTReTyH4(nV!jp$bD%iyZ(2@MaF#bDf7i9 z6VfLW^=8GY8*L5BGvbvuFhMS(bJI zwo?*D7GDtsb%`#knaZ|NAhp#t5^|(eq#nUTB8Cs&DLJ)yAoc2-ho+(QQmf5%TS`&7 zvmDJ{S-BLR);93#7nw4#gq$2niu2!LtSyeBN|NLO9y@yktq#;bL8-sNy@{8S5{iTg z9&2l~^18wjiW}3(fXqx$n#;c|zN;*Ge}iy*e}e{VoClezzm@)Sm=UnEOA-ASNBLh4 zw6RoBc6Pb)mBNw|E6LXutNJE~urGkjtGiYGKeH|*NAg=+XNtBUl$Bda5C6@UBRR{; z%q&qoSNiW@$^QVR{?Bof!vy=kn5ofyex`;LFb~VA+;wo4_oq{)tv{xJiIXBgf)f7x z=@HI2cj@9PdHm=Y2-(|FECFbvQDSXW3S8;4*GZ$-qeelM9Qmc@l0-)s?F#Z!tO@WE zG+I>MmTrH@z;@2&EbJ?-8^dq#@=@H3LL;`axwRCQ?)RAG;6JE1Q4t_qxO4wj`Y}Yv z7F6HAZ?kzFY+i@GOYI|ViM>Dqx~a#ATTS?pOvaYzE?HxUk8*KW+XUFwialR4`$|dK7Wx>9&{0UWk^jx0!@FDt=ruz~#xWULqpE8)V65hw1BsX;O&J0sO*~ z=ts~62dMB1HJpgF(t#WGM71L9FVPGd@9uK_@Vo6Wm?DJ!VMx$S(JpXxWRPn$Pc|yP zch^{sBAQiy5-Am|+-VK@?2m%{L*31K(JGb<_^T$Yz*Tq#=zS4Oq8`;gEsn0pAcx{p zCD+sEIZB#h$n8v|)>q~^TAE@=?e-Z!Z)z{|NOGRWwCN(1>5290R~6{G579mj%VU`?{byiD7gSTDv# z(*kC%G7Ko$L20plu2J3TF;e^(`1ALKFJ}0`JmW6)jgz;=%bSbL3$%a}nz(24L`QRg z*2Rvyqn|9k8F=*SGzj)QjKbk+_ko~(%Q}Dd{kZ}%>(J5ep8vTyOmfg$di>@sJJX7V z*7IZ!W9#M5SMcYohW{e2_XVo2@AGZ_^W{o#^3w_3JJ8y-T;nyL1S;)!erAOR!xPNAj zIjpI6LWm^g4=Ya0K}(R^(VJYskRWy-VuC-7GfKVEaWG^7{ZeOr+{=|g$KuX{xq3rA zE+fzncn7khk39|1xDecQn0P`|g-CteDNsd@mJErLg1*=OkRgh#U48Dns%mcMSldfU zJ!vX4=m|BK>`Fv-K1@d%E9dOP`v(ox^xTI*qpy&rkbZx-z z1uXdFycbdc^eb-c-@CuY`y&tV{OO*nb512#_$_WrH7?z)oa zhQ--oZyi*lXTWd&TA(Iz;yvn#PCzznqGAj#I0{KTtsq#Wk#~M42?Q_a*&j&t$V^1YQZ`~PWJ@7xlY#=Y+j4%D-pU4OP&@8>>N4oiZ0#zR zQrb)T?I?NIf5VS@Q?#>syY-fn$7-2zKExFz8!! z&Ca``S*_(dy0nL=FcR{m)VE_kAEw(s8yVXKA2HoK!}-oOtT#*Ui^_oP^6~DMx;`2sI9 zt97Sx$QdA_z0)Ky1F!U)f>Ov6w0`y+OX3nlpJ2)3)6iO}r-j!WIH5P7GG9$d*^>*t zO)`GIl?uL{{2^=>yz{|kIDx#W2gOO>~E1(+^7vt}Ta+zEc@ndSQGVnwez`xa$+|R4u z6Ceb-Z11#igFKs;Nx088ok~*>`uGsJRwKAONiip!e!fhHkv{{$I7f=*?6s31qOdp= z()C(=N!7lJ_g6Y}JBp(PzhIHJvG83s!pr^$UI9Oek=)qA$UbJVusH*G!ZB4k|6Key zy}k)1gH`^JMZlS6^#l)pxL|qHwRAc=-%^2*?Y^Nq*=*`$6`^U{pF6#j-_UzhEDOd? z8Tqrh+%6~%IXQU#XSg(k=O@ys&xRJelZ1Jli&fEc&l6Ly4@#DJ&Sd9t}Z;{ zd>93y-=Wk(_cMdAXkdT2Zq5Nm@-Jxua-=^bl?W?V!O|Dh812(!QVl={p0$skY6mxIu;RINa z0c#}7r>_kMwDu{Z-XUjQp<>_3{TJs9Km|`T%|;7e#;W&EIcXuod~i>kLUBiJbI#T+ zv+&$_-qHEE6#ZxR_|-807VNh{%&RR>V+=X zrtUXe_A>%-$JAzmkg30&w9r3#=Dk(o7Q1BvJYcWM$%WAD8s9cjVDM3yWB?Knj0{#3M=xFxtJo65aaIZ#nNUBnU+A-X7wE8-ACPB#+1r6s zo<812Vbq$8TU|y+x1D$xV64_rk7bau115sxX>SQT54btf3X$rZ1eI*ujD4=0U>^> zE@{wH^>Gf!CyOi(BDS`E7QKrRT(&K4MGHt zQ6&OJal9)`+d?ag*A`LW{XD0Mh7BqfU2)t4LIo=2Ov6tlH}omFLJBCqWKyCr5$v34 zY+~FEXD9t87sm3w@Bx*n{H9fWDB}rQ0kTdnKk$wh76euU4UprxHOQK5Wkxvp`o2s5 z!r)0SeDR5v9N|Qk3_!A#NjEvP#LY{CFh}9I{u9)!7M$BT&3ovcB$2Z+y3xGVyko`q zMH@u7mAJF3*^oI#|CS$o;73r8*1=$O&1-t+xz9$-yihRlCkMbFDVO3}Eq2Ic@jury~w*_DbGGgdvNL%lUh3<9Ij`Pr6?ZC5!c{^3j&fIH_#aU|@?4 z(p3GE58vjze=#tq$N+5&2UW%Hp72LYjD9-{76bg}#018G5p#%=R>uZyYmQ%Nk1t1D zgcdx5%{^M$y>N=-qL>UM`Y0AmX64a3bC`3HZ!%TNj`J`#r}xEVlLpQJ)hKmdbR(po z%gGCtF;O#{d-`_>m5tk~HM*9gub)%XPb6ra%zC}%S>1r@Nh6Uwwbd-`n5sAoXl(Gml3 zSI3@OB`?rEBInN>`;xGURMLky$m8}NFQI$T8tImAVhv))hB{*|ss6pNobxP{&yjYt z0rOJt1`3_|T-;4fQ`@`MzbqW2(n5$dwM#i5Lx5Lv$kWq$#aoS=46qP0%ah>d7TsZD ztvH@_W&7UN2qz=qu!J91fxV(1LLFs*%=&AwastT5K2&E_1&Kv(x97lDA_fzPt}f-& z37JA^K|7I`@MKL~@#eWy6Y|4$F~B-g=WRQ)S*#fSLu{vMQ|Q7E@U`lgzY&$s_*Z$WBq+ zy*RKK(YTAW5OCp-pa4;^u6a*nT4ulme$Q^He|c?d6O`Hd$TI4G1GA}LmvtEN2_alQ5gHWsC@rX+e#DWi{%Q_ z>%8wed^RImzlEn}gmb+4V$__#6H-wJ+F9{+YDi$aL*e`|QV%Ec zuWzRFbC5-Kr9huObJ~n{GeF%)&xTf=)7C*1**X2fsdnwc+*sG%w9>JLU0sWpj zK?!ARHW{Y%$OXPOZquq0geOI+jXI#IRjYRHs}+N?R;wsrc9!4ZM=N>k#9~yE$cWoM zx^>`iT=B^LvKOj+dCD_FhV~{mG;y96Mro;HF{^Z4L}^G;WdO#-Dp~o zSd`7ai_ghzn2E@_TDp<2OWTZPp6UTB+0BMLDw4IHyIp@y=OZ6gPm*z$^{BmvX5%S^ z)nCgvw@+XHE`fAc)F#drv%lI6)GqPNez1lo{2AaZUBAgZ^-LJ}s*Rs` z_QD;M(rtARBJ`}%0mw7~uY{i}g=rE^efJtyM~4A8tL2O?-+0lC9FZHrwedNRRW*i^ z)?6G#4I9X1N{=#BK}s$*UB^xlrK8T=&dq%-L{+nPH`PkQV;bme`FRFNxFt0Bd|#D1 z=l&p^R8ug0a-?Zc5OVXWPNebq44j#-yES2c?&JQni?&z91ZEEY6tLSK-;WbYp>*>W z7lj&RW$dSyakzBwUB!HhG!abBS^zD-?&}QgS9Hzi)hLeDQ*#@-G_)v~;IMfON!zd@ z%q)ha8Z})2X*L3RJ*2sc{3zvDIO;i~wh0>uGlNwzfi}aH)m>WKwcDOfVHYgMa>s{G z`r(vQT;CIR3(#looRc2^l7%Ktzu9hm(;6*Oo}5F9!!hicT!7yqa2z8=$?e?3z@Oi= z&^>x%Oh>1Z?0204iOIg6^K?GO zD*BlbC1B|mOtSMyy@>NB zz0Z0^`JO-f&72Ei2zmY9aAbVCk1#im)17(zQdPA|a71G5GFf*aNEsO=kX&q_9TB`5 zy|XH|7dRE{A2zngC*1*6$YccOYZy9@jjP@>SD!hiEP-L>Zb#q9$vEy;ckr9BtBn0@ znA{bX-yxgwKDzK2yX!cqQo1dk0v9(Wbepumuk8~^_NHjie2+IX?rJ9ib0)DFi}$b~ zFI@#gMSTW|yr^vl5+dDcJ6`$1 zK^(XRd_j{GPU+m%SvG~tzT;)QqM;f~phSVVwFmSFnTR`+35gp9eh8-j4;J5pe~H7? zpWq;en5$(Ho(RJE1vzM1bjy&_n_ceWt2kIp$AT=avq>D;;H6TT)SSRqaNRh)=sA@A)qnW2Bj^ePl(*z4s^sDxQM^l`g`RzcLj45~aR9h)z zRulP{{0;LaW#{r{LJwgu*3X9kp_OBP;DJq0kD>hQehxFoP3}8RDj3}hLEf7WavdM{ zM(nZRX_`*Xjv!UhFPPt}DKsA4p0Wxw>H$7aDxbGS!(!eYcp13hv4Db);ul`X%X!eS zxMdi|y$Q@}CplLEf!)RClZWP@&4p`fAJ9+mV&T$bZtr3n4U0#41s7e2zK)SKV9I^D z3WGo9sxOBk!29CN(2UHhi0(YWjN21wR&9n7Pl7K}oKfYRIyOmV30YP_p9F7g%Y5|M zy`;AGKDqgXzSqLZ{q%KO_S9cBpBS;H zrDjJS&cM-qxb`TugSI#5dsYh>Agl(l^fh{lfk!5@MN12F_N0y1yj9buouK?=9*EGl zQ%FmIi~nHLPSE?a_E)p$8jyHA$?U?@FNedTvwttP;=k+M=$odImqKk1VupwEpR|r{ z4T~LS`0C&(l56-ikmN%*Lqbs?g%ln@xGIPf|kq{6B3}S^jD5mWj=-s5`xRQpz z6Gx4>L7QL-9accf70)a$G1Mu6OrC!`0!DUUzn3Kn(RS0YmLsK>B!6|jlqD}xH;AfX zpR<)_C=QelGJ-Bx_p%`4*$)X;sFzk-^fHejcX)^udu2wMMQHp-L>wbsMpex}< zdc-x7^udi$F#2`=T||Xz?ns6m22blx>sx8OJZEy zQuX3(spTy`I)-V9YSwjU7wI`TFBs1(63P1>ip~`iD75nfzG=PF=04RmT@zxYzGrC# ztuy1!Ro-Hu=6nryZ++=o}uw1eE12iMq?wcynCt!d?*JYq?>PKg)N^KsUOHi6qx!4E9`*cpqsdjFei~iDZr$65ixz|(Bc0=JZci+4;ArjI%+s~S zO`J}zX+IoAqBDwgeTDyPsxg-eq?^OGRA@^}xoUWu%^>-gP)^%jNYGdLy{7gX_KE(+ z;Zl}!-iQS$?DU`r!)>W_q|4tiLQ~e=^~T#$L((1yVIe&-g~5N(Nkw+QXUi!WiEAQy zL=;D%vw2}xrjd??%I1fF7#!c?lGsI_$pgAeR(1aUtQEOVv>xnG1%wyfpuS*USaqi) zk0FfVr;zLU?=F-XJlg9}P~M~8F0y$&63aH_&Wv=K)GhXtpfFiH5-YO(u2G#tUgUF` z1V;oAh`mY_Bwg*|3E(>b7D%as4Ka81XE1QM zr3QqTm9n$c(6~ljWsa6QnVE%Q)<`rQDP7{8z1W8;a`XT*P<5p4+Vv{bj_;v){hQpK z{ld;h4+8O!HS~9Tb4SkK--$d93me@=j8~yyT`SKAn!o?a+Ti|h$|i3cFZ_jVu5bT0oGY5edW zM_*sMU=NK8zb=aNVbn-3okpZ{$=dk{w}M?~y>#YYkQ7oLIr{dRFR=@`_fwm}@ ziL1C!IM9B(t<8`0>*JF0kMWN^9chjp?N_L8reRKFi<|Db8t)K78 z%7E*24eUS-KvrLQ&MY!3wY35LIT1`XQ6tNId$j%SBO)SU&?M&T3MPT_y@ep;7MHe4 zXQAtUEX)<+iwhx`ZSIkk2intK@O2~M>tR^Xue(7Ewl=||1jXH-O5Il!=fYYghP^NF z1n*lh=|QR24fTSrppLsk$sN2BKh?7C5b7M4r1@oyfudOhd&`LO0q_opyLzU-qQ;j$ z_z3jOSw4<#Is|T~QZd4Hx;<=3NtN$E1>k2|F5R7P`C+#ZT&70w%nV7-9zBoft z&~esv;B0_vYCe@Qg~xdBK~Kh1^|)RWo9Eec(g)&9%2m<4>aB88g!ctq2j_X~aku5# zsbw2s1N9V4AjhysijIQdQTS=tfg!8@pMQff!_!&_tM56xg|2nJ*X`1HhXMtX6r z%}WVaXhO!8@A9cbeVJi_m0VQ%9urGLnc7r6WBrgV8fRSX)+=Bn=cs5Vm$VlL)bZzdJh^HSoBA z2BDnf(zC^?`=MmF2GQguq)TM3msle9apimx*Of%t5YK@PGI%^5_biB4*x2n~NgZ^akK9r;}O6$4YUS@^>T}yWODFyZIza&yjo(I%cEj_gdia19n5>V5_ zTjQhqhpcW*tKxB{l3tdQlx$H`^U9EAhv$$nQ(7P(AQD=o%U9&P*!D-Wj82rm-dVEIm=ncbYVH;TeMMbs zCBIBU3`}RBL0_AEoiJlCjZXz8ecV+y*TBjf(Qb5>ACk{8J^KBYE5V?KGU+O{vllf$ z7PEKdtunIeIKDZoyW;WR!0hYZh2>5k9cl8heHdA3@4?pJWc;CD;cta7;{2lg`NX?n+xLO*MaqlxI-mJ5UPQ9hBcuyu0fGn-d&S zd4^?ptNp!&#Si5Wcv~!aGQVG7^7}(x)$6U9^2cERhK8>>P{Yc`-~n(4KID~!5Ue7a zVyNv*q}Mg)IZ~Tq=s{ zJBo~{K!XD{H(X)lC9U)VlqGsrIjJq>gzLIF%0*<9Ydj2$v=kt;zVK~SC;=qy)hZU! zc=X+BytcUtDsjGkEq06cT<~WvC|PKBIu07hXLt72(cev+D?6q{O>rZdrDgHEl{k~h zcgExGORj57r%Lw~?u7r|No)ujjI?1f_e#$)j&eKqWS%2=V*YMoX)T3&ZSEvf7@~asx)PP}RZddN1~+|=FPLtW9)q3o z%D932Y6Wgl@kg1|PWB!F&ZBw?tq*G__N23J5AKCl*inoXQxY#7goD}@EcKUF5lSA; zy%GKMH{yBnYLvBG9Cd13QTTL?O_N6 zf3|3fz+<-uFAQH)FwKE!m^ZoZ3Ve5TpXBr?za99*ixJk9X%`3a9l2&JEr4JjH+$3J zG#QqkG(7LL(V8hP(%i#?v@nCBs3_>UAm+tL!8~$}&EA~6+Iww=X@Tj&a)?QBSekZ* zb>!6kk~0dhL`y=msW8^+u1ko`%VDJBwO+Jj#_#3%V+8?}i&yJ}RP#}j_gut2MUA%c4=Q;)TEJNO~>* z#bRR%aVuMyifFDhdwOYan%r(lT9(JtQY9D_Zf*g16b4eX#SjIYYG`|Xc@*aK5Xml` zj1kJIP}BFz(MRj-AOxL)=t2!?^KM<8FOv`<-cUL~qK2qk>PU``h)j$4{N9mvcpbCD ztJc}%Jht%mOS=Lps#b3SiZub3n;A^cmWe>h>H3INyOW&N{vjXLZDS?nZbQr#XPRFDI~LgVWKnmWK205ZSK;H9W*_LSs!))$Axz zE0c7=A;@&MKb8iy6G$s&>y=rTXkDllnUxpoEW`&AR1ES&n2Y*S!fz4e?}IyQ>CDpg5? zsh~I2gxWlSILtm4=iYN;Lb7@0m(&;jaXOW_D;X*U8UlT4q-JDKH|r!1w7`@hHXdP_ zt_9yOw|u->5$)xir_&-mvcWybhr$hwXl^nN>N)8*eT_0$6nrq_^Q4EcYL~)Eiiv<$ z^+?Yqd2v4SPf=%2f|f)uiKAxWoO1d}9IB)ajjk7_)mUg3dKztHrO*$r-L02?|h!9)*s=s}CmiH2JvH`t75ctO=w(omg`{B} z0Z-?6wB?BR>KBR+4I5my*C`z-a6bWG?-dzWnn+!dt6ynzqT5xtV(0*1|3Qahh?e-iU0dDd z)A+Fk=aj3L7|I6O1+^d@Ns6zS(j*s12`i$~Bz}C3d=d9AeAIy$E5cxBoX|juDGxr& zS?H}))C@)}rMl;bqJSIf@4a%J9>+4W)r*WCGm+<~y~1#n632&?0^elR-@b{s$TT#t zkFHv*)b00>BTBzHw%wA^WI(4WTV7oY6Zb+_7wm8=2z>Fx+-6h~X)$NTSUY_gl%<^W z{%|HDGW+N`pGVi|S0y7dfwuyN0ndfWG+(WQXP(8kZcO09?z$JknIbW{=kQI#YCX|XLpn*Xb+pMrP= zGRv>1$A!_9fosB-n;?hKz&7zhG>AKYz2Tp`VY}s&J}@O|Gc&!7@<{fN*Hz7}oabt&%IPb zvpl;E%|C74t`FfLK-DiNVjyYr*A3fdPT{eCV~V-EG%vN~MWiL>XLvt^qkWVLIA2Nw!*eSolw+)lGPi{(Xw zO9Rf96!f{6)iA*|r5v;58>=nl?lD=jswvOYW6Ym(zXZSPe2aQ6e9K#FNXtFDIdj&e zaoP?0nznh=p3>n^k;b>D{EeIv&kq8^Z6NpeO9E;u{uVQESKG*`&o5R()@yUPh?p|6 z1z|?i6UBq1T64WkrJL@+eME9b@_4%P6e4X>JW)FMJ{4r)4%N0q1 zN}ha|$q)oMzA1n98RSn<4SuMy6xm!w$Q2tW^(b=(!tQ~mD`!XrGLUsi-iXm<;(W>8 z4_uvPe^{G3$dIF`-?%}r98~+wNxbtc3ldWWr;l0)pcW6V-8azPfKnqA_hSuSk8@3h zE3PbRu`tO%KVV#lTZPn&Q*fxk^}sf2g_Nf{MOoa{4a!H`$J>V_5n!YLXl9P(2GNzI zyLXJa-}`z{IMQKr=LTAa%_M+Rg4~dZ8FgoT?bNU5?T60^+OcF%+RZ|18H9HRqJ9KT zk7C*cbRHe2Tsj^5%x0`MW+E5aAiJ+sQVd~uZGl$(K$d~vK~4*T%wO{(F0zA3jrpAu zXBN}?UB(hF47#75YtJ!S{)K%qcKuvT%3?4GUFSOmRi0r)If?KfJq34L@i(uc^AetC zA<1Q?{p#P~)=AXk4tPI|MNzlFWK2D##EbG!fcd(CA@zo65=C}W6FL$$D`8Yf&fIBl z9p!DYQnI*EYWo$#_Y0BE?m;ahn?$E^g!cgE3=z=hSYO44KD7oR@ouZHOOja4;40Ew z;2i;uk5rY38=)~HxFPcz*5HHq8Fd5S=2(N_G@~MLa~AxOzuQt&D&n@w&<&_8beIOW z0q2lETzyfX@3S+1=f#=k*C#up-?5KIoD$SILou~Y;wX0Is5#i7EF2f&=}=3y(6cbu zk_@QKMS`R)@HqCsZ`=rIpP({6uTL5x#@sQ8da`KC1WZ~@?a8JHOUgFJsoNIkVSc1@ z)W9f+(Sqs9pRK^8TRqHezyyseW*cfh0{tm>hUE!b)p><1X36R9S3IHW6F2JJ;Y5L@ z1Xtq@WcT-A(Q1jO_X%(@ztP|o3gmQ#LF9?+sJae9M#iNwPGn_U<5bsVnX6JcS&(Ky z%D!rZaOLVovklcAnaK($8(qI zo2nA=5oY&^`kd+r&yv*};HvEBGO8}CDT&7O67|ute1S~zqG|C(O-hQB#V)sKyYg|$ ze-C-)>Z{81g{9O*XomsmJ>Z+8z_I%lKTY{1dy^(F`4qYw9h8#X4S#gy_QUnNYpGK)i61ZP9U zRC3yUk2~6 zP(^um#@Z!>3p23MF79Lf5DHN*)$_>O30BLa4F#bJY$KSqh-^eXvz>Av`e?Py#}#(= zVm;IZg?y_!eudMAoK@>nS9Nm^GdH2TjXggCq*0o3;*bl}$97$c#~@xzspC}_0^@3p zqiP9yyr0~1M3XhnpBY}BfQo-LdztA3;(5x~#;tLCEqMwR_3!vtJ_s1zDTF?J%W8L4 z%cJ-{LL;=Fi-}o!{WMBBq@HAte@Q32c(CN>n}Pa*^@{gL+{{Ezp?A3U+wV5q`d7(p zFOS%j*XuhwtunAQ z^>y|KKyGmL6Tb_&n;fx3Cdg@~PI`GPS!RxgXklyO{LE*9_;l-$b$aeke}-$efe1EI zFQcwnnm+nk2210mRa*4LFPpGr+Z@MtQ4yNho(&xm7K$G_mnMhzXFH3!*gm{swU79@ zXjHja=ng(upmM&zM>YA)J|8$m-mrAFn&mkOsk5j#J$tnHra&c|DkTJt&+zjTt>kOw zOixACfkjLQUkCFkIzc=7`jp>=Mg?KXCM9(N%dS)rM{DTJ3>!i)^GrEMI`09uPrVV_ z%fpnC_}0pO)Cf(^EtF-{@f>5MjK$!_j|=@y?T4|t6~hg{ZC1KJJFX{0ydcb6mo^{6 zCW&;i`GWn#&n-h14_I^%>5%oJsA9G`np%xOUT?OuIKV%^n?8nMOuOs|;8%Utl$VUTZ6)PysAoI4W7vz}*-gliUj zK-v;eY`g?o?S3*Hpf@R>AN{CSrcX%VHHDL~ogdDRRLbyD5U=LHbo;3rxc>w%dXUiB zZb4Ioazv<+kiNX1o06=f!ipp(6*$$wBnfA?$85l@&dSi5zvXPvd@J3&v>oYK52TljDCaSu4&? z8eznOT99`vzs^jysRd)B?bm{IR5tHQ9HO=wqY+YR@Ktl9IOwkgk(<6%2F*%cstjUd zMwJ1$E5$9|-%N5Y5(|VBm-+YuOtdV0m`0iEW?ZBl4WLA{AJ=|=D++8W<9V=?UXY+G zMc~)60opc+KdKDo(=P04_YZTG91vFz*DNSp4;+&@a4Cj}>Rwn%_q2t^2Ww}SMiRN& z$&R?V|8|&ohfyY{#Ppr^QfPZaLCCAx8|?yrn@ql{Y-*B6?qkRln5>HB^Oug29=@vL z2on;rb?kp-2Fl;o^K_1J5Ps6jvo_w?c(h|91MQ&TaKuSfLXsx5i|oR-b|nD$PXsI7-!q4Ahq$A7y!Ae z0c)Xer&yz0MlGwqnuX2g-1LVDFRvi9Fc+*oSy@K3&`NQ`q8vR|OD30v+L|bb6ox-k zaBaWMLzLzFQNLK^|D+eR4&ubE^9=?KWjHnL-8RyJU~lqhaCG|A)cCTHsuW=Gf@#J5{XB|`NkG`^o#&&dAY{=YgO>|04~3#W+P+ZQ^76qO zQw6Swx}6E-gh05N_o=>Ecz5VJl}+;@^r0xxTC8 zu=wTaXcXfY>Gc71%r|Q{5O?6k-$JtSs+K^#cxkYR^5>i|WQE`2=jt=B6ex2M`M(-y z{wiZq|0rXB8))eNX`uP5jQwq(`Kydc{iBTa{G*JaORhsiGOHoba3xU4(KD3g#4hly z>NWCx-|p1Op**TATValQo+<81l-IO8w;-?{e6Jhe8_5n*U+_9J-l=xXU!rrJ)GZ!vTM9kg$?l9B zybFtEY&1cD6OecQY!e6S0<$aT7;BT$bA$#Rn*svsq_Giaa$WfWlf)xr5&US^ci5W` z@f;w^_#=e^0~OmRsuUwoXS(haP2PMQJ6#ycGs9j8D9Ga07G^8=Y%SihE6-?2KI;5a zU#zXda!yD6ox!?#T&>hD4ux?2a7r{oCc4(kl>x;NFdA-zY=IHokZQX;BZ+jJi$6J3 z&bZ99@hOsRty-PiNQAcGi_AU@|@*(#!7p#&oVZ9af*EfC9kEwi!7p!SC*;nYx$qCRZ zOlarK;z6ZMXyG{V3F1LlFOOKaZ%-#Z!fcoT*JAO4gs;X}uy}5qEC)E*CXq(*JD2DR z1%Btz`m)lMD>Xd<`jR6~cHVkPj|6>)RcD{`OUTqJLj+OV`8aR?Jh6(qS%&^%QfF1^ zV`SppJn;xt!~Vpg%Bqt-4!&pW&=!5YR%*IS={uT;1sIzxqp+DH-dC7Enr#1%vbT#mq05)H$W3)T-9WJ>M;Vqer%`lyJBGZ`N z8caj#I;Gb6H}$<^kR>Z)c8iY%54m~3IEfI-z~1* zWf*xIX@@H#pq31&+!o8)8U@y2U0@~T(=8em!}#K>xgDiqLYNoPdt%jh7b)XMNl-H4 zL^3F&X>^D}vpf{ivIkIklj(^b(I-F0iI%I|e+rBT8XN+~mxFlh3<_jJ+9>4Pp&=6? zAVc)lStuuxGxba&Bd^6$u2Yw%h?5qr-NZ1ExYfU@)Mf~85Tf4a*8HqT6Gk#ZOHrnV z+0%1RN!JU?obU{ySYf2?@B5Ns)jfqEFI%7C{Xp0Mwo`hSn7~YYHet?WMttj3?|J2j zn3;X?wS*fm{@q%Qc38+%L%jq)-UQ{dKgni2jGAab5=KiBLKiPCY0q>jqA=Q^=?8xT zHFuj?NjV#*0atk9I^pN`HEU*V{9BiLP5dIMAt667FPL6SJSq_}GB})h1{s{oex-~b ziB>k1maReO0bd3VabcScFirPmn)K+1G_&~R+3&vr6it$#5q}Oa_`PK(GgYn0$T@tI z2>QN$bCC%HYtu?qus>3gV_M$8-jAPf@7JL>_YG+K*v?6@6;8@&|8`O9rmunadO(kN z{&wB+&N~gLFXPGQa5mY^dtEl#k19@+ETg+=g zd;^^jAmQ6nc;MM{Odlv@8gi_biVvX4$G{5@3GFdbhud6H5np6)6E~bujy`|Wlz=-_ z?sY%C1;X7Wo=`dcWMYHu_}cH0b+08)_d~J%9TFWw=J#T3iMw|6b#y_!7Yi)f0YqQt z=s{XO%IXq80$xsXh9W_W*x7XntE_m%My&t=@NM4&hDF{)RnjTwJo3&0Z2m!*JGV}n z5_Oz^io%Vp5-u9H zDRLpyJwVx0w|(<7S3y+tk7^P{Stk35feu;Vj_widB|=P>3&hRA&&&qV=jrP?-O4c5 zb{}!Uj4y7+bdFUta6h)4&XElmg3p|N0Cw0yDB~A+c|{_zoJVU48uNCqlba~(?+W$S z_j_|GF8PnJfa8K}uj+_8meuytJ2^y{=dJ4C!CKj-RbCEFKLCvGNsL;jePUCEetrBS-9m>-ZuxH+a z^eAlA{CSBY-eiAmy=B89(gUlU?G}^FPObGB^Sr%}c)h!6-U*8}vpNN+NA)YAz|{h$ zMy04~He%aBGa`C03f(+dbEQOfj7_j+VEQ-L{Gh!OBk@30`4{`h?A}VyB)$$9Y2&#Exk|GH@Uxg&!VrA4U?eq&W zFY`qhM!s`9hNc$}|NY9>ET1B8neuV9u>z%Qmq$34XO?!qM6lhNA%l}1fY?&l`20x@ zmhO0ZPR`IM7B>fRw`dIBSyP&n_ghtbjq_l2Be#&-Pz{#(>(;2(sI^M?Tf!8!1yFub%ZJ8C))O%Wc(zilB>i+ zT@w*`{z=$cX_t?CbOR!0Koj7nM0K%U3Y)}lUm_N!{;GuuivpPF@ypJW0getb?P{_ z*RgO>2dObCW`d2^)D_A5^q-|!HW77LNu%C{=@zRYI|U&QBQPr-pdV#~lFBJ@9a{kWu*#;*p`R!yrIwFD*sF`M9AYWzJG zB|5cn+T)r)9Hy!hH_B}>|C;Fgu@`5o*E13`hFO$b zZC5#h7u^ z$vK0N`YFNPpNg0*PQQc;SE>h>BWRo*cv5+oh-k^M$QGqv;*s`J{!*v6LI&MI1Cze3 zbu|6x-{?3l`kS+K{H1qOTA-L(ZMSk$rkFz(;^0Q<0?@qbsF;es&p7OuTfInma*vz9 zdgCNH?tc%GB00mzWGp@ENqiRBxIyg4%yvlF6K}Id1_;TQ)+9m^Q-GNd7-NV_`dO1210R{$trt3ADUx}p9FuFxpV7;OV0;x zYRBH$yl(@{LC6On-pN(XQpc1ZvZ85}GxG7D0WOn;wr+WlW$kB%enPX~L$UQ{S&yO< z6}0+gj1DM0R7?3}?l_{TBb57Z8KWbGSf{>1c_HcHNZ+XECsF0V)8Qjfx{9!h-r^hrsWxmQ}&QP4CR*)#hbK@CDoB(*-n82T%(9Vl0+dl_9yoaMvb?52{7#=8h>BxFYM_+~4A%5|r>3sC zcC8(5L|K@HlbdN6^Ns`8L{~)H@E3i(XYBpGH&*+pL<{c}se19F`7icT+m!R0(_ReF zB)Lc(PpW}O74H=ad>&EihBC}jvE>F%*c0`lhteJy58(|}r- z5IdJ?;iTkj^X$l}ysagzqAV(>RYgjhemDMtWiRJ;VS$_Py{9Q@^n}WXO+mnMjW68l z7xvAo3j_=v>ybz(lNZG{&hdE#ZI;u=RL$sd22_Q4txgIaU1xA!_bK4qhc|^&XAzZm zV3D$jM!O7veD|ANqeLvvB9-Fha{#lvx9#Z-ijG2q;~jE*(?LuSCqA#kPvM4ROI!W% zro!w+F}^V+cXJHnK1GA|J1l;FWoi6axZHwn5{EN0(_8u9B6fsF3ncBFqT3MHvc`(> zY43M)YYj3(dFE{ovl@h^**)EBcAbu%nUZA(I_QnI#` zw_T0l_`^##@`*uva>UCg5CplVwdqk#`Fj0YZ*odVxW(T?9ZKMv^M7!pUnU``sBK&O zQd+1)u!}M^FNzPD8t~`RkO4T)K7|y!vqf8Y6^XVk%5vo&&!Ndzu$Ejwg%m4Onrd@p z)HQ3v-WKZj&sJag1y?I9%q>@+6iO^sEYTI@1mkG=rWQ`)WdAzF_>z0qB~iNajyid; ziKP*VL8G4fvWJg+3%(VX$~khZ@_Ozf8(R0{w;Ej@apOqVBx ztX6s)cMb>YHO@rVEDeT?8#907=3wa1IYMyWtRy3PJ#N7#zoo3W(9+v6uQ4NF5ks<<~z%KWH+o%b8u5~MPg4CQX~QmW|0D9td=Yx zUne|EzGe^&JcWeRO?{77Ci@xOoF#@+aqijJ9Xn zJO6AKYO-ZtKMyzx5wnz%*?{nZjjq|jh@N?#a~Jy6F0|Nk{dflO9CP58wrAM?!Orn@ z)*Tn4+=hND0WO1CR{(D;HEye$+`*A^X9$#S$1A_dXwe|B$>6uQhCw~+`7THW`En^m z2A*jqww}%&Cb&i`d*Z^ih&zMZdCsd=?Kt}BUK@QZUP^AZ`DG;P`m7py>C-a;Bn$u(peM(9A3eO?2 z9e+?98JP+fqFZGgYbN%QHG_-DOpBJ&XHdm@1XTH=0kc}bJv;97cB*{VZB4@x{+D)x ztDJM0hNE*yBLZ zq)wf;q6J1kFT`XX45q;KL0r}SS_(d869eF!E**dj zSILpg429XFJasPVhfdOQqG^X>_blK@?5d^NHsUbAdxKV{(7fynm796JLV9HqxN;|G z0|%cC9xfu-K6ss}NqiANm#X@sXTQ;J-D<%F*SQ?=*yDb9x;q&gPW_wpg?w2JYVla( zK-m=ic6H6slb!}rQH7JmZBHC=?;gw5@V5juddZSWQp+O_CwH|523F_3J2co|3E9l@ zCFPg^_PYEpT0+0eQ>+TE8|U)=F$8N_=}fc4TK#k94X}FiTZCj?snifP4JLwcz+LT@{c_ly#++zjAf4OS ziufXQr+Ljb7rwuyRPOXS=N3533etbnFM#O?tW2dE6#cAXn#`SQQs9;?clG?*r5m|{ z9^72s8( zOyVY`q5XIoGJYI=dDsrdS~=Ihu)6ffK_eAG9lBG!Uosa3%^p_dwe{c)+oV?Q2QRb5 z!vaZ>^GeUXF0!vVdH8gXk9h!>G+QK7gjfSrXUQqDSZkh$Aj??){nkvQ2_@uD$# zoaRnsXZ$?EGvqe%aK5S5Ek|ak7Zs0KmX0#wv&aeZTf$H6+Xi5&!SLtVqOvbbGN)CV zr>tMZ$5N|s?w_a0s4`WUt^tjVtgS)mtDk3W$g0U}MmsXr0!nW+s%Bj##RWIYp)wB} z4e0ohlWWd@rFgX#l4fFHTM#txFEu?SZlbY5t{$F8sMIU)WCtR`b?_YKw)I;aM#`kH zJ08;i?kpB<8;&&Rosi_+%-)<_RH3skUL-l}y>0*5r6N;#C8B~V^&2pu_w~~Huua3A zg^q8X{oLTNDjD1TWBc?gm{6>A=gyD>xr8wvjc8xLu6K}}%hrfK*h^q^xYCuFugS-z z=EOQhjUX+Rb;^kog|uC@(S%w>Dk3O|7DR$+g}`g|vWnUl1(4)-zomq4-$dmo=0b(TU-G2loENa>(`40LBdjjAzmxGq23GuJp(&GX1QgM13C zoHZW*O1(e`G@VVP&bUbJG8wUsH<)tSGz<2+M`a;d22`rif!WV4lb!m9R2Iz#!`b!v z;NqT7i^Rxzt)PHS)O7j^Vw>syG*@pfO@U>LT=VGB#!Rww z3-zwurE|)|8^M2O^%d!KdJ#5>C*?Wtl&&g+^5lz{0B`~}8kc5%NR;6}5owSY5k@O{KhaZy4`mrgNmc-gIrR2c9#*szJ?{fIJ|*s# zA2;@;m1N*DN3IW^%*fIRBW)Q?IgFFDK@vC{l!kV&zcuG8A}hEs-A5>7vhLpatUk_v zb?^IC0N6ySt}pG4@bVW$4Ye*^WSk8i6eTb`+BBfcKV+9uIc9+rc6rQk+{yvlj9)>8Qc(+-(@?{>?Sz;*dm#Rh$?6!}TeL&06 z$I(zL=^^rD6?_#mSC)H5{Q=6|mevjU#fd;7z<2a&!`}`L$-jwk&&#Lyxq>)D_g2p7 znLep9dTa z$OXC*$XfyZLOr_^Lk?llIyKV>f<+a%AO*=W70X=PwW#jNB-2tSjozx&%rJ}h1FM*T z0NUH(&qkizJZ>?v<%GXR$t2ruFjc`|bN!nf990;Vv=!Qu1Kd**)}|Nb(9d6UaFUupBCxGP1t zCB;am=cMj%`cmJbjJoS*T5bv_B~+*;S@ueu*tsusyVcYw@`Dk))F{q3_*eo~x(s0& zv;MYJ#8@Cwo6t5>&5iK6ua?Q8s4w~wJcwsdx>&mtyS|Mf(MPL!$|x>LFTM#&5g@;R z<8^%Db!@vgr?;_k?AnpdL1CmSA{i9t>l$FB#Fo_9qC^KqWo^hRBWbtEgonXgFh&!2 z8WZhFQ-*+1wj^zA?5@Z9Juo*voUKQ>*YrUUHIltd{kQR!I%I+(%(8o-`{0)B*P4nb zwK7%zCfe0=rExOcDEc{sRHD)@UVub|XIYAs@s@ax2K6_sHhDJm9Nn*`)f0;nrc7w! zM1lpvv(p|p0R}l{Uw?+6G|yuDu)vlH+fg%nwf*3u&2|17FjLj8v}j0PY{V-YXqMGr z5lnl<5nzl#t6BBrU}qQ=dQ=Tmm%SX;BXQ=WU6$B4`PlG}riZ{#TuIif$^zIS8q^~- zTd?md8SjmRruaieP-4%;01e$5;h9)tPY>=E@FUQ$Y}9o(CRi9+2|PNZ-1T)4E+X)| z5@*;``0UWo`;g|R=}Lesj}W@dVJw_Gz;wny8SKL?IG$?i`(9#pt4`rl^?Xm z$4Vuenh_eYW*BF6-R2E&Zdukbo2Q%)VH|0peErQB*r|1I-6?u_C!LRiPz^$xb>fJm>1O{cjfcr~_^@dRzllCOc@Wjn6Bl{FO1 zl*11}!^P9~^h=FqIRN42IlEeO)w41LeF+x>Wux1s0XQ*EcH^?);v=Hm$J=OO$m!rnIv1Iit?g#B|n@14}1>?WNeZY|_-fvPzm2Y_}Op)5!3WdYea_ z`lm&H|HqC4#6rEik=0_Q+*L5h;AeU2h*X?U7Qt;iDh4-rtAJ@O zm1Jk%bma@Te7ujwa(uDB7W%T1uGSc0SW9A+b9=9(=bHN$dgvvLj-LxBrh26}zk==- zF9mQFt&GHtLc1}R^NjU?R??3Iy@p!jh8nHYpa-AEJ_9muf2`j@Dx4$&#l&tb5;JpM z*X#s_vbdtZymr*N>J&MVa1@riu)d4wc&P^R2(=@=`H;sVr=7UlInq zX7Jr)@UhZ(3|4{fk8yBTQ+;Nio2sl(r|(tDO&4}9Ia=32ZI~UMSUtoen}NI}vyp#C z${eI}69+hR46`E#QIy`plA(neev+Kl>ASu#&DdW=p$ps6dGDdvCIxp8XHZ7m&IH9p zZlBiR%^##SYQYb%ARz)%v9)*bUHhWkP1SsGVaANQG5)QA^P}*Jh%ann^)U(%#GJ5Q z^{>KC4>zxtA&1ml`U+}6$W6vIw)lA`gz#qM(iz~xe6%$hFy9%>ugdgU7|u$%tsjn% zt{aS?R24$daKzIbM;8-K39JP#s=@`-aj^P!xhSdx&=mrU~rzBt8ji!Nddj zo@vJ$17PAPQu8zu{)X>ko!h5DQE;R$4f+6JAyy?dLHh%qK;3!9jt>d^L_tfzw+%wvz-JlX4|U<&b&rdBJY?-9q6pBlK;V2feJ^N1WxPM z!^AVQ080=_@g&EX@97ayyjFr_eG8AB>)@?>kl=4#d9rMCOAs2)(0Yd+3w4t6d!hTt zUzvjv?2X_HBsS+uLchtJ?9*33Hm=2p9-;R_0W^z@Gt}FIXRCZaflFbI{wovzlyl)u z3g%CD9~fFH)G;e;)AR)+rx&I7LqXU#w?z>wgHs^c2Tuj%#8#4@H3fp4-6NBUjtvzh zA-A#-{3aTFxtx@BAzwMz)BUws#kRBmfX}Av8s;0Xu!V9MfuM9oIBg+dci#Yt<=k_Q z3EkpS2y17P9TX+TPYVfNab1i4DSno6Y|QSNW5&FDp{rOZ8Z6+t%+5&T1)-aLq-FgI zUD`}>RMpdN-nG)yaOKE3&jU+wT}(m_kZzGtQU$QM^6y)^KOeq znre3bWUV1bcyI);SCKvyR&Z)6RweNYwJB zYbz_t^gg*5JsD+si~TLZat7LOT!6mu4;~7p&i0y0V8|kkk_O>@o5G6o)!V45&8gs&k+C9D!ekmRsK|?y zd*lnR`5{1SY`+ID*q*wx#sE3eeKq30Q4jnLMt>Ag%L5#@(D7{XsW@tvN%YRBg%w&|0yLhO{{VIZhCv z@4)m+V&ssuF9F&-y*xGmUV`7Bme)N`C=+=@qaZ1t@|cj0D-QNZ-Db9|TU5CP;(#zJ z^@eHk8lM6z(z|TS40%S?mR+GW#-SGQe}1m@Sif;wze%n++{Q6nDj3hSyCcqf!w>}A z-wdT-mq|LVvf0tC-xOPY=K5)$KqchXtKw_UVD_{JoOY?J_@Fz;2bujzO;dx|Pad#_ zYzdlKsM{M5fk20fO-_YovA4$g!oyuIwbDz#whVY+TVkKZyYrLh!_Gg>rL?`leL^RY} z=3u;mbK`CU=HUqm@=;kCzOY$A&$x7f9mgmP2p8sh6ZUj7GO08HXO=z zv=Z~7Ll>k2fBO2AjP>g@l8z^bqm?Lom&U92+ocFA`xmUpq?-==;rGj1V59W?stc>b zkhPsJ@Tk-l(fR4=0+8Auq;pAT|I3h zx+IU8&8c|`qxc}s2}S=Ta>m-aRc}?r)4icN@a;U0AI4)GUCwWrrvu0d!&=LgA7kjX z!%qH#mo&xbt|FwpNRD)V8M5Efb_@nXY%GzuCEdU_VM=Ri&qyoBV2b7t7nj}aO_Sj# zHA_B-Sk6MBDb$4Jk}$8*+i2Zk9Zbm@F(hvP$$a!3E^=m8ogdG-|52~-n3cxFq z{o|!IMyUg`pAzL~GK9ysR*4w%9o}Md?ek2`W|1*-6Ka7}fG<^Fu-7s($wK;rwsTNg zWEq`4w#z%FrMJJATg5R(UFfeCAfOlqhv5ocK+WOlR^74R=xDm#K+Om_-&}&^8+W`~ z;TZJpJq963L|`N$CX^zl@WV;_S(mefbX5TETLe6juGg?51Ann9#-%dSG6r`L0Vsdd z8Bf^|mP%is0*sEsTz934+f$&bQup4MV+8*3L6R|F&nNW#1$1;|#A;lM5(2rI4w7*V zX7ovHy2S`xrT2o~Bxln&!tfeZ5;plwGx~$;SG$wCO9D#a*r4R+qqid$nYh$C1o+pY z26W`Y!4-35_2Smf%%s!jv1#7s=5UiTeENd)a?}8GJ~k0C?$!wC1#sWM)w%!y8vupHakaP+bo?sFrtR(qtXM zuTy?ZTw$1apxB`D!<`rb+8=-FsUYl4v-8ZpTOJ=vWLvzTuqf3Iw;vSHw?Xx<(a!?W zM6&_Zwx2$5Ki%iH798Df3*mxf3KqbBd7Wj5Objev0TF&lFoH{2V%o+VZZfU9PfLZ{ zM5zfVXZ(8o!%+$#Vc(y3w_-fo-Hd~%9%mF3aoP?$d%D#+0BAZ=9{C=jzxm}nbY_~^ zeDfCa^V8OMx8~IX70mmFRT;rhjN{T;B+^?z0l@iF`ESG?!-Y?+t@_ro=wja;fV$e> z4eX2t!?A6;(!ppu({tXZ%%)m0FV8uUX>;JaDY>F^OB;@s;kYc+6GmU2ccjpuzv(t= zsb*sDUjG1pxU_->NxfCASmNR)TRMGtL~!sZd!_f9)UMGee=XdxeJBsA8qWnsn?~vY zh=*(atXXY8zE$vCu{@Z&N-+bKB9+m9%5X1&Ww)!fYe>~X@2kRS8RM5p8;ndFtVW%` z2({^x*-5(Sh?PclFj@9khR-3imF^)|-f1?RZdiHyafzKA^_*_A(&Aj{ z1hyWvLVAS;TxjWCJe3|_cq9$2Kl4rne4AVgL5=PWg?Bw^Dxfa%MG>cZqpj9*Y4e?Y zWcgvzDLuwgHhH%WP2q}g{p{POlkkZ{C|E!=>1VVy(cW3+qUQ5=G?atVo~sHP`}Q{w zix+iS1@+0er%U|-yx7@|Dit*1Ma&|4gZ zL9Vp71$8kHTgV20)0Z|;DSU8s#9oh|lSEfgerYL;+Pgsoepm!!*TK?r_$Yb#ZbZC}|EE2I1?*Jin49h`g` zH!0l*5*z$R#~c9y{Mj>U-@~ho)ZIj^fEM0u)W)GAHOgNkgjp1*VW1LrJ*y{jgicyujHAfM}0hbtYJ>74Y#4-Z>HmKXte} zfNm-??p$-^o;I@OYf#fF4xl|A*`;9>b$;F7zm5ZpgWaIV(W@uwH^>X=^a@WD0p#V_ zl1P-^d4w+3x7cuTT0qw}JQKA)v&GYGZv9ZrNweB{&MHY`@S#zUr~#TkF>z{J8&mzK zP^;=l-kaVm3l>`t$OG=h#jdsL&Jq?c{eh@cOT3xQ23Fvd zivu#%w^jog+_W3LuKnbp9B*Uf<G#OxK{Ie$%WNF4=C`0Vw=vs-V8*;%H-ic9r`gE7?8 z1vhrluek-qW5=QmoO#2?8`x-6YL-20GIYy(qPcFn5B7 zou4|Z*#%~<;)8|+Lbxyq8h0;!$;3T3Lf7A0>R1-cu!FiT(hCR6uu1}b18Q-Fg}1tW zv68@!zxAakOY>};hMtl}*2sL--#0Sj>x)ABQ1>}pLxit`gDF%EVM9TN5J$}YlBX8a}9R2z zNsWK5;0$fSu1sH+TCi!W5F!?)OKch!fQ_S_I83j(P-+|PEXY5=O843_Np#cTV~(u zMS^2Z*2I9;oDRitk~ohySl*i`R#x;vJt#OPvsB54$P+G<7 z5cGHDY?s$=*bkqqm#Zj+Q&uCGS#u@r^*-5ksu)(YQA`jJ9S}lbj{_L)haeo_ zLlBO^!O`Bx(ZE#4+Q>!6$-&58$IRNnQP0wn{%?^uR@VPYB<@uAzZHqAQ`U&iWyki4 zW_|BcuUk^4{)GJ<#H`q`2C2ndrR8W;*}R0>-p3G73Mo}}Xn8k3PtM2`O0=V(ZO_#| zu*6>Z!40NWb`m9l3m@rj_H;+uFsCYvxYDRj>U4x_vWQuK! zq5j-Qn{bzVG!v1BgYBNz3R0SmD1MeEUfOU7UGh8cG1M@UDhg>)n`0gOKD2!<`2KBU zNdLwVi8y)epzWs;6!LGc?X`q{d)371%B?YgZ7}Qh$icI`7(PfrH&DZ${%mwLFn$82 zunN^~TN|HEG{bu3A++&|+{v=(=-@9_3m>;zVw>?E_~?@crS-CTVO1ws$#_A$4<6{N zX)lx?FhLB7=NV&t)5Jy)SIl%%0e(**b`YwcgP07CnZ@hf4X{`>;oO92|F!=c%tDZv@} z?n?Qfp7WeZ#-`WvMlu()+6bXN(TzL7n@^+Erb|i*+-XYMlT15hxD?@=H{Z3IG>&cg zZRFMYlMoj^mFrZ?x;%UCM|+(UHiceuueUfMIkj*1?t5g0jbIf9mSU>}$QpP7Jo@*J*z|FYI>%H5_Lmh?zsemZHxhYn$JWu7Sc*@0+;O7B9z z=BO`zEux|Nn1FN+CqHXh*RUf%!Cm~@$;&ERu832IhxZw=(5KqAE>ajFXs5V?B8brK z?jn|?-V9+2cfXw&sTe5{@=p{^)JS)+I`aLtl=c=+ah>(B6l0LDSv*&Cak%8R8pB&X zJKiT-9J5 zy0~T7CCLv-f=R}dq11v!$LmlcbuNm{@X{K3tO125@$&dOV7Dy}Y#}+a8bd=4RWo@g zo5IJv4Mh_3ua<(OwA^radJg|)oZsmNHUhx9<@>r}P{X$LdZ4uo;S7~1H3?RQ`DDx}AhI*k3o3@nV|caT4r*1Xr( z%?AMj@&~#8#xx}FzcCH%Lzz;+QPd z%eNSGr+kTTW^%D|A;KV_!bLTA#%1mRzF0o;fWnHlMM2Z18otc&G%l7oW&0}V4oG3- zG*Zd6&tjSuM(kM#d6=;Z1yx(zS~T(G+kHECPxg((4FYrB>B@*p>| z;55Oc{EAO~!wd~FJsxJ|LyfYdX=NTPraq1KmnfOOrZ#~bQP#>;ng^mZSYGo0{k z*~rp7LHr@u+)1!KMUcvtSRr_-lsm1Bt?h-u!>x!y3^$}a|`!7v3ZmuR=WE27f#o&!g)uk!v|(a zBJr1~%rjtmGvjpwo1OG&geYlfgj<4JSo!nbs~cCvYL(Twf6>*0#w+HZX&>R6H7I*b z{!1;@(f=W-H%!|1^U_xu}=`Fh;kx^nH?oDg4EU+L_d1d#vH+4@#_}U@} zv<5Rz*y?QKbD`5aw}@P4?s`Krem9qkoP^wo!`cavmgoMaC*%-yG+K9c(W;Q3cdOzi zP)|R%pNh5VNP3nZo|fsF1%m*PF6e$r?BOvsU2zt(o$ytUUs6AKwVSmN6B|Xb#*E)h z)0*zmz>Dw6+9-u6SYVApj~LZe9I6gjq~d_Aj#27}@#0cVOej8^vAP6J?_Cle?w_Hu zs=gn1_t$cj#*O&h7cS9Ubx%fMujY5ge=9lI4*Dlb=p3?E*dK9d{q*tj&oKRwwC#`A zCFExQ_sZoz3KpHe3lB?iTJ!8Dtu+3Uqi;>9t@Ob9w^46+DXB@syrCbL|qb0N)7RcmRS2LI{ zc2=bD@ZDnMcC+5ErXVyITI5P{8EoC36C>dYH92=!nNL3?h>kY+~bSWE30hW z{p|@;)V8VF5%GZ#^(OpPqn|FOf)I|@T|P=Gb3eemqU%$>OB`j7G)rY0&$nEB>bqWr zTfWGg%R{>_(r-~1(m5Q=q@Qttv7j7e(vy*X8{V9J{uGJ+nJdUljzL zv2Yqz2nyWbGC@6d5xW(yWyVh0mqZg)!wpdpYQb<$oVZEZ;1%$Fl#wwrn68GdK<*%P z(L%L$wBHcn^%}eE=%uLVy!dUGi1&#(PIP%Sh6?%a=b?hWFBLV?ITM7YzXh$t5 zW|VeFYBW@KY+Q0gih)joc4%~bnx=@-x1}F`nytyV{w)~s+nMLwm_ar3qsK_GkdF~S zWN=0a5p`*RhR4aEG<>T(k8kB|!Egj~(u`feyS;?!y@5iTqwKlKctKhDRCQokt1L)b zgCjHP_|4jA0x9}rT8F^eVyU&%fvM$~;(S@^K9%lUp*Lzu(}QA~-H2F%v}K&E4Q!wE z(_`UHMd*9pxz0NF9pBiXXZ}sM{4o*Wz7S6uR$nMep=v!5Zj7BfvF?p(UVO2qn)#T3DHcrT zihcg#NuJFxRS}6k#}amwhTZ3Y@oGn#aBp++YGY^F-)7O(icuYRvJ4pSG6yi`EfwDG zWdV0(_g%i!b1qF$T02M=#)feQ{Rv^DS+t-*4N-(CrL28$p&}7C5Y^Z!VScIzJIu6Z zqh#ay#8X)Gz^ZCrQ*uIdDDEzzK-gaJk3@xrvC{MxBl<|%Kk4~5uz`BT>k)GO{~#MH z<3EwD=l_Fjg#UJ*$M%0fwvQ42cgXhg-y>UsytHh$AadKiN^AVE@0HVb7L$5H03j%% zSoNw5pamDWYRrRpw`JoZ3E7qEkeYn2z1F(8Qh{01ca2`M-L<5)Ge&Z+ zNYz?cxBJvswoz)fbs!Y`GEv_-Pbedj-}-IuSM@xC9&c!=kZ4nV0ML*hu*&~Ce#eT( zBVwCVJ*64OJx1cekGGsKDNr#+6zFBvTOLsb;5M!NdYDc+Y&(r$dH!kgS8y0#m7iqWdRkv zibx>|tj-}@&Zw%Fs*d?mdJ=wo$1wZVSf*n+qE9B^EQT9S+1U9gaZ4bw2$x|7BwD^Y$<@v%0aakHDqAj*hWFuk$0r|GueN`a1V9gTUEb);M*?SNX&y6*xM z0(kT>$ts$0N%3elhPsC4y862MhKBlC1UTJ8ot~WkHJDXnppn%$gSutAjc(> zC5Lid4n4Y=B_HOCgpwscy1A2KklD_E>2{PheHW_e`9xg44sbqQt1Z!;_h=xqZl(jM zM=@}ma_t-R5&$}k3L3+1-c*GqlO78R#(2_N-rqFbYOoH=ijPs}nyKa%LP&J9yum*ewHWDZhCF8VK0c*^BCf?Vb*Q-MV4fvtRe6)o`3E z8d;c}IWN4poW7<4?`X6t^x*yp8W>ina)}^48e+)O%B0o8euyMsLYUiO!XbSTf~|Q> zK;H1s5IIeH10!%~Xk<9>^h%5!1gL*NhR8E%YUoFL{7Hg8>5*syCI_<~_-gW3X#=#g z0TYDzS2!D*Y``SJV3z*FBewiAu+jkx8OUZ2#sNbc@pnHmV6r`!wa| zH?6-6z_V~LG9b4jn8{x~YRX|vz<|guU{-&df)Q9DfRXlK2*5rMFvGv>4NRV3xS)SE zYvfQ-!GQFRA8z&lFoVDBz*PX)_Ft`+7-$v*=J1!PW)c?;40sj-X8fNfLIn&A1=IP<>rY4Jcz6=xd37O zm&d;i-nRhp@6lU?K>Ry;4WdgBTFxlqfWHgsXe1C2%0J~6KET@fU-c8cgXupiCpg3Z z7HsNNG@_S8(Y^OozEu4>G_opO##lL}4Z^FzHlx6t5%wv%H zoL@O~)&sWf(C(1#z`wka1(yc1ouuX$nY{F74NaS_2w)7sREV%mJJDrGC)n8NIPkE+yf)eey&A+m-2D*(BvQxJ=-l`enjFs+w0 zKn`33l?!E$im{wha#td3yr5lhh#4ho1rm(d7mz1+-k{P=B3=MvSsnan%n-Cn_~O&| zME)Rj0N+2;tVr6W_z3UD3L5xz!_)?^D-X?Rh*PvMWaax=D(&qP*gg)XAtCY)7_+zu z$CQR!MH-sduYPY}Qc_lUihj-^_667tWD-Rz;H(joa7^TquJ8i9erQ`O;wt-f^@!B*f7{I=5bx-X$oDt1?#8N=`SYFy0~XEO=vZTLnhfx#1xt2 z0hz4(q*7S&%5!C#7-s$vU-7j%ZxE;2(g5S;>2zp^3NH1;7bNL|#Yp0%Ea>z?B9s?} zywDiQfe2uq5|h5jyf2+$`=*%PuF|4Ep=jjkB4!zrKY6{qhg|S$lZ7}c3YPXkcFPza zz17fFV>{u%DOts-X4?Eety_CkR7Duyy}Ki>;6uqY^HsO+ghQBhAtk%g2Z87X3KB%cYIm6TeUOH|m4BQ`VF-8*;Lq)z<< z=iJ|KzTY?Be0S!%-@uN;O?S|D4fA|?sv`RRYjr-YTV9sB?p~|9GiRUM^oSi(zx|>p zxX-kv@A1N@?>_hU?>HQgQEf7O?y@tiGgW`(leVdyXWP;LMt*msPhT*9>iYVkjCa>8 zseI0*To4f`9<%viT-O45L$AYu71=`{b&AezuP8kd*BGq{nH;3IQ(vsT?R@0GqK?wd z7tWo3Hf!aM;N-Hy>nrsK`a5Uso)Y_lta-nxr)W=Uo1)q$ef90PwHGhB>aM&t+TfAY zux^6hd*zw>tU7IPk1p?!t5-);(m7FL=zGiC3hlOr7kH;&Fs%H`8Bu4iYCRRJuhwVj zCO_4fc(NvZqp{8B@G?ovv5#+VsZ$*Fzq}z(m%kVGyLr@x2f1G3H}`(odbF&*zR^(qNqCjX$#1#6{i%i5PKEpGPEK=r^?Yg6`Q~-w z<#UgX6V4d#c{%rlOK|7@>v>!Kszyy3d*d0C_lcG(v(tw!sl7gA=d^u0k5=nniVrUt zx3^4NTQ;W3|I?ILit0~zj`KfWT<}VF(i`6>et+%w*pwvSun{p{*L$173uBr>=Vf+P zcSHu=DcTm|Sk`i8VQ<@S%(&!AqhOf+%(~h**`D?V8{|I*9PIzq z9hYT>T`x_nK>bI&wM8xvGR`7r+=LKxZ`=f9wP1$S@5$a(=2I4GmIEa`rUg-!gO@xK zh2>SifW;tEdJ~<;`1HlU;ldS&{rX~%ZLeKVKbjo_a;zSKSfFC;#BgE)cmzhof)U(q zjWoSv(*$bb7XD$qIDh{|Z3+?dm zS;oy06(>XBz$XywHx$D&p-U!^ONUdtI-Za_yns0u9f%3&7Y~$HHnQRo{?l+4|1=(` zt#Ek)!V5=WJkAPw6GKa#*r9ui88;j9+C;?pOb#;>0d=Gy&%EH!rD*OSy5Qk6;g31M z0rso_Vk?cxrD%NKD57DkSGsaAiC^$CWaC#QhJ)bra>hyecp}wfB08rrr@+m^aWKBE z;xAYrGkG)%&uS1!;axo#;z$n30mfv})XE+*G+PaGwweM-hI!Nwlcqq$D*5FD8NB-> z$_PvON4yPALf#78EhLoIeDr_JRqi$k0d+ z3sH`4i})V7!^V|p3HDJOcu*}G27Qu&Kerkr*ES_1X24sVHO$lu$OL9GJ;oWy)PX>zLPty3L)j&<)mwlBt*(EqYctOD^#VC;)N}(B3)0&`i>Bclv+f|BCYwEUk}Oz& z?!Bh)7INw}vp_VxkpELloeeGafNr8C^F6p>_8{^fdvKpF3Ek0WBTRQ?21Z!=y;**c z2;1!CkqF~n9^DY)N4U}=Fi3={F3|liE$G%B8(};}y63=;aOn@`DGm~0%7gnvnC?Td zHPnzbjI-4IwzOzkO=dlKD-8_iY8o6Zs7go0(S&Y7fi-IwYH!DTA>4}b(jJVf;pud= zH1J>|c*us4=W-A!SA36z>NILSiIp+Ai0}v?gb-AtjpQ4##M&{8+7?WjIy(^* zmNU`3sznKroDAyLqWWN>Jt53T<(dP$TFy|VA!~Cj((Oj1?awmvsG@CPwh)`7=E#?D zbQdlf79ZjKJfs{nl(4imTytDRP2!N@B+Cqkv-0V%oEP-tuVt#GQW}745{RV0b|6@rB zml%+0ZVXWk#kHRG+x-V7>|X-#8|4qfOMnyKZx7s`qd#IFrb73kVd-fy(oJ>$ejV&7 U0SbpGL6l(9B!M950zg~yUy{HoNB{r; From a948432e6a4113b0f981c4b245ea865a50e67234 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Fri, 25 Apr 2025 16:38:24 -0700 Subject: [PATCH 12/14] Lint/refactor to get rid of some squiggly lines --- ...stretch_simulation_web_interface.launch.py | 3 +- .../stretch_mujoco_driver.py | 79 ++++++++++--------- 2 files changed, 44 insertions(+), 38 deletions(-) diff --git a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py index 3a671aae..e374aea5 100644 --- a/stretch_simulation/launch/stretch_simulation_web_interface.launch.py +++ b/stretch_simulation/launch/stretch_simulation_web_interface.launch.py @@ -21,6 +21,7 @@ LaunchConfiguration, PathJoinSubstitution, ) +from stretch_mujoco_driver.stretch_mujoco_driver import DEFAULT_SIM_TOOL def generate_launch_description(): @@ -125,7 +126,7 @@ def generate_launch_description(): parameters=[ { "has_beta_teleop_kit": False, - "stretch_tool": "eoa_wrist_dw3_tool_sg3", + "stretch_tool": DEFAULT_SIM_TOOL, "use_sim_time": True } ], diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index 6060d22f..1a83afe9 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -57,8 +57,9 @@ from sensor_msgs.msg import BatteryState, JointState, Imu, MagneticField, Joy from std_msgs.msg import Bool, String, Float64MultiArray +from hello_helpers.joint_qpos_conversion import SE3_dw3_sg3_Idx +from hello_helpers.joint_qpos_conversion import get_Idx from hello_helpers.gripper_conversion import GripperConversion -from hello_helpers.joint_qpos_conversion import get_Idx, UnsupportedToolError from hello_helpers.gamepad_conversion import ( unpack_joy_to_gamepad_state, unpack_gamepad_state_to_joy, @@ -73,6 +74,12 @@ from rclpy import time as rclpyTime +DEFAULT_TIMEOUT = 0.5 +DEFAULT_GOAL_TIMEOUT = 10.0 +DEFAULT_ROBOCASA_TASK = "PnPCounterToCab" +DEFAULT_ACTION_SERVER_HZ = 30.0 +DEFAULT_JOINT_STATE_HZ = 30.0 +DEFAULT_SIM_TOOL = "eoa_wrist_dw3_tool_sg3" class StretchMujocoDriver(Node): def __init__(self): @@ -81,7 +88,7 @@ def __init__(self): self.declare_parameter("use_cameras", False) self.declare_parameter("use_mujoco_viewer", True) self.declare_parameter("use_robocasa", True) - self.declare_parameter("robocasa_task", "PnPCounterToCab") + self.declare_parameter("robocasa_task", DEFAULT_ROBOCASA_TASK) self.declare_parameter("robocasa_layout", None) self.declare_parameter("robocasa_style", None) @@ -92,7 +99,7 @@ def __init__(self): use_robocasa = self.get_parameter("use_robocasa").value if use_robocasa: - robocasa_task = self.get_parameter("robocasa_task").value + robocasa_task:str|None = self.get_parameter("robocasa_task").value robocasa_layout = self.get_parameter("robocasa_layout").value robocasa_style = self.get_parameter("robocasa_style").value @@ -119,7 +126,7 @@ def __init__(self): robocasa_style = -1 model, xml, objects_info = model_generation_wizard( - task=robocasa_task, + task=robocasa_task or DEFAULT_ROBOCASA_TASK, layout=robocasa_layout, style=robocasa_style, ) @@ -230,13 +237,8 @@ def set_robot_streaming_position_callback(self, msg): def move_to_position(self, qpos): try: - try: - Idx = get_Idx("tool_stretch_gripper") - except UnsupportedToolError: - self.get_logger().error( - "Unsupported tool for streaming position control." - ) - return + Idx: SE3_dw3_sg3_Idx = get_Idx(DEFAULT_SIM_TOOL) # type: ignore + if len(qpos) != Idx.num_joints: self.get_logger().error( "Received qpos does not match the number of joints in the robot" @@ -292,7 +294,7 @@ def command_mobile_base_velocity_and_publish_state(self): self.sim.set_base_velocity( self.linear_velocity_mps, self.angular_velocity_radps ) - elif time_since_last_twist < Duration(seconds=self.timeout_s + 1.0): + elif time_since_last_twist < Duration(seconds=self.timeout_s + 1.0): #type: ignore # self.sim.set_base_velocity(0.0, 0.0) self.sim.move_by(Actuators.base_translate, 0.0) else: @@ -933,16 +935,17 @@ def get_joint_states_callback(self, request, response): return response def self_collision_avoidance_callback(self, request, response): - enable_self_collision_avoidance = request.data - if enable_self_collision_avoidance: - self.sim.enable_collision_mgmt() - else: - self.sim.disable_collision_mgmt() + # enable_self_collision_avoidance = request.data + # if enable_self_collision_avoidance: + # self.sim.enable_collision_mgmt() + # else: + # self.sim.disable_collision_mgmt() - response.success = True - response.message = ( - f"is self collision avoidance enabled: {enable_self_collision_avoidance}" - ) + response.success = False + response.message = "collision avoidance is not supported in simulation mode." + # response.message = ( + # f"is self collision avoidance enabled: {enable_self_collision_avoidance}" + # ) return response def parameter_callback(self, parameters: list[Parameter]) -> SetParametersResult: @@ -951,9 +954,9 @@ def parameter_callback(self, parameters: list[Parameter]) -> SetParametersResult """ for parameter in parameters: if parameter.name == "default_goal_timeout_s": - self.default_goal_timeout_s = parameter.value + self.default_goal_timeout_s = parameter.value or DEFAULT_GOAL_TIMEOUT self.default_goal_timeout_duration = Duration( - seconds=self.default_goal_timeout_s + seconds=self.default_goal_timeout_s #type: ignore ) self.get_logger().info( f"Set default_goal_timeout_s to {self.default_goal_timeout_s}" @@ -1213,29 +1216,30 @@ def ros_setup(self): callback_group=self.main_group, ) - self.declare_parameter("rate", 30.0) - self.joint_state_rate = self.get_parameter("rate").value + self.declare_parameter("rate", DEFAULT_JOINT_STATE_HZ) + self.joint_state_rate: float = self.get_parameter("rate").value or DEFAULT_JOINT_STATE_HZ + self.declare_parameter( "timeout", - 0.5, + DEFAULT_TIMEOUT, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description="Timeout (sec) after which Twist/Joy commands are considered stale", ), ) - self.timeout_s = self.get_parameter("timeout").value - self.timeout = Duration(seconds=self.timeout_s) + self.timeout_s = self.get_parameter("timeout").value or DEFAULT_TIMEOUT + self.timeout = Duration(seconds=self.timeout_s) #type: ignore self.declare_parameter( "default_goal_timeout_s", - 10.0, + DEFAULT_GOAL_TIMEOUT, ParameterDescriptor( type=ParameterType.PARAMETER_DOUBLE, description="Default timeout (sec) for goal execution", ), ) - self.default_goal_timeout_s = self.get_parameter("default_goal_timeout_s").value + self.default_goal_timeout_s:float = self.get_parameter("default_goal_timeout_s").value or DEFAULT_GOAL_TIMEOUT self.default_goal_timeout_duration = Duration( - seconds=self.default_goal_timeout_s + seconds=self.default_goal_timeout_s #type: ignore ) self.get_logger().info(f"rate = {self.joint_state_rate} Hz") self.get_logger().info(f"twist timeout = {self.timeout_s} s") @@ -1342,21 +1346,21 @@ def ros_setup(self): # start action server for joint trajectories self.declare_parameter("fail_out_of_range_goal", False) - self.fail_out_of_range_goal: bool = self.get_parameter( + self.fail_out_of_range_goal = bool(self.get_parameter( "fail_out_of_range_goal" - ).value + ).value) self.declare_parameter( "fail_if_motor_initial_point_is_not_trajectory_first_point", True ) - self.fail_if_motor_initial_point_is_not_trajectory_first_point: bool = ( + self.fail_if_motor_initial_point_is_not_trajectory_first_point: bool = bool( self.get_parameter( "fail_if_motor_initial_point_is_not_trajectory_first_point" ).value ) - self.declare_parameter("action_server_rate", 30.0) - self.action_server_rate: float = self.get_parameter("action_server_rate").value + self.declare_parameter("action_server_rate", DEFAULT_ACTION_SERVER_HZ) + self.action_server_rate: float = self.get_parameter("action_server_rate").value or DEFAULT_ACTION_SERVER_HZ self.joint_trajectory_action = JointTrajectoryAction( self, self.action_server_rate @@ -1375,7 +1379,7 @@ def ros_setup(self): # start loop to command the mobile base velocity, publish # odometry, and publish joint states - timer_period = 1.0 / self.joint_state_rate + timer_period:float = 1.0 / self.joint_state_rate self.timer = self.create_timer( timer_period, self.command_mobile_base_velocity_and_publish_state, @@ -1589,6 +1593,7 @@ def get_camera_frame(camera: StretchCameras): raise NotImplementedError(f"Camera {camera} frame is not implemented") + def main(): rclpy.init() From 47ebc6a8d19d5c47830dd91987a5d95941a7c2a9 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Thu, 1 May 2025 15:24:56 -0700 Subject: [PATCH 13/14] Action server now waits for trajectories to be reached. --- .../joint_trajectory_server.py | 12 +++- .../stretch_mujoco_driver.py | 69 ++++++++++++++----- 2 files changed, 62 insertions(+), 19 deletions(-) diff --git a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py index d69800ba..628684fc 100644 --- a/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py +++ b/stretch_simulation/stretch_mujoco_driver/joint_trajectory_server.py @@ -112,6 +112,8 @@ def execute_callback(self, goal_handle): point.velocities if point.velocities else [None] * len(joint_names) ) + actuators_in_use = [] + for i, joint in enumerate(joint_names): try: actuator = get_actuator_by_joint_names_in_command_groups(joint) @@ -128,7 +130,15 @@ def execute_callback(self, goal_handle): self.node.sim.set_base_velocity(velocity, 0) continue - self.node.sim.move_to(actuator, target_position, timeout=None) + self.node.sim.move_to(actuator, target_position) + + actuators_in_use.append(actuator) + + for actuator in actuators_in_use: + self.node.sim.wait_until_at_setpoint(actuator) + + for actuator in [Actuators.left_wheel_vel, Actuators.right_wheel_vel]: + self.node.sim.wait_while_is_moving(actuator) # Simulate wait until point.time_from_start # self._wait_until( diff --git a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py index 1a83afe9..33d532d6 100755 --- a/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py +++ b/stretch_simulation/stretch_mujoco_driver/stretch_mujoco_driver.py @@ -80,6 +80,8 @@ DEFAULT_ACTION_SERVER_HZ = 30.0 DEFAULT_JOINT_STATE_HZ = 30.0 DEFAULT_SIM_TOOL = "eoa_wrist_dw3_tool_sg3" + + class StretchMujocoDriver(Node): def __init__(self): @@ -99,7 +101,7 @@ def __init__(self): use_robocasa = self.get_parameter("use_robocasa").value if use_robocasa: - robocasa_task:str|None = self.get_parameter("robocasa_task").value + robocasa_task: str | None = self.get_parameter("robocasa_task").value robocasa_layout = self.get_parameter("robocasa_layout").value robocasa_style = self.get_parameter("robocasa_style").value @@ -237,7 +239,7 @@ def set_robot_streaming_position_callback(self, msg): def move_to_position(self, qpos): try: - Idx: SE3_dw3_sg3_Idx = get_Idx(DEFAULT_SIM_TOOL) # type: ignore + Idx: SE3_dw3_sg3_Idx = get_Idx(DEFAULT_SIM_TOOL) # type: ignore if len(qpos) != Idx.num_joints: self.get_logger().error( @@ -267,6 +269,32 @@ def move_to_position(self, qpos): pos = self.gripper_conversion.finger_to_robotis(qpos[Idx.GRIPPER]) self.sim.move_to(Actuators.gripper, pos) + for actuator in [ + Actuators.arm, + Actuators.lift, + Actuators.wrist_pitch, + Actuators.wrist_roll, + Actuators.wrist_yaw, + Actuators.head_pan, + Actuators.head_tilt, + Actuators.gripper, + ]: + succeeded = self.sim.wait_until_at_setpoint(actuator) + if not succeeded: + raise Exception( + f"{actuator} failed to move to {self.sim.data_proxies.get_command().move_to[actuator.name]}" + ) + + for actuator in [ + Actuators.base_translate, + Actuators.base_rotate, + ]: + succeeded = self.sim.wait_while_is_moving(actuator) + if not succeeded: + raise Exception( + f"{actuator} failed to move to {self.sim.data_proxies.get_command().move_to[actuator.name]}" + ) + self.get_logger().info(f"Moved to position qpos: {qpos}") except Exception as e: self.get_logger().error("Failed to move to position: {0}".format(e)) @@ -294,7 +322,7 @@ def command_mobile_base_velocity_and_publish_state(self): self.sim.set_base_velocity( self.linear_velocity_mps, self.angular_velocity_radps ) - elif time_since_last_twist < Duration(seconds=self.timeout_s + 1.0): #type: ignore + elif time_since_last_twist < Duration(seconds=self.timeout_s + 1.0): # type: ignore # self.sim.set_base_velocity(0.0, 0.0) self.sim.move_by(Actuators.base_translate, 0.0) else: @@ -342,9 +370,11 @@ def command_mobile_base_velocity_and_publish_state(self): t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] self.tf_broadcaster.sendTransform(t) - + # This is important, otherwise all the joints are not transformed correctly. The alternative is to broadcast a static_transform, but that doesn't help if another node is trying to lookup transforms. - self.tf_buffer.wait_for_transform_async("base_link", "link_lift", rclpyTime.Time(seconds=0)) + self.tf_buffer.wait_for_transform_async( + "base_link", "link_lift", rclpyTime.Time(seconds=0) + ) b = TransformStamped() b.header.stamp = current_time @@ -956,7 +986,7 @@ def parameter_callback(self, parameters: list[Parameter]) -> SetParametersResult if parameter.name == "default_goal_timeout_s": self.default_goal_timeout_s = parameter.value or DEFAULT_GOAL_TIMEOUT self.default_goal_timeout_duration = Duration( - seconds=self.default_goal_timeout_s #type: ignore + seconds=self.default_goal_timeout_s # type: ignore ) self.get_logger().info( f"Set default_goal_timeout_s to {self.default_goal_timeout_s}" @@ -1051,11 +1081,9 @@ def ros_setup(self): self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) self.tf_static_broadcaster = StaticTransformBroadcaster(self) - self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) - stretch_core_path = get_package_share_path("stretch_core") self.declare_parameter( "controller_calibration_file", @@ -1217,7 +1245,9 @@ def ros_setup(self): ) self.declare_parameter("rate", DEFAULT_JOINT_STATE_HZ) - self.joint_state_rate: float = self.get_parameter("rate").value or DEFAULT_JOINT_STATE_HZ + self.joint_state_rate: float = ( + self.get_parameter("rate").value or DEFAULT_JOINT_STATE_HZ + ) self.declare_parameter( "timeout", @@ -1228,7 +1258,7 @@ def ros_setup(self): ), ) self.timeout_s = self.get_parameter("timeout").value or DEFAULT_TIMEOUT - self.timeout = Duration(seconds=self.timeout_s) #type: ignore + self.timeout = Duration(seconds=self.timeout_s) # type: ignore self.declare_parameter( "default_goal_timeout_s", DEFAULT_GOAL_TIMEOUT, @@ -1237,9 +1267,11 @@ def ros_setup(self): description="Default timeout (sec) for goal execution", ), ) - self.default_goal_timeout_s:float = self.get_parameter("default_goal_timeout_s").value or DEFAULT_GOAL_TIMEOUT + self.default_goal_timeout_s: float = ( + self.get_parameter("default_goal_timeout_s").value or DEFAULT_GOAL_TIMEOUT + ) self.default_goal_timeout_duration = Duration( - seconds=self.default_goal_timeout_s #type: ignore + seconds=self.default_goal_timeout_s # type: ignore ) self.get_logger().info(f"rate = {self.joint_state_rate} Hz") self.get_logger().info(f"twist timeout = {self.timeout_s} s") @@ -1346,9 +1378,9 @@ def ros_setup(self): # start action server for joint trajectories self.declare_parameter("fail_out_of_range_goal", False) - self.fail_out_of_range_goal = bool(self.get_parameter( - "fail_out_of_range_goal" - ).value) + self.fail_out_of_range_goal = bool( + self.get_parameter("fail_out_of_range_goal").value + ) self.declare_parameter( "fail_if_motor_initial_point_is_not_trajectory_first_point", True @@ -1360,7 +1392,9 @@ def ros_setup(self): ) self.declare_parameter("action_server_rate", DEFAULT_ACTION_SERVER_HZ) - self.action_server_rate: float = self.get_parameter("action_server_rate").value or DEFAULT_ACTION_SERVER_HZ + self.action_server_rate: float = ( + self.get_parameter("action_server_rate").value or DEFAULT_ACTION_SERVER_HZ + ) self.joint_trajectory_action = JointTrajectoryAction( self, self.action_server_rate @@ -1379,7 +1413,7 @@ def ros_setup(self): # start loop to command the mobile base velocity, publish # odometry, and publish joint states - timer_period:float = 1.0 / self.joint_state_rate + timer_period: float = 1.0 / self.joint_state_rate self.timer = self.create_timer( timer_period, self.command_mobile_base_velocity_and_publish_state, @@ -1593,7 +1627,6 @@ def get_camera_frame(camera: StretchCameras): raise NotImplementedError(f"Camera {camera} frame is not implemented") - def main(): rclpy.init() From aa643cb1fdf38a0f3b406516ac70ffb2691adb15 Mon Sep 17 00:00:00 2001 From: Shehab Attia Date: Thu, 1 May 2025 16:04:16 -0700 Subject: [PATCH 14/14] Added instructions to add a .env file to stretch_web_teleop. Added export MUJOCO_GL=egl to tell mujoco to use the gpu --- stretch_simulation/README.md | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/stretch_simulation/README.md b/stretch_simulation/README.md index be534490..a3ff92bf 100644 --- a/stretch_simulation/README.md +++ b/stretch_simulation/README.md @@ -27,6 +27,7 @@ ros2 launch stretch_nav2 online_async_launch.py use_sim_time:=true ros2 launch stretch_nav2 navigation.launch.py use_slam:=true use_sim_time:=true use_rviz:=true teleop_type:=none # Terminal 2: Stretch Mujoco Driver +export MUJOCO_GL=egl # On Ubuntu, tell Mujoco to use the GPU ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=true mode:=navigation # Terminal 3: Keyboard Teleop @@ -99,7 +100,7 @@ Use the following commands to start Stretch Mujoco with Web Teleop: parallel_terminal="gnome-terminal --tab -- /bin/bash -c " # or "xterm -e" # Terminal 1 -$parallel_terminal "ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true map_yaml:=${HELLO_FLEET_PATH}/maps/gshaped_modern1_robocasa.yaml" & +$parallel_terminal "MUJOCO_GL=egl ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=false mode:=position robocasa_layout:='G-shaped' robocasa_style:=Modern_1 use_rviz:=false use_cameras:=true map:=${HELLO_FLEET_PATH}/maps/gshaped_modern1_robocasa.yaml" & # Terminal 2 $parallel_terminal "ros2 launch stretch_simulation stretch_simulation_web_interface.launch.py" & @@ -108,7 +109,7 @@ $parallel_terminal "ros2 launch stretch_simulation stretch_simulation_web_interf $parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; npm run localstorage" & # Terminal 4 -$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; sudo keyfile="server.key" certfile="server.crt" node ./server.js" & +$parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; sudo node ./server.js" & # Terminal 4 $parallel_terminal "cd ~/ament_ws/src/stretch_web_teleop; node start_robot_browser.js" & @@ -259,6 +260,10 @@ sudo npx playwright install-deps # Create a certificate for SSL/HTTPS: openssl req -new -x509 -nodes -out certificates/server.crt -keyout certificates/server.key + +touch .env +echo certfile=server.crt >> .env +echo keyfile=server.key >> .env ``` Some more steps to get IK for gripper working: