Import ergoCub in Gazebo

xela-95 commented 4 months ago

With this issue we want to import one of the ergoCub models available in https://github.com/icub-tech-iit/ergocub-software into Gazebo.

Upgrades required to make it work

[x] #68
[x] #80
[ ] ~RGBD Camera~ -> postponed
[x] #89
[x] #108
[x] Enable all (necessary) Yarp Devices used in Robot Interface

xela-95 commented 4 months ago

CC @traversaro

xela-95 commented 4 months ago

Habemus the first version of ergocub in Gazebo Harmonic!

@traversaro I continued from the work you started on Friday, this time I:

Converted the https://github.com/icub-tech-iit/ergocub-software/blob/dbbdd3940718d71ce5763999bfb857504cd772c5/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf to sdf format with gz sdf -p model.urdf > model.sdf and commented out from the resulting sdf all the sensors and plugins;

The sdf model is then included into another model that simply adds the fixed joint between world and root link:

<?xml version='1.0'?>
<sdf version='1.7'>
<model name="ergocub_fixed">
<include>
  <uri>model://ergoCub/robots/ergoCubGazeboV1_1</uri>
  <pose>0.0 0 1.0 0 0 3.14</pose>
</include>

<joint name="fixed_base" type="fixed">
  <parent>world</parent>
  <child>ergoCub::root_link</child>
</joint>
</model>
</sdf>

This model is then loaded into a world file:

<?xml version="1.0"?>

<sdf version="1.7">
    <world name="turorial_controlboard">
        <physics name="1ms" type="ignored">
            <max_step_size>0.001</max_step_size>
            <real_time_factor>1.0</real_time_factor>
        </physics>
        <plugin
            filename="gz-sim-physics-system"
            name="gz::sim::systems::Physics">
        </plugin>
        <plugin
            filename="gz-sim-user-commands-system"
            name="gz::sim::systems::UserCommands">
        </plugin>
        <plugin
            filename="gz-sim-sensors-system"
            name="gz::sim::systems::Sensors">
        </plugin>
        <plugin
            filename="gz-sim-scene-broadcaster-system"
            name="gz::sim::systems::SceneBroadcaster">
        </plugin>

        <light type="directional" name="sun">
            <cast_shadows>true</cast_shadows>
            <pose>0 0 10 0 0 0</pose>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.2 0.2 0.2 1</specular>
            <attenuation>
                <range>1000</range>
                <constant>0.9</constant>
                <linear>0.01</linear>
                <quadratic>0.001</quadratic>
            </attenuation>
            <direction>-0.5 0.1 -0.9</direction>
        </light>

        <model name="ground_plane">
            <static>true</static>
            <link name="link">
                <collision name="collision">
                    <geometry>
                        <plane>
                            <normal>0 0 1</normal>
                            <size>100 100</size>
                        </plane>
                    </geometry>
                </collision>
                <visual name="visual">
                    <geometry>
                        <plane>
                            <normal>0 0 1</normal>
                            <size>100 100</size>
                        </plane>
                    </geometry>
                    <material>
                        <ambient>0.8 0.8 0.8 1</ambient>
                        <diffuse>0.8 0.8 0.8 1</diffuse>
                        <specular>0.8 0.8 0.8 1</specular>
                    </material>
                </visual>
            </link>
        </model>

        <include>
            <pose>0 0 0.5 0 -0.1 3.14</pose>
            <!-- <uri>model://ergoCubSN001_fixed</uri> -->
            <uri>model://ergoCub/robots/ergoCubGazeboV1_1_fixed</uri>
        </include>

    </world>
</sdf>

Launching Gazebo Harmonic with gz sim example.world --verbose it is possible to obtain the result in the image

P.S.: in order to make the of sdf find the models the following environment variable has been set:

export GZ_SIM_RESOURCE_PATH=${GZ_SIM_RESOURCE_PATH}:<path-to-ergocub-sw-installation>/share/

xela-95 commented 4 months ago

Here's the converted and modifiedergoCubGazeboV1_1.sdf: model.zip

xela-95 commented 4 months ago

Now I will try to add the sensors and plugins one at a time and see if everything works.

xela-95 commented 4 months ago

The first plugin I tried to enable is the yarprobotinterface:

    <plugin name="gzyarp::RobotInterface" filename="gz-sim-yarp-robotinterface-system">
      <yarpRobotInterfaceConfigurationFile>model://ergoCub/conf/ergocub.xml</yarpRobotInterfaceConfigurationFile>
    </plugin>

The plugin is recognized and starts, but it has troubles finding the configuration file using the model:// syntax. This happened to me previously when working on tutorials for the plugins.

xela-95 commented 4 months ago

Added the FT sensors: model.zip

xela-95 commented 4 months ago

Added all IMUs: model.zip

xela-95 commented 4 months ago

Added Laser: model.zip

xela-95 commented 4 months ago

I added the RGB camera updating the plugin tag to the new version but I got a SEGFAULT error, so I commented the plugin out for the moment. model.zip

At the current status we do not have the plugin of the depth camera (issue #106) so I will leave that commented out.

traversaro commented 4 months ago

Yes, I think at a first round we can concentrate on having:

controlboard
imu
ft

that are the sensors required for walking.

xela-95 commented 4 months ago

@traversaro a curiosity: while all the other IMU plugins are inside <sensor> blocks, there is only one that is not:

https://github.com/icub-tech-iit/ergocub-software/blob/dbbdd3940718d71ce5763999bfb857504cd772c5/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf#L2960-L2964

Is it on purpose? It seems related to https://github.com/icub-tech-iit/ergocub-software/blob/dbbdd3940718d71ce5763999bfb857504cd772c5/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf#L2965-L2974, since they also have the same configuration file. Since our plugin needs a mandatory sensorName parameter I have to understand how to handle that if it is necessary.

traversaro commented 4 months ago

@traversaro a curiosity: while all the other IMU plugins are inside <sensor> blocks, there is only one that is not:

https://github.com/icub-tech-iit/ergocub-software/blob/dbbdd3940718d71ce5763999bfb857504cd772c5/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf#L2960-L2964

Is it on purpose? It seems related to https://github.com/icub-tech-iit/ergocub-software/blob/dbbdd3940718d71ce5763999bfb857504cd772c5/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf#L2965-L2974, since they also have the same configuration file. Since our plugin needs a mandatory sensorName parameter I have to understand how to handle that if it is necessary.

That seems to be a bug in the model. fyi @Nicogene

xela-95 commented 4 months ago

I'm getting an error when trying to launch the model with one controlboard active:

[INFO] gz-sim-yarp-controlboard-system: configuration of device  left_leg_hardware_device  loaded from yarpConfigurationFile :  /home/acroci/ergocub_ws/install/share/ergoCub/conf/gazebo_ergocub_left_leg.ini 
[DEBUG] gz-sim-yarp-controlboard-system : robot scoped name:  model/ergocub_fixed/model/ergoCub
[DEBUG] gz-sim-yarp-controlboard-system : device scoped name:  model/ergocub_fixed/model/ergoCub/left_leg_hardware_device
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (IMPEDANCE_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (damping 0.0 0.0 0.0 0.0 0.0 0.0)) (LIMITS (jntPosMax 102.0 108.0 78.0 5.0 43.0 23.0) (jntPosMin -42.0 -15.0 -78.0 -103.0 -43.0 -23.0) (jntVelMax 100.0 100.0 100.0 100.0 100.0 100.0)) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 70.0 70.0 40.0 100.0 100.0 100.0) (kd 0.149999999999999994449 0.149999999999999994449 0.349999999999999977796 0.149999999999999994449 0.149999999999999994449 0.149999999999999994449) (ki 0.170000000000000012212 0.170000000000000012212 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796 0.349999999999999977796) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (TRAJECTORY_GENERATION (trajectory_type minimum_jerk)) (VELOCITY_CONTROL (velocityControlImplementationType integrator_and_position_pid) (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686 8.72600000000000086686) (kd 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907 0.348999999999999976907) (ki 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786 0.175999999999999989786) (maxInt 9999 9999 9999 9999 9999 9999) (maxOutput 9999 9999 9999 9999 9999 9999) (shift 0.0 0.0 0.0 0.0 0.0 0.0) (ko 0.0 0.0 0.0 0.0 0.0 0.0) (stictionUp 0.0 0.0 0.0 0.0 0.0 0.0) (stictionDwn 0.0 0.0 0.0 0.0 0.0 0.0)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (jointNames l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll) (max_damping 100.0 100.0 100.0 100.0 100.0 100.0) (max_stiffness 1000.0 1000.0 1000.0 1000.0 1000.0 1000.0) (max_torques 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0 1000000.0) (min_damping 0.0 0.0 0.0 0.0 0.0 0.0) (min_stiffness 0.0 0.0 0.0 0.0 0.0 0.0) (name "model/ergocub_fixed/model/ergoCub/left_leg_hardware_device") (robotScopedName "model/ergocub_fixed/model/ergoCub") (yarpDeviceName left_leg_hardware_device)
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class ControlBoardDriver for documentation.
[INFO] Found 6 joints from the plugin configuration.
[INFO] Found 65 joints from the model description.
[INFO] Joint  l_hip_pitch  added to the control board data.
[INFO] Joint  l_hip_roll  added to the control board data.
[INFO] Joint  l_hip_yaw  added to the control board data.
[INFO] Joint  l_knee  added to the control board data.
[INFO] Joint  l_ankle_pitch  added to the control board data.
[INFO] Joint  l_ankle_roll  added to the control board data.
[INFO] Trajectory generator type set to  minimum_jerk
[ERROR] refSpeed  not found
[WARNING] Parameter refSpeed not found in TRAJECTORY_GENERATION group. Defaults will be applied
[ERROR] refAcceleration  not found
[WARNING] Parameter refAcceleration not found in TRAJECTORY_GENERATION group. Defaults will be applied
[DEBUG] Joint  l_hip_pitch  trajectory generation reference speed:  10  [deg/s]
[DEBUG] Joint  l_hip_pitch  trajectory generation reference acceleration:  10  [deg/s^2]
[DEBUG] Joint  l_hip_roll  trajectory generation reference speed:  10  [deg/s]
[DEBUG] Joint  l_hip_roll  trajectory generation reference acceleration:  10  [deg/s^2]
[DEBUG] Joint  l_hip_yaw  trajectory generation reference speed:  10  [deg/s]
[DEBUG] Joint  l_hip_yaw  trajectory generation reference acceleration:  10  [deg/s^2]
[DEBUG] Joint  l_knee  trajectory generation reference speed:  10  [deg/s]
[DEBUG] Joint  l_knee  trajectory generation reference acceleration:  10  [deg/s^2]
[DEBUG] Joint  l_ankle_pitch  trajectory generation reference speed:  10  [deg/s]
[DEBUG] Joint  l_ankle_pitch  trajectory generation reference acceleration:  10  [deg/s^2]
[DEBUG] Joint  l_ankle_roll  trajectory generation reference speed:  10  [deg/s]
[DEBUG] Joint  l_ankle_roll  trajectory generation reference acceleration:  10  [deg/s^2]
[WARNING] No initial configuration found, initializing trajectory generator with current values
terminate called after throwing an instance of 'std::out_of_range'
  what():  vector::_M_range_check: __n (which is 0) >= this->size() (which is 0)
Stack trace (most recent call last):
#31   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7fa8d40bfc96, in rb_protect
#30   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7fa8d425f461, in rb_yield
#29   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7fa8d425adfc, in rb_vm_exec
#28   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7fa8d4257f39, in vm_exec_core
#27   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7fa8d4253ab9, in vm_sendish
#26   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7fa8d4249ec0, in vm_call_cfunc_with_frame
#25   Object "/home/acroci/mambaforge/envs/gz8/lib/ruby/3.2.0/x86_64-linux/fiddle.so", at 0x7fa8cf486dfc, in function_call
#24   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7fa8d421bce2, in rb_nogvl
#23   Object "/home/acroci/mambaforge/envs/gz8/lib/ruby/3.2.0/x86_64-linux/fiddle.so", at 0x7fa8cf486748, in nogvl_ffi_call
#22   Object "/home/acroci/mambaforge/envs/gz8/lib/libffi.so.8.1.0", at 0x7fa8cf389fe9, in ffi_call_int
#21   Object "/home/acroci/mambaforge/envs/gz8/lib/libffi.so.8.1.0", at 0x7fa8cf38aa49, in ffi_call_unix64
#20   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8-gz.so.8.1.0", at 0x7fa8ce712c00, in runServer
#19   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce1b53cf, in gz::sim::v8::Server::Server(gz::sim::v8::ServerConfig const&)
#18   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce1c297e, in gz::sim::v8::ServerPrivate::CreateEntities()
#17   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce1dae79, in gz::sim::v8::SimulationRunner::SimulationRunner(sdf::v14::World const*, std::shared_ptr<gz::sim::v8::SystemLoader> const&, gz::sim::v8::ServerConfig const&)
#16   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce155e9a, in gz::sim::v8::LevelManager::UpdateLevelsState()
#15   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce14a3f8, in gz::sim::v8::LevelManager::LoadActiveEntities(std::set<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)
#14   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce196c40, in gz::sim::v8::SdfEntityCreator::CreateEntities(sdf::v14::Model const*)
#13   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce18cb0f, in void gz::sim::v8::EventManager::Emit<gz::common::EventT<void (unsigned long, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> >), gz::sim::v8::events::LoadPluginsTag>, unsigned long const&, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> > const&>(unsigned long const&, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> > const&)
#12   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce1ccb95, in gz::sim::v8::SimulationRunner::LoadPlugins(unsigned long, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> > const&)
#11   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce1e98c4, in gz::sim::v8::SystemManager::LoadPlugin(unsigned long, sdf::v14::Plugin const&)
#10   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7fa8ce1e926f, in gz::sim::v8::SystemManager::AddSystemImpl(gz::sim::v8::SystemInternal, std::shared_ptr<sdf::v14::Element const>)
#9    Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim-yarp-controlboard-system.so", at 0x7fa89f7d76cb, in gzyarp::ControlBoard::Configure(unsigned long const&, std::shared_ptr<sdf::v14::Element const> const&, gz::sim::v8::EntityComponentManager&, gz::sim::v8::EventManager&)
#8    Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim-yarp-controlboard-system.so", at 0x7fa89f7d2c37, in gzyarp::ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::v8::EntityComponentManager&)
#7    Source "../../../../../libstdc++-v3/src/c++11/functexcept.cc", line 101, in __throw_out_of_range_fmt [0x7fa8cf1c9f45]
#6    Source "../../../../libstdc++-v3/libsupc++/eh_throw.cc", line 98, in __cxa_throw [0x7fa8cf1ae67f]
#5    Source "../../../../libstdc++-v3/libsupc++/eh_terminate.cc", line 58, in terminate [0x7fa8cf1ae48d]
#4    Source "../../../../libstdc++-v3/libsupc++/eh_terminate.cc", line 48, in __terminate [0x7fa8cf1ae43b]
#3    Source "../../../../libstdc++-v3/libsupc++/vterminate.cc", line 95, in __verbose_terminate_handler [0x7fa8cf1afeff]
#2    Source "./stdlib/abort.c", line 79, in abort [0x7fa8d3c287f2]
#1    Source "../sysdeps/posix/raise.c", line 26, in raise [0x7fa8d3c42475]
#0  | Source "./nptl/pthread_kill.c", line 89, in __pthread_kill_internal
    | Source "./nptl/pthread_kill.c", line 78, in __pthread_kill_implementation
      Source "./nptl/pthread_kill.c", line 44, in __pthread_kill [0x7fa8d3c969fc]
Aborted (Signal sent by tkill() 243247 1000)

This error comes from https://github.com/robotology/gz-sim-yarp-plugins/blob/98f78ae5fff2fbcc8f5f240b3cb87475cded5451/plugins/controlboard/src/ControlBoard.cpp#L874

part of the method void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponentManager& ecm) called from the Configure method: https://github.com/robotology/gz-sim-yarp-plugins/blob/98f78ae5fff2fbcc8f5f240b3cb87475cded5451/plugins/controlboard/src/ControlBoard.cpp#L152

Apparently the position vector returned has size 0 causing the std::out_of_range exception. I don't have currently understood why the position vector is empty during the configure, what I could do is to add an if condition to avoid this case.

traversaro commented 4 months ago

It would be useful to get via debug or print the joint name on which you are experiencing this problem.

Nicogene commented 4 months ago

@traversaro a curiosity: while all the other IMU plugins are inside <sensor> blocks, there is only one that is not: icub-tech-iit/ergocub-software@dbbdd39/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf#L2960-L2964 Is it on purpose? It seems related to icub-tech-iit/ergocub-software@dbbdd39/urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf#L2965-L2974, since they also have the same configuration file. Since our plugin needs a mandatory sensorName parameter I have to understand how to handle that if it is necessary.

That seems to be a bug in the model. fyi @Nicogene

I think that this is because it has been added as xmlBlob, since right now we are missing the CSYS in the cad, but it can be fixed by changing the blob. See:

Is it causing any issues?

traversaro commented 4 months ago

Is it causing any issues?

OT:

To be honest, I am not sure how that can work in Gazebo Classic. libgazebo_yarp_imu is a sensor plugin, so I am not sure how it can work without being in a sensor tag. Did we tested its measurements in @martinaxgloria's test?

martinaxgloria commented 4 months ago

Did we tested its measurements in @martinaxgloria's test?

Nope, I didn't test it since I'm working mainly with iCub models. Btw the test may be easily adaptable to ergoCub models. I did a rapid check by attaching to this MASserver and testing only the waist_imu_0 sensor. The output of the test was:

(Imu) checks: Testing sensor waist_imu_0: the max rotation angle error is 0.002386 rad!

cc @traversaro

traversaro commented 4 months ago

Ok, I have no idea how that works. Anyhow, I can open a separate issue.

xela-95 commented 4 months ago

For convenience, I've created a temporary repository containing all the changes to the ergocub sdf to make it work with gz-sim-yarp-plugins: https://github.com/xela-95/ergocub_ws, so I don't have to upload copies of the modified sdf directly here.

xela-95 commented 3 months ago

After completing #89 I'm able to handle all the controlboards of ergoCub on Gazebo:

https://github.com/robotology/gz-sim-yarp-plugins/assets/57228872/0119146a-3cf5-4b5d-8bca-8533c117eebf

The updated version of the ergoCub model I've used in this video is https://github.com/xela-95/ergocub_ws/tree/deda44006360e08fd630754518db227d4185562e

CC @traversaro

traversaro commented 3 months ago

For convenience, I've created a temporary repository containing all the changes to the ergocub sdf to make it work with gz-sim-yarp-plugins: https://github.com/xela-95/ergocub_ws, so I don't have to upload copies of the modified sdf directly here.

Just in case anyone from AMI is wondering why we are using a personal repo: we want to avoid as much as possible for people to think that this is a repo from which the updated model of ergocub is available, so using a personal repo seems the perfect way of doing so.

xela-95 commented 3 months ago

With the new modifications introduced by #116 I'm now able to use URI paths for yarp configuration strings: https://github.com/xela-95/ergocub_ws/tree/9657015d3adbcd1e20e5f89595cf63f27c9f8885

Now I'm commenting one at a time the devices in https://github.com/xela-95/ergocub_ws/blob/9657015d3adbcd1e20e5f89595cf63f27c9f8885/install/share/ergoCub/conf/ergocub.xml to see if they are working. I'm currently stuck at https://github.com/xela-95/ergocub_ws/blob/9657015d3adbcd1e20e5f89595cf63f27c9f8885/install/share/ergoCub/conf/ergocub.xml#L25C1-L25C75 since the wrappers/FT/left_leg-FT_wrapper.xml device is causing a crash with the following logs:

[ERROR] Target device ergocub_left_leg_ft (network = left_leg_ft ) does not exist.
[ERROR] Cannot run attach action on device left_leg-FT_remapper
[INFO] Executing attach action, level 5 on device right_leg-FT_remapper with parameters [("networks" = "(right_leg_ft right_foot_rear_ft right_foot_front_ft)"), ("right_leg_ft" = "ergocub_right_leg_ft"), ("right_foot_rear_ft" = "ergocub_right_foot_rear_ft"), ("right_foot_front_ft" = "ergocub_right_foot_front_ft")]
[ERROR] Target device ergocub_right_leg_ft (network = right_leg_ft ) does not exist.
[ERROR] Cannot run attach action on device right_leg-FT_remapper
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[INFO] Entering action level 10 of phase startup
[INFO] Executing attach action, level 10 on device left_arm-mc_nws_yarp with parameters [("device" = "left_arm-mc_remapper")]
[INFO] Executing attach action, level 10 on device right_arm-mc_nws_yarp with parameters [("device" = "right_arm-mc_remapper")]
[INFO] Executing attach action, level 10 on device left_leg-mc_nws_yarp with parameters [("device" = "left_leg-mc_remapper")]
[INFO] Executing attach action, level 10 on device right_leg-mc_nws_yarp with parameters [("device" = "right_leg-mc_remapper")]
[INFO] Executing attach action, level 10 on device head-mc_nws_yarp with parameters [("device" = "head-mc_remapper")]
[INFO] Executing attach action, level 10 on device torso-mc_nws_yarp with parameters [("device" = "torso-mc_remapper")]
[INFO] Executing attach action, level 10 on device left_leg-FT_wrapper with parameters [("networks" = "(FirstStrain)"), ("FirstStrain" = "left_leg-FT_remapper")]
[INFO] left_leg-FT_wrapper is not an IWrapper. Trying IMultipleWrapper
Stack trace (most recent call last):
#31   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c53ab9, in vm_sendish
#30   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c49ec0, in vm_call_cfunc_with_frame
#29   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593b95c5d, in rb_f_fork
#28   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593abfc96, in rb_protect
#27   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c5f461, in rb_yield
#26   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c5adfc, in rb_vm_exec
#25   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c57f39, in vm_exec_core
#24   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c53ab9, in vm_sendish
#23   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c49ec0, in vm_call_cfunc_with_frame
#22   Object "/home/acroci/mambaforge/envs/gz8/lib/ruby/3.2.0/x86_64-linux/fiddle.so", at 0x7d8593836dfc, in function_call
#21   Object "/home/acroci/mambaforge/envs/gz8/lib/libruby.so.3.2.2", at 0x7d8593c1bce2, in rb_nogvl
#20   Object "/home/acroci/mambaforge/envs/gz8/lib/ruby/3.2.0/x86_64-linux/fiddle.so", at 0x7d8593836748, in nogvl_ffi_call
#19   Object "/home/acroci/mambaforge/envs/gz8/lib/libffi.so.8.1.0", at 0x7d85934f8fe9, in ffi_call_int
#18   Object "/home/acroci/mambaforge/envs/gz8/lib/libffi.so.8.1.0", at 0x7d85934f9a49, in ffi_call_unix64
#17   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8-gz.so.8.1.0", at 0x7d858e1b5c00, in runServer
#16   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858dbb53cf, in gz::sim::v8::Server::Server(gz::sim::v8::ServerConfig const&)
#15   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858dbc297e, in gz::sim::v8::ServerPrivate::CreateEntities()
#14   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858dbdae79, in gz::sim::v8::SimulationRunner::SimulationRunner(sdf::v14::World const*, std::shared_ptr<gz::sim::v8::SystemLoader> const&, gz::sim::v8::ServerConfig const&)
#13   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858db55e9a, in gz::sim::v8::LevelManager::UpdateLevelsState()
#12   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858db4a3f8, in gz::sim::v8::LevelManager::LoadActiveEntities(std::set<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)
#11   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858db96c40, in gz::sim::v8::SdfEntityCreator::CreateEntities(sdf::v14::Model const*)
#10   Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858db8cb0f, in void gz::sim::v8::EventManager::Emit<gz::common::EventT<void (unsigned long, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> >), gz::sim::v8::events::LoadPluginsTag>, unsigned long const&, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> > const&>(unsigned long const&, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> > const&)
#9    Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858dbccb95, in gz::sim::v8::SimulationRunner::LoadPlugins(unsigned long, std::vector<sdf::v14::Plugin, std::allocator<sdf::v14::Plugin> > const&)
#8    Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858dbe98c4, in gz::sim::v8::SystemManager::LoadPlugin(unsigned long, sdf::v14::Plugin const&)
#7    Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim8.so.8.1.0", at 0x7d858dbe926f, in gz::sim::v8::SystemManager::AddSystemImpl(gz::sim::v8::SystemInternal, std::shared_ptr<sdf::v14::Element const>)
#6    Object "/home/acroci/mambaforge/envs/gz8/lib/libgz-sim-yarp-robotinterface-system.so", at 0x7d855d706710, in gzyarp::RobotInterface::Configure(unsigned long const&, std::shared_ptr<sdf::v14::Element const> const&, gz::sim::v8::EntityComponentManager&, gz::sim::v8::EventManager&)
#5    Object "/home/acroci/mambaforge/envs/gz8/lib/libYARP_robotinterface.so.3.9.0", at 0x7d855dbe94f1, in yarp::robotinterface::Robot::enterPhase(yarp::robotinterface::ActionPhase)
#4    Object "/home/acroci/mambaforge/envs/gz8/lib/libYARP_robotinterface.so.3.9.0", at 0x7d855dbe7289, in yarp::robotinterface::Robot::Private::attach(yarp::robotinterface::Device const&, std::vector<yarp::robotinterface::Param, std::allocator<yarp::robotinterface::Param> > const&)
#3    Object "/home/acroci/mambaforge/envs/gz8/lib/libYARP_robotinterface.so.3.9.0", at 0x7d855dbdb4ce, in yarp::robotinterface::Device::attach(yarp::dev::PolyDriverList const&) const
#2    Object "/home/acroci/mambaforge/envs/gz8/lib/yarp/yarp_multipleanalogsensorsserver.so", at 0x7d8556d4cb9b, in MultipleAnalogSensorsServer::attachAll(yarp::dev::PolyDriverList const&)
#1    Object "/home/acroci/mambaforge/envs/gz8/lib/yarp/yarp_multipleanalogsensorsserver.so", at 0x7d8556d4ad38, in MultipleAnalogSensorsServer::populateAllSensorsMetadata()
#0    Object "/home/acroci/mambaforge/envs/gz8/lib/yarp/yarp_multipleanalogsensorsremapper.so", at 0x7d8556d60474, in non-virtual thunk to MultipleAnalogSensorsRemapper::getNrOfThreeAxisGyroscopes() const
Segmentation fault (Address not mapped to object [0x8])

xela-95 commented 3 months ago

The origin of the errors is:

[ERROR] |yarp.device.multipleanalogsensorsremapper| Impossible to find sensor name l_leg_ft , exiting.
[ERROR] |yarp.device.multipleanalogsensorsremapper|     Names of available sensors are:
[ERROR] Device left_leg-FT_remapper cannot execute attach
[ERROR] Cannot run attach action on device left_leg-FT_remapper
[INFO] Executing attach action, level 5 on device right_leg-FT_remapper with parameters [("networks" = "(right_leg_ft right_foot_rear_ft right_foot_front_ft)"), ("right_leg_ft" = "ergocub_right_leg_ft"), ("right_foot_rear_ft" = "ergocub_right_foot_rear_ft"), ("right_foot_front_ft" = "ergocub_right_foot_front_ft")]

The error logs are coming from MultipleAnalogSensorsRemapper: https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L202-L207

Apparently it cannot find any sensor available. Any clue on how to debug this @traversaro?

traversaro commented 3 months ago

Can you check if the devices to which the remapper is attached return the correct value for the yarp::dev::ISixAxisForceTorqueSensors::getSixAxisForceTorqueSensorName method? This is the method used by the remapper to find the devices specified in the configuration, in particular in https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L235 and https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L202 .

xela-95 commented 3 months ago

Can you check if the devices to which the remapper is attached return the correct value for the yarp::dev::ISixAxisForceTorqueSensors::getSixAxisForceTorqueSensorName method? This is the method used by the remapper to find the devices specified in the configuration, in particular in https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L235 and https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L202 .

Thanks for pointing me in that direction. I am logging the sensor names and they seem correct, but I noted that the yarp::dev::ISixAxisForceTorqueSensors::getSixAxisForceTorqueSensorName method is never called.

xela-95 commented 3 months ago

Ok

Can you check if the devices to which the remapper is attached return the correct value for the yarp::dev::ISixAxisForceTorqueSensors::getSixAxisForceTorqueSensorName method? This is the method used by the remapper to find the devices specified in the configuration, in particular in https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L235 and https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L202 .

Thanks for pointing me in that direction. I am logging the sensor names and they seem correct, but I noted that the yarp::dev::ISixAxisForceTorqueSensors::getSixAxisForceTorqueSensorName method is never called.

The exception was due to the method MultipleAnalogSensorsRemapper::getTemperatureSensorName https://github.com/xela-95/yarp/blob/493b6d3134031c5bd621ef643bf8a22929677148/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L531-L535 in the remappers: https://github.com/icub-tech-iit/ergocub-software/blob/335ed25cab5ff24ccdc4647b4ea318e0f5f9f305/urdf/ergoCub/conf/wrappers/FT/right_leg-FT_remapper.xml#L9-L11

Commenting out these lines make Gazebo start.

xela-95 commented 3 months ago

The exception was due to the method MultipleAnalogSensorsRemapper::getTemperatureSensorName https://github.com/xela-95/yarp/blob/493b6d3134031c5bd621ef643bf8a22929677148/src/devices/multipleanalogsensorsremapper/MultipleAnalogSensorsRemapper.cpp#L531-L535 in the remappers: https://github.com/icub-tech-iit/ergocub-software/blob/335ed25cab5ff24ccdc4647b4ea318e0f5f9f305/urdf/ergoCub/conf/wrappers/FT/right_leg-FT_remapper.xml#L9-L11

Commenting out these lines make Gazebo start.

The same for ThreeAxisMagnetometersNames parameters https://github.com/icub-tech-iit/ergocub-software/blob/335ed25cab5ff24ccdc4647b4ea318e0f5f9f305/urdf/ergoCub/conf/wrappers/inertials/right_foot-IMU_remapper.xml#L12-L14 in right and left feet IMU remappers.

traversaro commented 3 months ago

Good catch! Indeed the temperature sensors are supported in gyp:

Probably it would be also good to change the error message in YARP to refer to the type of sensors could not be found.

xela-95 commented 3 months ago

Note: when using yarpmotorgui the error logs:

[INFO] head iencs->getEncoders() ok!
[ERROR] Error while getting current range, part  head  joint  0
[ERROR] Error while getting current range, part  head  joint  1
[ERROR] Error while getting current range, part  head  joint  2
[ERROR] Error while getting current range, part  head  joint  3

are wrote from https://github.com/robotology/yarp/blob/1326677585ffb3303b42760f5839e5e1f1a8338e/src/yarpmotorgui/partitem.cpp#L217 due to a missing implementation of ICurrentControl::getCurrentRange: https://github.com/robotology/gz-sim-yarp-plugins/blob/85d7d8a34d48006039f4b874b95ba5da9ecfb605/plugins/controlboard/src/ControlBoardDriver.cpp#L1378-L1381

xela-95 commented 3 months ago

Ergocub has been imported in Gazebo and it works with yarpmotorgui. Sensors not currently implemented are still commented out but this will not block us from trying to go forward with trying walking in #94.

xela-95 commented 3 months ago

Good catch! Indeed the temperature sensors are supported in gyp:

* https://github.com/robotology/gazebo-yarp-plugins/blob/bad19ba865278d38b7e93b31706e42ff487b0bb7/plugins/forcetorque/src/ForceTorqueDriver.cpp#L227-L237

* https://github.com/robotology/gazebo-yarp-plugins/blob/bad19ba865278d38b7e93b31706e42ff487b0bb7/plugins/imu/src/IMUDriver.cpp#L234-L237

I will add issues to implement the missing interfaces in FT and IMUs, or there are reasons not to do it?

Probably it would be also good to change the error message in YARP to refer to the type of sensors could not be found.

Yep that would be useful! Maybe I can open a PR in future :)

traversaro commented 3 months ago

I will add issues to implement the missing interfaces in FT and IMUs, or there are reasons not to do it?

No, let's for go for it!

xela-95 commented 3 months ago

will add issues to implement the missing interfaces in FT and IMUs

xref: #120 #121

xela-95 commented 3 months ago

Concerning the camera, at the moment it is not possible to enable https://github.com/icub-tech-iit/ergocub-software/blob/master/urdf/ergoCub/conf/sensors/rgbd_camera_wrapper.xml since this is causing errors, I think due to the missing depth camera, so this device will remain commented until #106.

xela-95 commented 3 months ago

Ergocub has been imported successfully into Gazebo. Closing!

robotology / gz-sim-yarp-plugins

Import ergoCub in Gazebo #103

Upgrades required to make it work