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 – Better handling two controllers loaded at the same time #4

Closed martinaxgloria closed 1 year ago

martinaxgloria commented 1 year ago

This is a follow-up of #3.

In particular, as showed in https://github.com/icub-tech-iit/study-moveit/issues/3#issuecomment-1660677301, when two ros2_controllers are loaded at the same time (in this case, r_arm_controller and l_arm_controller) and you start planning trajectory with one of them, the group of joints under the control of the other seems to go into idle mode.

We ought to handle the presence of more than one controller in order to avoid this unwanted behavior

pattacini commented 1 year ago

Just put on the bill of PI17-sprtin01-fix, @martinaxgloria. However, you can already start working it out 👍🏻

martinaxgloria commented 1 year ago

Today I started investigating this problem, sending commands to the left arm and seeing the right behavior. This is the result of one of these attempts:

l_arm_control

My first impression was that the arm went into idle mode, but to double-check this hypothesis, I opened the yarpmotorgui and I saw that all the joints were running in position control. Manually switching into idle the right arm, I obtained:

idle

You can see that the two poses are different.

After that, I used the MotionPlanning plugin offered by MoveIt on Rviz and I checked the joints' position from there when they fall down:

no_idle

So, they don't switch to idle (also because if I tried to move the arm into a random pose after it fall down using position control, I'm able to do it, while if the arm was in idle mode, it should be necessary to manually run the joints and then compute the movement) but all the joints go to 0 degrees.

I will do further investigations about this odd behavior

martinaxgloria commented 1 year ago

Ok, I found the cause of the behavior: the joints belonging to two controllers were under the same ros2_control macro inside the configuration files generated by MoveIt Setup Assistant: https://github.com/icub-tech-iit/study-moveit/blob/ff8c6b9f2a58e1484843eb6e0d3d388a16bbc5ae/icub_moveit_config/config/iCub.ros2_control.xacro#L6-L99

To solve this, I created two different ros2 control macros with the prefix left and right to separate the two groups of joints. I pushed the changes here https://github.com/icub-tech-iit/study-moveit/commit/f8ad16b7eaec596103080c70d7a72c51b3e2e658. Here the result:

https://github.com/icub-tech-iit/study-moveit/assets/114698424/a09cb4b2-15d9-4e45-b9d7-ab8154ea925e

pattacini commented 1 year ago

Well done @martinaxgloria 👍🏻 We can create a follow-up for the next sprint to address the head, torso and the legs control.

Nicogene commented 1 year ago

Closing in favour of #6