search

YaskawaEurope / ros2-starter-for-yaskawa-robots

6 stars 0 forks source link

Controller 'moto_gp4' failed [...] Final position was outside tolerance [...] 0.0000 deviation #1

Closed TE-2 closed 4 months ago

TE-2 commented 4 months ago

Hello, I have a Yaskawa GP4 in combination with a YRC1000micro. I have adapted the MotoXY package to the GP4 using the robot descriptions from the ros-industrial motoman repo and so far everything is working. I also changed the joint names in the motoros2_config.yaml to - [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] and also adapted this change in the moveit configurations.

I then used hello_moveit.cpp to execute the first trajectories and it worked. My launch file to start the move group node looks like this:

Click to expand ```Python import os from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder import xacro def generate_launch_description(): robot_name_space = "/moto_gp4" #is used for remapping the joint states, yaskawa node: /moto_gp4/yrc_1000_micro motoman_directory = "moveit_resources_moto_gp4_moveit_config" # URDF motoman_robot_description_config = xacro.process_file( os.path.join( get_package_share_directory(motoman_directory), "config", "motoman_gp4.urdf.xacro", ), # mappings={'prefix': '/yaskawa_a1/'}, ) motoman_robot_description = { "robot_description": motoman_robot_description_config.toxml() } moveit_config = ( MoveItConfigsBuilder("moveit_resources_moto_gp4") .robot_description(file_path="config/motoman_gp4.urdf.xacro") .robot_description_semantic(file_path="config/motoman_gp4.srdf") .trajectory_execution(file_path="config/moveit_controllers.yaml") .to_moveit_configs() ) planning_scene_monitor_parameters = { "publish_planning_scene": True, "publish_geometry_updates": True, "publish_state_updates": True, "publish_transforms_updates": True, "publish_robot_description": True, "publish_robot_description_semantic": True, } # Start the actual move_group node/action server # robot run_move_group_nodeX = Node( package="moveit_ros_move_group", executable="move_group", output="screen", parameters=[ moveit_config.to_dict(), planning_scene_monitor_parameters, # {"use_sim_time": True}, ], remappings=[ ('joint_states', f'{robot_name_space}/joint_states'), ] ) # RViz rviz_base = os.path.join( get_package_share_directory( "moveit_resources_moto_moveit_config"), "launch" ) rviz_full_config = os.path.join(rviz_base, "moveit.rviz") rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", arguments=["-d", rviz_full_config], parameters=[ moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.planning_pipelines, moveit_config.robot_description_kinematics, # {"use_sim_time": True}, ], ) ################################# joint_state_publisherX = Node( package='joint_state_publisher', executable='joint_state_publisher', name='joint_state_publisher', parameters=[ { "robot_description": motoman_robot_description, "rate": 43, "source_list": [f'{robot_name_space}/joint_states'], } ], ) robot_state_publisherX = Node( package="robot_state_publisher", executable="robot_state_publisher", name="robot_state_publisher", parameters=[ motoman_robot_description, { "robot_description": motoman_robot_description, "publish_frequency": 43.0, "frame_prefix": "", }, ] ) return LaunchDescription( [ run_move_group_nodeX, joint_state_publisherX, robot_state_publisherX, rviz_node, ] # + load_controllers ) ```

However, despite the trajectory being executed correctly, I always get the following error in the terminal of my hello_moveit.cpp script:

[ERROR] [1712654628.470447936] [move_group_interface]: MoveGroupInterface::execute() failed or timeout reached

When I look at the logs of the terminal of my move_group node, I get the following:

[move_group-1] [INFO] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-1] [INFO] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-1] [INFO] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-1] [INFO] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'pilz_industrial_motion_planner'
[move_group-1] [INFO] [moveit.pilz_industrial_motion_planner.trajectory_generator]: Generating LIN trajectory...
[move_group-1] [INFO] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed successfully.
[move_group-1] [INFO] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-1] [INFO] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-1] [INFO] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-1] [INFO] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-1] [INFO] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-1] [INFO] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to moto_gp4
[move_group-1] [INFO] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: moto_gp4 started execution
[move_group-1] [INFO] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
-[move_group-1] [WARN] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'moto_gp4' failed with error unknown error: Final position was outside tolerance. Check robot safety-limits that could be inhibiting motion. [joint_1: 0.0000 deviation] [joint_2: 0.0000 deviation] [joint_3: 0.0000 deviation] [joint_5: 0.0000 deviation] [joint_6: 0.0000 deviation]**
-[move_group-1] [WARN] [moveit_ros.trajectory_execution_manager]: Controller handle moto_gp4 reports status ABORTED
[move_group-1] [INFO] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...
[move_group-1] [INFO] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: ABORTED

The message indicates that the goal deviations for the individual joints are set to 0.0. I'm not sure about the safety limits, but I think the robot trajectories are not interrupted or interfered with by any limits. So I edited the hello_moveit script a bit and added a goal tolerance before planning and executing the trajectory:

move_group_interface.setGoalTolerance(0.01);

Which I can also validate via the following methods:

move_group_interface.getGoalJointTolerance();
move_group_interface.getGoalPositionTolerance();
move_group_interface.getGoalOrientationTolerance();

However, setting the GoalTolerance had no effect..

I've also looked in the moto_description and moveit_configuration to see if I can adjust anything there, but unfortunately I couldn't find anything that indicated goal tolerances or safety limits.

I have also noticed that I have problems when calling getCurrentJointValuesor getCurrentPose:

Click to expand ``` [INFO] [moveit_ros.current_state_monitor]: Listening to joint states on the topic 'joint_states' [INFO] [moveit_ros.current_state_monitor]: No robot state (joint angle) with current timestamp was received within 1.000000 seconds. Requested time 1712654248.514416, but the last received state has the time 0.000000. Check the clock synchronisation if you are running ROS on multiple machines ```

But I'm not sure if these Problems are related or have any influence on each other.

I'm using version 0.1.2 of motoros2 and the ROS2 distribution Humble.

I would appreciate any helpful input. Thanks

TE-2 commented 4 months ago

Issue has been moved to here: https://github.com/Yaskawa-Global/motoros2/discussions/235