Running MoveIt 2 & ROS 2 on Stretch RE1

[chintujaguar]

Hey, everyone! My name is Chintan Desai. I am pursuing an MS in Robotics Engineering at Worcester Polytechnic Institute while working as an intern at Hello Robot towards porting the codebase to ROS 2, with a present focus on MoveIt 2.

Stretch has a kinematically simple 3 DoF arm (+2 with DexWrist) with a compliant gripper that is suitable for pick and place tasks of varied objects. Its mobile base provides it with additional degrees of freedom that afford it more dexterity and also the ability to navigate freely in its environment. To fully utilize this clever design, we need a planner that can plan for both the arm and the mobile base at the same time. With MoveIt 2 and ROS 2, it is now possible to achieve this, empowering users to plan more complicated and coordinated robot trajectories in difficult and uncertain environments.

Here’s a sneak peek at my progress so far:

It is now possible to interact with Stretch using the Move Group C++ API for MoveIt 2. Here’s a sample node that you can run to plan a simple joint goal:
https://github.com/hello-robot/stretch_ros2/blob/feature/hybrid_planning/stretch_moveit_config/src/movegroup_test.cpp

Feel free to interact with the code in the above demonstration with the setup instructions described in the link below:
https://github.com/hello-robot/stretch_ros2/blob/feature/hybrid_planning/stretch_moveit_config/demo.md

To follow my most current work, you can checkout to the feature/hybrid_planning branch of stretch_ros2:
https://github.com/hello-robot/stretch_ros2/tree/feature/hybrid_planning

My progress so far would not be possible without help from Binit, who laid the groundwork for me to build upon. Special thanks to Charlie and Aaron for their advice and encouragement, and for coming up with this clever robot design! It has been an amazing learning experience so far. I’m excited to share more updates here as I continue to make progress in the coming weeks and over the summer.

I’d love to hear from anyone who is looking to work on Stretch with ROS 2 and MoveIt. Feel free to reach out to me through this post or via email: cdesai@hello-robot.com

Happy tinkering!

Chintan Desai

[Yufei_Wang]

Hi Chintan,

Thanks for the post!

I am now following the instructions here stretch_ros2/demo.md at feature/hybrid_planning · hello-robot/stretch_ros2 · GitHub to install the moveit2 package. I am able to install it and proceed to step 7:

ros2 launch stretch_moveit_config movegroup_moveit2.launch.py

However, I am getting an error msg:

[INFO] [launch]: All log files can be found below /home/stretch-1082/.ros/log/2022-07-25-21-57-36-269002-stretch1082-NUC8i5BEH-34055
[INFO] [launch]: Default logging verbosity is set to INFO
Task exception was never retrieved
future: <Task finished name='Task-2' coro=<LaunchService._process_one_event() done, defined at /opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py:226> exception=SubstitutionFailure('executed command failed. Command: xacro /home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf\nCaptured stderr output: Traceback (most recent call last):\n  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 984, in parse\n    inp = f = open(filename)\nFileNotFoundError: [Errno 2] No such file or directory: \'/home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf\'\n\nDuring handling of the above exception, another exception occurred:\n\nTraceback (most recent call last):\n  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1093, in _process\n    doc = process_file(input_file_name, **opts)\n  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1072, in process_file\n    doc = parse(None, input_file_name)\n  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 988, in parse\n    raise XacroException(e.strerror + ": " + e.filename, exc=e)\nxacro.XacroException: No such file or directory: /home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf [Errno 2] No such file or directory: \'/home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf\'\n\nDuring handling of the above exception, another exception occurred:\n\nTraceback (most recent call last):\n  File "/opt/ros/galactic/bin/xacro", line 33, in <module>\n    xacro.main()\n  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1146, in main\n    _process(input_file_name, vars(opts))\n  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1098, in _process\n    except xml.parsers.expat.ExpatError as e:\nAttributeError: module \'xml\' has no attribute \'parsers\'\n')>
Traceback (most recent call last):
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py", line 228, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py", line 248, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  [Previous line repeated 3 more times]
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/actions/node.py", line 465, in execute
    self._perform_substitutions(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/actions/node.py", line 420, in _perform_substitutions
    evaluated_parameters = evaluate_parameters(context, self.__parameters)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/utilities/evaluate_parameters.py", line 164, in evaluate_parameters
    output_params.append(evaluate_parameter_dict(context, param))
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/utilities/evaluate_parameters.py", line 72, in evaluate_parameter_dict
    evaluated_value = perform_substitutions(context, list(value))
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in perform_substitutions
    return ''.join([context.perform_substitution(sub) for sub in subs])
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/perform_substitutions_impl.py", line 26, in <listcomp>
    return ''.join([context.perform_substitution(sub) for sub in subs])
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_context.py", line 197, in perform_substitution
    return substitution.perform(self)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/substitutions/command.py", line 119, in perform
    raise SubstitutionFailure(on_error_message)
launch.substitutions.substitution_failure.SubstitutionFailure: executed command failed. Command: xacro /home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf
Captured stderr output: Traceback (most recent call last):
  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 984, in parse
    inp = f = open(filename)
FileNotFoundError: [Errno 2] No such file or directory: '/home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf'

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1093, in _process
    doc = process_file(input_file_name, **opts)
  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1072, in process_file
    doc = parse(None, input_file_name)
  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 988, in parse
    raise XacroException(e.strerror + ": " + e.filename, exc=e)
xacro.XacroException: No such file or directory: /home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf [Errno 2] No such file or directory: '/home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf'

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/opt/ros/galactic/bin/xacro", line 33, in <module>
    xacro.main()
  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1146, in main
    _process(input_file_name, vars(opts))
  File "/opt/ros/galactic/lib/python3.8/site-packages/xacro/__init__.py", line 1098, in _process
    except xml.parsers.expat.ExpatError as e:
AttributeError: module 'xml' has no attribute 'parsers'`
which I think essentially says that I am missing the file `/home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/stretch.urdf

I am wondering how should I proceed here?

As an attempt to solve the error, I tried to copy /home/stretch-1082/stretch_user/stretch-re1-1082/exported_urdf/stretch.urdf to the location ‘/home/stretch-1082/ament_ws/install/stretch_description/share/stretch_description/urdf/’, and then I get the following error:

stretch-1082@stretch1082-NUC8i5BEH:~/stretch_user/stretch-re1-1082/exported_urdf$ ros2 launch stretch_moveit_config movegroup_moveit2.launch.py
[INFO] [launch]: All log files can be found below /home/stretch-1082/.ros/log/2022-07-25-22-05-04-312665-stretch1082-NUC8i5BEH-34453
[INFO] [launch]: Default logging verbosity is set to INFO
Task exception was never retrieved
future: <Task finished name='Task-2' coro=<LaunchService._process_one_event() done, defined at /opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py:226> exception=TypeError('Unable to parse the value of parameter robot_description as yaml. If the parameter is meant to be a string, try wrapping it in launch_ros.parameter_descriptions.ParameterValue(value, value_type=str)')>
Traceback (most recent call last):
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/utilities/evaluate_parameters.py", line 79, in evaluate_parameter_dict
    yaml_evaluated_value = yaml.safe_load(evaluated_value)
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/__init__.py", line 125, in safe_load
    return load(stream, SafeLoader)
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/__init__.py", line 81, in load
    return loader.get_single_data()
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/constructor.py", line 49, in get_single_data
    node = self.get_single_node()
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/composer.py", line 36, in get_single_node
    document = self.compose_document()
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/composer.py", line 55, in compose_document
    node = self.compose_node(None, None)
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/composer.py", line 84, in compose_node
    node = self.compose_mapping_node(anchor)
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/composer.py", line 127, in compose_mapping_node
    while not self.check_event(MappingEndEvent):
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/parser.py", line 98, in check_event
    self.current_event = self.state()
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/parser.py", line 428, in parse_block_mapping_key
    if self.check_token(KeyToken):
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/scanner.py", line 115, in check_token
    while self.need_more_tokens():
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/scanner.py", line 152, in need_more_tokens
    self.stale_possible_simple_keys()
  File "/home/stretch-1082/.local/lib/python3.8/site-packages/yaml/scanner.py", line 291, in stale_possible_simple_keys
    raise ScannerError("while scanning a simple key", key.mark,
yaml.scanner.ScannerError: while scanning a simple key
  in "<unicode string>", line 2, column 1:
    <?xml version="1.0" ?>
    ^
could not find expected ':'
  in "<unicode string>", line 3, column 1:
    <!-- =========================== ... 
    ^

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py", line 228, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py", line 248, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  [Previous line repeated 3 more times]
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/actions/node.py", line 465, in execute
    self._perform_substitutions(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/actions/node.py", line 420, in _perform_substitutions
    evaluated_parameters = evaluate_parameters(context, self.__parameters)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/utilities/evaluate_parameters.py", line 164, in evaluate_parameters
    output_params.append(evaluate_parameter_dict(context, param))
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch_ros/utilities/evaluate_parameters.py", line 81, in evaluate_parameter_dict
    raise TypeError(
TypeError: Unable to parse the value of parameter robot_description as yaml. If the parameter is meant to be a string, try wrapping it in launch_ros.parameter_descriptions.ParameterValue(value, value_type=str)

I am not sure how to resolve this either.

Any suggestion would be appreciated!

BTW, I think there is one dependent package MoveIt Task Constructor missing in the demo.md for installing moveit2. I have to manually download and install it from here: MoveIt Task Constructor — MoveIt Documentation: Galactic documentation to resolve the error during installation.

[chintujaguar]

Hello @Yufei_Wang

Welcome to the forum! Thank you for posting the question here and bringing the issue to light.

In the absence of support for stretch_calibration in ROS 2 (porting in progress which will make this step redundant), we need to manually move the calibrated urdf file named stretch.urdf from the stretch_description/urdf directory in stretch_ros (in ROS Melodic) to its counterpart in stretch_ros2 (in ROS 2 Galactic). Doing so from the stretch_user directory is not recommended.

You also need to move the controller_calibration_head.yaml file from the stretch_core/config directory in stretch_ros to its counterpart in stretch_ros2.

Before you proceed, as the last step, build the workspace again from scratch with colcon build.

You’re right, MoveIt Task Constructor is indeed a dependency and should be installed with the rest of the packages listed in stretch_moveit2.repos. I’ll add these steps to the installation guide. Thank you for helping us make the instructions more complete! Let me know how it goes.

Best,
Chintan

[Yufei_Wang]

Hi Chintan,

Thank you very much for the reply! I am able to finish the whole installation process and correctly run moveit2 with Rviz.

However, I notice that currently the planning module seems to only support planning with joint_wrist_yaw, and does not support planning on joint_wrist_pitch nor joint_wrist_roll. See the attached gif for an example. I am wondering if there is anything wrong on my side or do I need to further do something to enable moveit to also plan those two joints?

Thanks!
Yufei

[chintujaguar]

Hi @Yufei_Wang

That’s a great question! Unfortunately, the config files in the hybrid_planning branch were generated for the simple wrist. However, you should still be able to use the DexWrist with a few simple changes.

Thanks to @bshah, we have exactly what you need to run MoveIt 2 with the DexWrist in a separate feature branch. Have a look at the diff here. Making these changes to your setup should allow you to control the wrist pitch and roll joints. Let me know if you have any additional questions.

Best,
Chintan

[chintujaguar]

Hello everyone!

I just wanted to update everyone that the development in the original post has been merged into the main branch of the stretch_ros2 repo. Find the updated links underneath:

1. To follow the most recent setup instructions:
   Robot Install - Stretch Documentation

2. To review the sample node to plan joint goals and more using the MoveGroup C++ API:
   stretch_ros2/demo.md at galactic · hello-robot/stretch_ros2 · GitHub

Happy tinkering,
Chintan Desai