search

icub-tech-iit / xcub-moveit2

Collect the outcomes of our study on the use of MoveIT with our robots
BSD 3-Clause "New" or "Revised" License
3 stars 0 forks source link

ROS2 – Explore the moveIT capabilities in the cartesian space #8

Closed Nicogene closed 11 months ago

Nicogene commented 1 year ago

We would like to use moveIT for cartesian control w/ just one arm.

martinaxgloria commented 12 months ago

Yesterday I started working on this activity. In particular, after the last update meeting on ROS2 activities, it came out the need to control the joints in position direct instead of position. For this purpose, I implemented a function that checks if the joints are in position control and switches them into position direct. I verified that it works through the yarpmotorgui. After that, I tried to move a little bit the left elbow from the slider inside the motion planning plugin and run the plan and execute command to reach the desired position in the joint space (I tried also in the cartesian space but with the same output). The result was that the gazebo node died and no movement was executed:

[ros2_control_node-5] [INFO] [1694070046.978232758] [left_arm_controller]: Received new action goal
[ros2_control_node-5] [INFO] [1694070046.978273886] [left_arm_controller]: Accepted new action goal
[move_group-13] [INFO] [1694070046.978351547] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: left_arm_controller started execution
[move_group-13] [INFO] [1694070046.978362978] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ERROR] [gzserver-1]: process has died [pid 5146, exit code -11, cmd 'gzserver -slibgazebo_ros_init.so -slibgazebo_ros_factory.so -slibgazebo_ros_force_system.so'].
[move_group-13] [WARN] [1694070048.532626646] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: waitForExecution timed out
[move_group-13] [WARN] [1694070048.532663942] [moveit_ros.trajectory_execution_manager]: Controller handle left_arm_controller reports status RUNNING
[move_group-13] [INFO] [1694070048.532670161] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status RUNNING ...
[move_group-13] [INFO] [1694070048.537973044] [moveit_move_group_default_capabilities.move_action_capability]: Solution found but controller failed during execution
[rviz2-12] [INFO] [1694070048.538553259] [move_group_interface]: Plan and Execute request aborted
[rviz2-12] [ERROR] [1694070048.611565454] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached

I started investigated this odd behavior from the yarp side, maybe there's something I'm missing

cc @Nicogene @pattacini

Nicogene commented 11 months ago

We await the f2f meeting w/ Marco and Ettore

martinaxgloria commented 11 months ago

Today, @randaz81 gave me some guidelines to debug the problem relative to control in position direct instead of position and I found the error. It was in the callback to the position direct topic within the ControlBoard_nws_ros2. I am going to open the PR to fix it

cc @Nicogene @pattacini

martinaxgloria commented 11 months ago

Here the PR that fixes the problem:

martinaxgloria commented 11 months ago

After the PR mentioned in the previous comment, I tried to move some joints in the cartesian space with the position direct control. Here the result:

cartesian_path.webm

The log:

[move_group-6] [INFO] [1694760760.578547121] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Received request to compute Cartesian path
[move_group-6] [INFO] [1694760760.578669757] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Attempting to follow 1 waypoints for link 'l_hand' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[move_group-6] [WARN] [1694760760.721653286] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
[move_group-6] [INFO] [1694760760.722983714] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Computed Cartesian path with 24 points (followed 100.000000% of requested trajectory)
[rviz2-5] [INFO] [1694760760.723240617] [moveit_ros_visualization.motion_planning_frame_planning]: Achieved 100.000000 % of Cartesian path
[rviz2-5] [INFO] [1694760760.733037672] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[rviz2-5] [WARN] [1694760760.754443351] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
[rviz2-5] [INFO] [1694760760.755413802] [moveit_ros_visualization.motion_planning_frame_planning]: Computing time stamps SUCCEEDED
[move_group-6] [INFO] [1694760760.755698391] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[rviz2-5] [INFO] [1694760760.755789148] [move_group_interface]: Execute request accepted
[move_group-6] [INFO] [1694760760.755816907] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-6] [INFO] [1694760760.755842520] [moveit.plugins.moveit_simple_controller_manager]: Returned 6 controllers in list
[move_group-6] [INFO] [1694760760.755861122] [moveit.plugins.moveit_simple_controller_manager]: Returned 6 controllers in list
[move_group-6] [INFO] [1694760760.755933212] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-6] [INFO] [1694760760.755998993] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-6] [INFO] [1694760760.756026858] [moveit.plugins.moveit_simple_controller_manager]: Returned 6 controllers in list
[move_group-6] [INFO] [1694760760.756042368] [moveit.plugins.moveit_simple_controller_manager]: Returned 6 controllers in list
[move_group-6] [INFO] [1694760760.756172245] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to left_arm_controller
[ros2_control_node-7] [INFO] [1694760760.756430697] [left_arm_controller]: Received new action goal
[ros2_control_node-7] [INFO] [1694760760.756498429] [left_arm_controller]: Accepted new action goal
[move_group-6] [INFO] [1694760760.756613027] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: left_arm_controller started execution
[move_group-6] [INFO] [1694760760.756627729] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ros2_control_node-7] [INFO] [1694760762.303062298] [left_arm_controller]: Goal reached, success!
[move_group-6] [INFO] [1694760762.306921137] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller 'left_arm_controller' successfully finished
[move_group-6] [INFO] [1694760762.317206857] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ...
[move_group-6] [INFO] [1694760762.317281986] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: SUCCEEDED
[rviz2-5] [INFO] [1694760762.317512701] [move_group_interface]: Execute request success!

cc @pattacini @Nicogene

pattacini commented 11 months ago

Awesome! 🥇

67b2a9ba5e85822f237caae92111e938

Nicogene commented 11 months ago

Objective reached, well done 🎖️

Closing in favour of #14