AndrejOrsula
commented
2 years ago Hello,
I have never tried Hybrid Planning, so I am not aware of its peculiarities. Therefore, I am unable to help with this issue at the moment.
If you are able to control the robot with pymoveit2.MoveIt2 in the same script, then the issue is most likely with your configuration of moveit_hybrid_planning. If it is not possible to use pymoveit2.MoveIt2 with moveit_ros_move_group, then I would try looking more closely at the controller manager and spawning of controllers.
First, you could also test it with fake control (in RViz2) to rule out any issue with gz_ros2_control.
marc-wittwer
I tried to add the hybrid planning capability in the launchfile
ex_py_follow_target.launch.py. However, the robot successfully plans but does not execute.The hybrid planner nodes seem to spawn correctly. Afterwards I send a hybrid planning request to the ROS hybrid planner action server.
Toggle for launch file
``` #!/usr/bin/env -S ros2 launch """Configure and setup move group for planning with MoveIt 2""" from os import path from typing import List import yaml from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.conditions import IfCondition from launch.substitutions import ( Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, PythonExpression, ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from launch_ros.descriptions import ComposableNode from launch_ros.actions import ComposableNodeContainer def generate_launch_description(): # Declare all launch arguments declared_arguments = generate_declared_arguments() # Get substitution for all arguments description_package = LaunchConfiguration("description_package") description_filepath = LaunchConfiguration("description_filepath") moveit_config_package = "panda_moveit_config" name = LaunchConfiguration("name") prefix = LaunchConfiguration("prefix") gripper = LaunchConfiguration("gripper") collision_arm = LaunchConfiguration("collision_arm") collision_gripper = LaunchConfiguration("collision_gripper") safety_limits = LaunchConfiguration("safety_limits") safety_position_margin = LaunchConfiguration("safety_position_margin") safety_k_position = LaunchConfiguration("safety_k_position") safety_k_velocity = LaunchConfiguration("safety_k_velocity") ros2_control = LaunchConfiguration("ros2_control") ros2_control_plugin = LaunchConfiguration("ros2_control_plugin") ros2_control_command_interface = LaunchConfiguration( "ros2_control_command_interface" ) gazebo_preserve_fixed_joint = LaunchConfiguration( "gazebo_preserve_fixed_joint") enable_servo = LaunchConfiguration("enable_servo") enable_rviz = LaunchConfiguration("enable_rviz") rviz_config = LaunchConfiguration("rviz_config") use_sim_time = LaunchConfiguration("use_sim_time") log_level = LaunchConfiguration("log_level") # URDF _robot_description_xml = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [FindPackageShare(description_package), description_filepath] ), " ", "name:=", name, " ", "prefix:=", prefix, " ", "gripper:=", gripper, " ", "collision_arm:=", collision_arm, " ", "collision_gripper:=", collision_gripper, " ", "safety_limits:=", safety_limits, " ", "safety_position_margin:=", safety_position_margin, " ", "safety_k_position:=", safety_k_position, " ", "safety_k_velocity:=", safety_k_velocity, " ", "ros2_control:=", ros2_control, " ", "ros2_control_plugin:=", ros2_control_plugin, " ", "ros2_control_command_interface:=", ros2_control_command_interface, " ", "gazebo_preserve_fixed_joint:=", gazebo_preserve_fixed_joint, ] ) robot_description = {"robot_description": _robot_description_xml} # SRDF _robot_description_semantic_xml = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( [ FindPackageShare(moveit_config_package), "srdf", "panda.srdf.xacro", ] ), " ", "name:=", name, " ", "prefix:=", prefix, ] ) robot_description_semantic = { "robot_description_semantic": _robot_description_semantic_xml } # Kinematics kinematics = load_yaml( moveit_config_package, path.join("config", "kinematics.yaml") ) # Joint limits joint_limits = { "robot_description_planning": load_yaml( moveit_config_package, path.join("config", "joint_limits.yaml") ) } # Servo servo_params = { "moveit_servo": load_yaml( moveit_config_package, path.join("config", "servo.yaml") ) } servo_params["moveit_servo"].update({"use_gazebo": use_sim_time}) # Planning pipeline planning_pipeline = { "planning_pipelines": ["ompl"], "default_planning_pipeline": "ompl", "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved # "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints", "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints default_planner_request_adapters/SetMaxCartesianEndEffectorSpeed", # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled "start_state_max_bounds_error": 0.31416, }, } _ompl_yaml = load_yaml( moveit_config_package, path.join("config", "ompl_planning.yaml") ) planning_pipeline["ompl"].update(_ompl_yaml) # Planning scene planning_scene_monitor_parameters = { "publish_planning_scene": True, "publish_geometry_updates": True, "publish_state_updates": True, "publish_transforms_updates": True, } # MoveIt controller manager moveit_controller_manager_yaml = load_yaml( moveit_config_package, path.join( "config", "moveit_controller_manager.yaml") ) moveit_controller_manager = { "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", "moveit_simple_controller_manager": moveit_controller_manager_yaml, } # Trajectory execution trajectory_execution = { "allow_trajectory_execution": True, "moveit_manage_controllers": False, "trajectory_execution.allowed_execution_duration_scaling": 1.2, "trajectory_execution.allowed_goal_duration_margin": 0.5, "trajectory_execution.allowed_start_tolerance": 0.01, } # Controller parameters declared_arguments.append( DeclareLaunchArgument( "__controller_parameters_basename", default_value=["controllers_", ros2_control_command_interface, ".yaml"], ) ) controller_parameters = PathJoinSubstitution( [ FindPackageShare(moveit_config_package), "config", LaunchConfiguration("__controller_parameters_basename"), ] ) # List of nodes to be launched nodes = [ # robot_state_publisher Node( package="robot_state_publisher", executable="robot_state_publisher", output="log", arguments=["--ros-args", "--log-level", log_level], parameters=[ robot_description, { "publish_frequency": 50.0, "frame_prefix": "", "use_sim_time": use_sim_time, }, ], ), # ros2_control_node (only for fake controller) Node( package="controller_manager", executable="ros2_control_node", output="log", arguments=["--ros-args", "--log-level", log_level], parameters=[ robot_description, controller_parameters, {"use_sim_time": use_sim_time}, ], condition=( IfCondition( PythonExpression( [ "'", ros2_control_plugin, "'", " == ", "'fake'", ] ) ) ), ), # move_group (with execution) Node( package="moveit_ros_move_group", executable="move_group", output="log", # arguments=["--ros-args", "--log-level", log_level], arguments=["--ros-args", "--log-level", 'info'], parameters=[ robot_description, robot_description_semantic, kinematics, joint_limits, planning_pipeline, trajectory_execution, planning_scene_monitor_parameters, moveit_controller_manager, {"use_sim_time": use_sim_time}, ], ), # move_servo Node( package="moveit_servo", executable="servo_node_main", output="log", arguments=["--ros-args", "--log-level", log_level], parameters=[ robot_description, robot_description_semantic, kinematics, joint_limits, planning_pipeline, trajectory_execution, planning_scene_monitor_parameters, servo_params, {"use_sim_time": use_sim_time}, ], condition=IfCondition(enable_servo), ), # rviz2 Node( package="rviz2", executable="rviz2", output="log", arguments=[ "--display-config", rviz_config, "--ros-args", "--log-level", log_level, ], parameters=[ robot_description, robot_description_semantic, kinematics, planning_pipeline, joint_limits, {"use_sim_time": use_sim_time}, ], condition=IfCondition(enable_rviz), ), ] # Add nodes for loading controllers for controller in moveit_controller_manager_yaml["controller_names"] + [ "joint_state_broadcaster" ]: nodes.append( # controller_manager_spawner Node( package="controller_manager", executable="spawner", output="log", arguments=[controller, "--ros-args", "--log-level", log_level], parameters=[{"use_sim_time": use_sim_time}], ), ) # Any parameters that are unique to your plugins go here common_hybrid_planning_param = load_yaml( moveit_config_package, path.join("config", "common_hybrid_planning_params.yaml") ) global_planner_param = load_yaml( moveit_config_package, path.join("config", "global_planner.yaml") ) local_planner_param = load_yaml( moveit_config_package, path.join("config", "local_planner.yaml") ) hybrid_planning_manager_param = load_yaml( moveit_config_package, path.join("config", "hybrid_planning_manager.yaml") ) # The global planner uses the typical OMPL parameters planning_pipelines_config = { "ompl": { "planning_plugin": "ompl_interface/OMPLPlanner", "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", "start_state_max_bounds_error": 0.1, } } ompl_planning_yaml = load_yaml( moveit_config_package, path.join("config", "ompl_planning_hybrid.yaml") ) planning_pipelines_config["ompl"].update(ompl_planning_yaml) moveit_simple_controllers_yaml = load_yaml( moveit_config_package, path.join("config", "moveit_controller_hybrid.yaml") ) moveit_controllers = { "moveit_simple_controller_manager": moveit_simple_controllers_yaml, "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", } # Generate launch description with multiple components container = ComposableNodeContainer( name="hybrid_planning_container", namespace="/", package="rclcpp_components", executable="component_container", composable_node_descriptions=[ ComposableNode( package="moveit_hybrid_planning", # plugin="moveit_hybrid_planning::GlobalPlannerComponent", plugin="moveit::hybrid_planning::GlobalPlannerComponent", name="global_planner", parameters=[ common_hybrid_planning_param, global_planner_param, robot_description, robot_description_semantic, kinematics, # planning_pipeline, planning_pipelines_config, # moveit_controller_manager, # moveit_controllers, {"use_sim_time": use_sim_time}, ], ), ComposableNode( package="moveit_hybrid_planning", # plugin="moveit_hybrid_planning::LocalPlannerComponent", plugin="moveit::hybrid_planning::LocalPlannerComponent", name="local_planner", parameters=[ common_hybrid_planning_param, local_planner_param, robot_description, robot_description_semantic, kinematics, {"use_sim_time": use_sim_time}, ], ), ComposableNode( package="moveit_hybrid_planning", # plugin="moveit_hybrid_planning::HybridPlanningManager", plugin="moveit::hybrid_planning::HybridPlanningManager", name="hybrid_planning_manager", parameters=[ common_hybrid_planning_param, hybrid_planning_manager_param, {"use_sim_time": use_sim_time}, ], ), ], output="screen", ) return LaunchDescription(declared_arguments +[container]+ nodes) def load_yaml(package_name: str, file_path: str): """ Load yaml configuration based on package name and file path relative to its share. """ package_path = get_package_share_directory(package_name) absolute_file_path = path.join(package_path, file_path) return parse_yaml(absolute_file_path) def parse_yaml(absolute_file_path: str): """ Parse yaml from file, given its absolute file path. """ try: with open(absolute_file_path, "r") as file: return yaml.safe_load(file) except EnvironmentError: return None def generate_declared_arguments() -> List[DeclareLaunchArgument]: """ Generate list of all launch arguments that are declared for this launch script. """ return [ # Locations of robot resources DeclareLaunchArgument( "description_package", default_value="panda_description", description="Custom package with robot description.", ), DeclareLaunchArgument( "description_filepath", default_value=path.join("urdf", "panda.urdf.xacro"), description="Path to xacro or URDF description of the robot, relative to share of `description_package`.", ), # Naming of the robot DeclareLaunchArgument( "name", default_value="panda", description="Name of the robot.", ), DeclareLaunchArgument( "prefix", default_value="panda_", description="Prefix for all robot entities. If modified, then joint names in the configuration of controllers must also be updated.", ), # Gripper DeclareLaunchArgument( "gripper", default_value="true", description="Flag to enable default gripper.", ), # Collision geometry DeclareLaunchArgument( "collision_arm", default_value="true", description="Flag to enable collision geometry for manipulator's arm.", ), DeclareLaunchArgument( "collision_gripper", default_value="true", description="Flag to enable collision geometry for manipulator's gripper (hand and fingers).", ), # Safety controller DeclareLaunchArgument( "safety_limits", default_value="true", description="Flag to enable safety limits controllers on manipulator joints.", ), DeclareLaunchArgument( "safety_position_margin", default_value="0.15", description="Lower and upper margin for position limits of all safety controllers.", ), DeclareLaunchArgument( "safety_k_position", default_value="100.0", description="Parametric k-position factor of all safety controllers.", ), DeclareLaunchArgument( "safety_k_velocity", default_value="40.0", description="Parametric k-velocity factor of all safety controllers.", ), # ROS 2 control DeclareLaunchArgument( "ros2_control", default_value="true", description="Flag to enable ros2 controllers for manipulator.", ), DeclareLaunchArgument( "ros2_control_plugin", default_value="ign", description="The ros2_control plugin that should be loaded for the manipulator ('fake', 'ign', 'real' or custom).", ), DeclareLaunchArgument( "ros2_control_command_interface", default_value="effort", description="The output control command interface provided by ros2_control ('position', 'velocity', 'effort' or certain combinations 'position,velocity').", ), # Gazebo DeclareLaunchArgument( "gazebo_preserve_fixed_joint", default_value="false", description="Flag to preserve fixed joints and prevent lumping when generating SDF for Gazebo.", ), # Servo DeclareLaunchArgument( "enable_servo", default_value="true", description="Flag to enable MoveIt2 Servo for manipulator.", ), # Miscellaneous DeclareLaunchArgument( "enable_rviz", default_value="true", description="Flag to enable RViz2." ), DeclareLaunchArgument( "rviz_config", default_value=path.join( get_package_share_directory("panda_moveit_config"), "rviz", "moveit.rviz", ), description="Path to configuration for RViz2.", ), DeclareLaunchArgument( "use_sim_time", default_value="false", description="If true, use simulated clock.", ), DeclareLaunchArgument( "log_level", default_value="warn", description="The level of logging that is applied to all ROS 2 nodes launched by this script.", ), ] ```Toggle for message publisher script
``` import rclpy from rclpy.action import ActionClient from rclpy.node import Node from std_msgs.msg import String from rclpy.callback_groups import ReentrantCallbackGroup from global_state_msgs.msg import CollisionPlane as CollisionPlaneMessage from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Point, Vector3 from moveit_msgs.msg import MotionPlanRequest, MotionSequenceItem, MotionSequenceRequest, PlanningOptions, \ PositionConstraint, OrientationConstraint, Constraints from moveit_msgs.action import HybridPlanner from shape_msgs.msg import SolidPrimitive from rclpy.qos import ( QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile, QoSReliabilityPolicy, ) from pymoveit2 import MoveIt2, MoveIt2Gripper from pymoveit2.robots import panda class MinimalPublisher(Node): def __init__(self): super().__init__('minimal_publisher') topic = "/collision_plane" # self.publisher_ = self.create_publisher(String, topic, 10) self.publisher_ = self.create_publisher(CollisionPlaneMessage, topic, 10) timer_period = 4 # seconds self._callback_group = ReentrantCallbackGroup() self._moveit2 = MoveIt2( node=self, joint_names=panda.joint_names(), base_link_name=panda.base_link_name(), end_effector_name=panda.end_effector_name(), group_name=panda.MOVE_GROUP_ARM, execute_via_moveit=True, callback_group=self._callback_group, ) # hybrid_planning/run_hybrid_planning hybrid_planner_action = "run_hybrid_planning" self.__hybrid_planner_action_client = ActionClient( node=self, action_type=HybridPlanner, action_name=hybrid_planner_action, goal_service_qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_LAST, depth=1, ), result_service_qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_LAST, depth=5, ), cancel_service_qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.RELIABLE, history=QoSHistoryPolicy.KEEP_LAST, depth=5, ), feedback_sub_qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=1, ), status_sub_qos_profile=QoSProfile( durability=QoSDurabilityPolicy.VOLATILE, reliability=QoSReliabilityPolicy.BEST_EFFORT, history=QoSHistoryPolicy.KEEP_LAST, depth=1, ), callback_group=self._callback_group, ) self.timer = self.create_timer(timer_period, self.timer_callback) self.i = 0 def timer_callback(self): publish_messages_limit = 2 if (publish_messages_limit - 1 < self.i): exit(0) msg = HybridPlanner.Goal() goal_motion_request = MotionPlanRequest() goal_motion_request.goal_constraints = [Constraints()] # goal_motion_request.group_name = 'panda_arm' # 'arm' goal_motion_request.group_name = 'arm' # 'arm' goal_motion_request.num_planning_attempts = 10 goal_motion_request.allowed_planning_time = 0.5 goal_motion_request.workspace_parameters.header.frame_id = 'world' goal_motion_request.workspace_parameters.header.stamp = self.get_clock().now().to_msg() goal_motion_request.workspace_parameters.min_corner.x = -2.0 goal_motion_request.workspace_parameters.min_corner.y = -2.0 goal_motion_request.workspace_parameters.min_corner.z = -2.0 goal_motion_request.workspace_parameters.max_corner.x = 2.0 goal_motion_request.workspace_parameters.max_corner.y = 2.0 goal_motion_request.workspace_parameters.max_corner.z = 2.0 goal_motion_request.planner_id = "ompl" goal_motion_request.pipeline_id = "ompl" goal_motion_request.start_state.joint_state = self._moveit2.joint_state # # goal_motion_request.request.max_velocity_scaling_factor = 0.6 # goal_motion_request.request.max_acceleration_scaling_factor = 0.6 # # goal_motion_request.planning_options = PlanningOptions() # goal_motion_request.planning_options.replan = True # goal_motion_request.planning_options.replan_attempts = 50 # # # # The amount of time to wait in between replanning attempts (in seconds) # # TODO: Check if this can be zero # goal_motion_request.planning_options.replan_delay = 0.0 # # # TODO: How to get the latest configuration? # # move_action_goal.request.start_state.joint_state = self._moveit2.joint_state # # # set goal constraints goal_constraint = PositionConstraint() goal_constraint.header.frame_id = 'world' # goal_constraint.link_name = 'panda_link8' # goal_constraint.link_name = 'panda_hand_tcp' goal_constraint.link_name = 'panda_hand' # Define target position goal_constraint.constraint_region.primitive_poses.append(Pose()) pointA = Point(x=0.3 * (self.i / 2 + 1), y=0.3, z=0.3) goal_constraint.constraint_region.primitive_poses[0].position = pointA # Define goal region as a sphere with radius equal to the tolerance goal_constraint.constraint_region.primitives.append(SolidPrimitive()) goal_constraint.constraint_region.primitives[0].type = 2 # Sphere goal_constraint.constraint_region.primitives[0].dimensions = [0.001] # Set weight of the constraint goal_constraint.weight = 1.0 # goal_motion_request.goal_constraints[-1].position_constraints.append( goal_constraint) # # Add goal orientation goal_orientation = OrientationConstraint() quat = Quaternion() quat.x = -1.0 quat.y = 0.0 quat.z = 0.0 quat.w = 0.0 # goal_orientation.orientation = quat goal_orientation.link_name = 'panda_hand' # goal_orientation.link_name = 'panda_hand_tcp' goal_orientation.header.frame_id = 'world' goal_orientation.weight = 1.0 goal_orientation.absolute_x_axis_tolerance = 0.109 goal_orientation.absolute_y_axis_tolerance = 0.109 goal_orientation.absolute_z_axis_tolerance = 0.109 # goal_motion_request.goal_constraints[-1].orientation_constraints.append( goal_orientation) motion_sequence = MotionSequenceItem() motion_sequence.req = goal_motion_request motion_sequence.blend_radius = 0.0 motion_sequence_request = MotionSequenceRequest() motion_sequence_request.items = [motion_sequence] msg.motion_sequence = motion_sequence_request msg.planning_group = 'arm' # 'panda_arm' result = self.__hybrid_planner_action_client.send_goal(msg) print(result) self.i += 1 def main(args=None): rclpy.init(args=args) minimal_publisher = MinimalPublisher() rclpy.spin(minimal_publisher) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) minimal_publisher.destroy_node() rclpy.shutdown() if __name__ == '__main__': main() ```Do you have an idea why the robot is only planning and not moving? Is the topic for joint_trajectory commands wrongly set?
For more logs see my other post: https://github.com/ros-planning/moveit2/issues/1536