Closed Nicogene closed 11 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
We await the f2f meeting w/ Marco and Ettore
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
Here the PR that fixes the problem:
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:
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
Awesome! 🥇
Objective reached, well done 🎖️
Closing in favour of #14
We would like to use moveIT for cartesian control w/ just one arm.