Closed Nicogene closed 1 year ago
/remind tomorrow @Nicogene, @martinaxgloria please finalize this task.
⏰ Reminder Friday, June 30, 2023 10:00 AM (GMT+02:00)
@Nicogene, @martinaxgloria please finalize this task.
🔔 @pattacini
@Nicogene, @martinaxgloria please finalize this task.
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:
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
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
We should speak w/ @traversaro and see how this work can fit this ticket: https://github.com/icub-tech-iit/tickets/issues/218
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.
I'm not able to reach this service.
Ok, this was a stupid forgetfulness that I fixed.
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:
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
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
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).
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
Great achievement! 🚀
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.
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:
I will add another client to the service SetControlModes
in order to be able to switch in position-direct
control.
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.
Sure, I'm going to do it between today and Monday
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
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
.
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.
I've already started a refactoring of the ros packages included in this repo and necessary to run the joint_trajectory_controller on iCub. I would like to rearrange all the files divided into well-organized folders like icub_config
for the configuration files, icub_bringup
for the startup files, icub_hardware
for the controller files, and so on.
I've already modified the README.md with the correct procedure to install and use the package (I will open a PR against master asap) and I'm going to implement the getAvailableControlModes
and setControlMode
to respectively return all the control modalities and to switch between them.
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.
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
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.
👍🏻
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
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
Super!
Regarding iCubGazeboV2_7
, strangely enough, I managed yesterday to make it run in Gitpod.
I'll check it out one more time.
Today I will check again too 👍🏻
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.
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.
Closing in favor of a couple of follow-ups:
Well done 🚀
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.