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 – Start playing around for controlling simulated robot via MoveIT #3

Closed Nicogene closed 1 year ago

Nicogene commented 1 year ago

We would like to start playing around for controlling simulated robot via MoveIT, but first we need to have a chat w/ Ettore to understand how to start.

We would like to move a part.

pattacini commented 1 year ago

/remind tomorrow @Nicogene, @martinaxgloria please finalize this task.

octo-reminder[bot] commented 1 year ago

⏰ Reminder Friday, June 30, 2023 10:00 AM (GMT+02:00)

@Nicogene, @martinaxgloria please finalize this task.

octo-reminder[bot] commented 1 year ago

🔔 @pattacini

@Nicogene, @martinaxgloria please finalize this task.

martinaxgloria commented 1 year ago

Yesterday I started working on this activity. Before implementing a ros2_controller, I tried to set a goal state (in orange in the figure below) from rviz by means of MoveIt MotionPlanning plugin:

image

The plan step finishes without any problem, while if I try to run execute, I retrieve a failure. From the log I have:

[move_group-1] [INFO] [1688393243.421194283] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: l_arm_controller started execution
[move_group-1] [INFO] [1688393243.421240283] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[move_group-1] [WARN] [1688393246.880804718] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: waitForExecution timed out
[move_group-1] [WARN] [1688393246.880888118] [moveit_ros.trajectory_execution_manager]: Controller handle l_arm_controller reports status RUNNING
[move_group-1] [INFO] [1688393246.880998818] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status RUNNING ...
[move_group-1] [INFO] [1688393246.881168218] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution completed: RUNNING
[ros2-7] [rviz2-1] [INFO] [1688393246.882064918] [move_group_interface]: Execute request aborted
[ros2-7] [rviz2-1] [ERROR] [1688393246.920994519] [move_group_interface]: MoveGroupInterface::execute() failed or timeout reached
[ros2-7] [rviz2-1] [WARN] [1688393247.579587945] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Maybe failed to update robot state, time diff: 1688393112.786s

So far, I was not able to find any solution to this error, but from the last warning, it seems that after setting the new pose I'm not able to update the joints' position accordingly. I will investigate the problem more in-depth, maybe I can try to set the goal state from code by running a specific node that does it.

cc @pattacini @Nicogene

martinaxgloria commented 1 year ago

I pushed here the first attempt to create a ros2_control hardware_interface in order to try to obtain msgs from the control board and send position msgs to the joints in the gazebo environment. This is a WIP

cc @Nicogene @pattacini

Nicogene commented 1 year ago

We should speak w/ @traversaro and see how this work can fit this ticket: https://github.com/icub-tech-iit/tickets/issues/218

pattacini commented 1 year ago

Something relevant:

cc @martinaxgloria @Nicogene

martinaxgloria commented 1 year ago

Thank you for the ref @pattacini.

Unfortunately, it is something I already investigated and I didn't find there any solution to my problem. But, after a debug session of my ros2_control code, I found out that by manually adding a service GetPosition (like in https://github.com/robotology/yarp-devices-ros2/pull/53/commits/e5c02de085136852c41cf3ad3a26d029139497ca), I'm not able to reach this service. In fact, it remains stacked in: https://github.com/icub-tech-iit/study-moveit/blob/55b0b8cc65c2af6ebb320a8236d70f5f3d512302/icub_controller/src/icub_controller.cpp#L117

causing the "controller" to not be inizialized at all. For the other services currently integrated into yarp-devices-ros2, instead, I don't have this problem, I'm able to get the joints' names for example using the getJointsNames srv.

martinaxgloria commented 1 year ago

I'm not able to reach this service.

Ok, this was a stupid forgetfulness that I fixed.

martinaxgloria commented 1 year ago

Today @Nicogene and I had a call to better understand which is the current state of this activity and how to handle the errors I have. In particular, so far I implemented a ros2_control named l_arm_controller of type hardware_interface that has two state interfaces (i.e. read-only interfaces) position and velocity, and one command interface (i.e. read and write) position. When I launch the node that manages this controller, I retrieve a message that confirms that the controller is loaded, configured and activated but from the error (see below) seems that it remains in an unconfigured status:

[spawner-8] [INFO] [1689936166.985513377] [spawner_l_arm_controller]: Loaded l_arm_controller
[ros2_control_node-7] [INFO] [1689936166.988004374] [controller_manager]: Configuring controller 'l_arm_controller'
[ros2_control_node-7] [INFO] [1689936166.988343674] [l_arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-7] [INFO] [1689936166.988428574] [l_arm_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ros2_control_node-7] [INFO] [1689936166.988462874] [l_arm_controller]: Using 'splines' interpolation method.
[ros2_control_node-7] [INFO] [1689936166.990198372] [l_arm_controller]: Controller state will be published at 100.00 Hz.
[ros2_control_node-7] [INFO] [1689936166.994436569] [l_arm_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2_control_node-7] [ERROR] [1689936167.007429657] []: Caught exception in callback for transition 13
[ros2_control_node-7] [ERROR] [1689936167.007501557] []: Original error: double hardware_interface::ReadOnlyHandle::get_value() const failed. value_ptr_ is null.
[ros2_control_node-7] [WARN] [1689936167.007535757] []: Error occurred while doing error handling.
[ros2_control_node-7] [ERROR] [1689936167.007605057] [controller_manager]: After activating, controller 'l_arm_controller' is in state 'unconfigured', expected Active
[spawner-8] [INFO] [1689936167.009266655] [spawner_l_arm_controller]: Configured and activated l_arm_controller

The life_cycle node, which is responsible for managing the nodes, acts in this way:

image

At this point, as @Nicogene suggests, it is necessary to properly debug the code and see where the error occurs before moving forward.

cc @pattacini

martinaxgloria commented 1 year ago

Analyzing the errors reported in the previous comment:

[ros2_control_node-7] [ERROR] [1689936167.007429657] []: Caught exception in callback for transition 13
[ros2_control_node-7] [ERROR] [1689936167.007501557] []: Original error: double hardware_interface::ReadOnlyHandle::get_value() const failed. value_ptr_ is null.
[ros2_control_node-7] [WARN] [1689936167.007535757] []: Error occurred while doing error handling.

I found out the callback referred to the transition 13 is the one from state 1 (inactive) to state 3 (active), that is the execution of onActive callback. After an accurate analysis, I noticed that at the beginning of this activity, I declared a vector of velocities that should be populated with the velocities returned by the hardware/simulation but I didn't initialize it since I was working with positions only. My bad, but I think the log was not so clear in that sense.

Now, solved this problem, I'm again in a situation like the one reported in https://github.com/icub-tech-iit/study-moveit/issues/3#issuecomment-1618400186. I'm still working on it

cc @Nicogene @pattacini

martinaxgloria commented 1 year ago

Just to clarify: the controller I'm trying to make work is of type Joint Trajectory Control that allows executing joint-space trajectories on a group of joints.

Moreover, the controller I've implemented so far works as a Position control. In detail, this controller opens a specific topic called <controller_name>/commands and it's possible to publish on it by passing a message of type std_msgs/msg/Float64MultiArray with the desired positions to be reached to the joints that belong to the joint_group on which the user wants to make motion planning.

https://github.com/icub-tech-iit/study-moveit/assets/114698424/ee6ec3d1-b968-4e22-903b-ce635e40aac0

In this video, for the first movement I set the positions with ros2 topic pub <controller_name>/commands std_msgs/msg/Float64MultiArray "{data: [0.0 0.0 0.0 0.0 0.0 0.0]}" (I passed 6 float since I'm using a single arm made by 6 joints).

martinaxgloria commented 1 year ago

This morning I've finally succeeded in using Motion Planning plugin by MoveIt to move a group of joints (left_arm) in simulation on Gazebo. Here is the video:

https://github.com/icub-tech-iit/study-moveit/assets/114698424/990eb2e5-fe2f-4d60-9156-f943647c9788

Until now, I wasn't able to do it because the controller parameters were not located in the same namespace of the move_group node (which is the main node of MoveIt for the movement execution). After a refactoring of the launch.py file that runs all the needed nodes, I was able to plan and execute the movement.

cc @pattacini @Nicogene

pattacini commented 1 year ago

Great achievement! 🚀

pattacini commented 1 year ago

What we see in the video is the robot following a planned trajectory. What is the control modality used in this case?

It ought to be position-direct and not simple position control. If this is not the case, we can address this point later on.

martinaxgloria commented 1 year ago

By sending a request to the GetControlModes srv in order to retrieve the current control modality of each joint, I've just verified that the joints are in position control:

image

I will add another client to the service SetControlModes in order to be able to switch in position-direct control.

pattacini commented 1 year ago

Super, @martinaxgloria!

Please, jot down below what you deem necessary to deliver a complete API for controlling our robot in MoveIT! This way, we can be precise in planning on next activities.

martinaxgloria commented 1 year ago

Sure, I'm going to do it between today and Monday

martinaxgloria commented 1 year ago

Hi @pattacini, reading carefully the documentation I read that the standard interfaces available are position, velocity, acceleration, and effort. If I understood well, it means that it is possible to implement the controller based on those types of interfaces and switch between them. I have to investigate more in-depth this aspect if we have to work on position-direct control

pattacini commented 1 year ago

position from ROS2 standpoint should be fine. It depends on how we implement the interface from our side, that is whether we go with position (in our terms) or position-direct.

martinaxgloria commented 1 year ago

Hi @pattacini,

about this:

Please, jot down below what you deem necessary to deliver a complete API for controlling our robot in MoveIT! This way, we can be precise in planning on next activities.

All these points concern the use of moveit2 with simulated robots. After finishing the simulation part, I think that we could start thinking about how to implement the communication with the real robot.

I don't know if I'm missing something important, in this case, every suggestion is welcome

pattacini commented 1 year ago

Since I worked with the iCub3 urdf so far, I was wondering if we should develop and include the configuration files for the other robots and/or create other ros packages for them or even change what is done in order to use it with another specific robot.

I reckon we should change our baseline from iCub3 to iCub2.7 and ergoCub, as iCub3 won't be our flagship any longer. Ideally, we should address both iCub2.7 and ergoCub.

All these points concern the use of moveit2 with simulated robots. After finishing the simulation part, I think that we could start thinking about how to implement the communication with the real robot.

👍🏻

martinaxgloria commented 1 year ago

I reckon we should change our baseline from iCub3 to iCub2.7 and ergoCub, as iCub3 won't be our flagship any longer. Ideally, we should address both iCub2.7 and ergoCub.

Ok, I will generate the new configuration files for iCub2.7 and I will do some tests to see if the pipeline works. Regarding ergoCub, I will check if the problem with the visual and collision properties pointing to the same .stl mesh inside the urdf (see https://github.com/icub-tech-iit/fix/issues/1387#issuecomment-1558796164) is still present, but with iCub* we should not have the problem

martinaxgloria commented 1 year ago

Today I worked on:

I reckon we should change our baseline from iCub3 to iCub2.7 and ergoCub, as iCub3 won't be our flagship any longer. Ideally, we should address both iCub2.7 and ergoCub.

In particular, I started generating the configuration files for iCub2_7 but when I tried to import the urdf on Gazebo, I retrieved the following errors:

[WARNING] GazeboYarpIMU : [WRAPPER] group not found in config file, maybe you are using the old version of the ini file, please update icub-gazebo
[WARNING] GazeboYarpIMU : trying to open it with the legacy behaviour device-subdevice
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[ERROR] |yarp.os.YarpPluginSettings| Error while opening /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so:
  /home/martinagloria/robotology-superbuild/build/install/lib/yarp/yarp_inertial.so: undefined symbol: _ZN4yarp2os19AbstractContactable15setCallbackLockEPNS0_5MutexE
[WARNING] |yarp.os.YarpPluginSettings| Cannot load plugin from shared library (yarp_inertial)
[WARNING] |yarp.os.YarpPluginSettings| (yarp_inertial: cannot open shared object file: No such file or directory
yarp_inertial.so: cannot open shared object file: No such file or directory
libyarp_inertial.so: cannot open shared object file: No such file or directory)
[ERROR] |yarp.dev.PolyDriver|inertial| Could not find device <inertial>
[ERROR] GazeboYarpIMU Plugin Load failed: error in opening yarp driver

As suggested by @pattacini, I switched to iCubV2_5 which worked smoothly and I generated a new ros2 pkg with its configuration files in order to use it with MoveIt. I uploaded all these files here and I update the README.md with the correct instruction to use the packages contained in this repo (I have only to change the image with a video of iCub2_5/ergoCub maybe).

I attach here a video to show the two controllers (r_arm and l_arm) working with iCub2_5:

https://github.com/icub-tech-iit/study-moveit/assets/114698424/0dc6d4d5-4d7b-428a-8671-211bc4fb32cb

There are two things to fix:

cc @pattacini @Nicogene

pattacini commented 1 year ago

Super!

Regarding iCubGazeboV2_7, strangely enough, I managed yesterday to make it run in Gitpod. I'll check it out one more time.

martinaxgloria commented 1 year ago

Today I will check again too 👍🏻

pattacini commented 1 year ago

I can confirm that with iCubGazeboV2_7 I'm having the same error, which is not blocking though[^1].

The logs:

[^1]: That's the reason why I didn't spot it in the first place as the simulated robot was actually running ok for my purposes.

pattacini commented 1 year ago

In particular, I started generating the configuration files for iCub2_7 but when I tried to import the urdf on Gazebo, I retrieved the following errors:

See https://github.com/robotology/icub-models-generator/issues/245.

pattacini commented 1 year ago

Closing in favor of a couple of follow-ups:

Well done 🚀