robotology / gazebo-yarp-plugins Goto Github PK

Using robot motor gui, velocity control mode does not work.
We should check values for the velocity control PIDs.

I started with the code we had, fixed some simple bugs, and then started to tune the PIDS. To do that, I managed to obtain some debug from the joint position and speed and changed P,I,D values searching for a good compromise.
In the attachments you can see the step response of a joint going from 0 to 10°/s with a little noise due to gravity.
If somebody wants to edit those values, just uncomment the logger lines and use it to plot graphs in matlab etc etc.

https://github.com/robotology/gazebo_yarp_plugins/blob/PidTuningResults/tuningResults/debugPID/position.fig
https://github.com/robotology/gazebo_yarp_plugins/blob/PidTuningResults/tuningResults/debugPID/speed.fig
https://github.com/robotology/gazebo_yarp_plugins/blob/PidTuningResults/tuningResults/debugPID/speedavg.fig

Also, here you can find a matlab script to plot those results (custom script, not official at all)
https://github.com/robotology/gazebo_yarp_plugins/tree/PidTuningResults/tuningResults

The time is come...the son leaves its house in order to go in a better place...when we are going to do the passage?

Hi everyone,
as I'm working on the iCub "self-touch" project, I was not so happy to see the icubSim going through his body without colliding. I decided to change that, created a branch of the simulator and redefined the collision spaces. At the moment, I have a version that collides, I read out the coordinates, map them into a local reference frame of a respective skin part and emulate a skinEvent / skinContact port (/skinManager/skin_events:o), not the raw /icub/skin/... My thing is still not working with the covers though, as they have no bodies and mass in icubSim.

Anyway, I just discussed with Silvio the state of affairs in the gazebo - it seems that much of this functionality could come quite easily there - adding a contact sensor to individual body parts and reading the coordinates. For now, I just wanted to make you aware that there would be interest to have a skin emulation there and that some of the code I developed for icubSim could be ported to gazebo too.
Please do keep me updated on developments in this direction.

Thanks!
Matej Hoffmann

The deprecated (and removed in boost 1.53) function shared_dynamic_cast is used in file IMU.cc . We have to fix this to be compatible with boost 1.53
@EnricoMingo

See https://bitbucket.org/osrf/gazebo/issue/581/boost-shared_-_cast-are-deprecated-removed

To proper support Force/Torque sensors on the simulated iCub (and on arbitrary simulated robots) I wanted to port the force/torque sensor implementation to use proper yarp device, with device wrappers and device factory, in a similar way as I have done for the IMU sensor #41.

@barbalberto
I guess that the proper driver interface is IAnalogSensor, while the proper wrapper is AnalogWrapper/AnalogServer ?

@arocchi
As the author of the previous version of the force/torque sensor implementation, does it make sense to you? I guess that the biggest differences of the new implementation will be:

• Identifying each sensor with the yarpConfigurationFile parameter instead of boardId one.
• onUpdate logic moved from the plugin to the sensor

What do you thing if we create a sort of configuration file for the PID gains? This file stores the initial values that are ridden at the beginning.

Removing a model using gazebo_yarp_controlboard, gazebo_yarp_imu or gazebo_yarp_forcetorque using the GUI frequently (always?) causes the crash of Gazebo.
I don't have a clear idea of where the bug is, but I presume this can be solved by appropriately handling the removal event in the plugins.

I think the method onUpdate at line 146 of FakebotForceTorquePlugin.cc should be inside a mutex session. The mutex should be the same used by the FakebotFtSensor::read line 121. Please check this.

I checked the code, and apparently (please correct me if I am wrong) with the current implementation of the control that uses the Gazebo topic we have to control all the joint of a given model with one plugin.
Than for implementing the different controlBoard acting on a single model we have two possible alternatives:

• Simulate multiple controlBoards with a single Gazebo plugin. I don't like this solution because I would really prefer to keep the clean approach of 1 Yarp Device <==> 1 Gazebo Plugin, but is however a possibility.
• Avoid using the topic interface. Correct me if I understood badly: I guess that the use of the gazebo topics was just a workaround, but in the middle/long term we want to control directly the Gazebo model with C++ direct calls, also to simulate accurately the low level control of the robots (Mirko seemed to be very interested on this aspect). In this case I guess we can implement the parts handling when we will control directly the joints.

Currently the yarp configuration files (.ini) for a given model are loaded from the conf/ directory of the SDF gazebo model (actually, this is what we usually do in our iCub/COMAN modeles, in the code [1] we just assume that the configuration file path is specified by a valid Gazebo URI [2] ).
This imply that if the user wants to modify some parameters of the yarp plugins (for example the PID gains) he has to directly modify the content of the gazebo model directory.

In Yarp the common practice is to use a way to clearly separate default configuration files (i.e. installation provided) and the user-modified configuration files (and possibly having multiple versions of the same configuration file installed at the same time, simplifying the switching). [3]

A first way to support the yarp-style configuration is to add the possibility to specify the yarpConfigurationFile tag in the SDF as yarp-style filename. We can do that without adding special tag, but just checking the yarpConfigurationFile argument: if it starts with "model://", "file://" or "http://" it specify the configuration filename with a Gazebo URI, otherwise we assume that it is a yarp-style filename, that we can find with the yarp::os::ResourceFinder class. Another possibility is to use a custom "yarp:://" prefix for yarp-style configuration files.

The next desirable features could be:

• start the Gazebo simulation/model using arbitrary contexts (this kind of behaviour is obtained in Ros-Gazebo integration using roslaunch, a yarp style way of doing this is not clear to me).
• handle also world simulation specific parameters (such as simulation timestep) or model specific parameters (with parts to simulate, fixed or floating robot) using yarp contexts.

All this changes can be implemented without affecting the users that want to continue to use gazebo_yarp_plugins in the way as it is currently used, with Gazebo "pure" models.

[1] : https://github.com/robotology/gazebo_yarp_plugins/blob/master/src/gazebo_plugins/ControlBoard.cc#L59
[2] : http://gazebosim.org/wiki/Uri
[3] : http://wiki.icub.org/yarpdoc/yarp_resource_finder_tutorials.html

cc @lornat75 @EnricoMingo @barbalberto

Hi,
I am following the instruction in README.md file but when I try to run "make" on the gazebo_yarp_plugins/build/ directory I get this error:

[57%] Built target fakebot
Linking CXX shared library libfakebotFTsensor.so
/usr/bin/ld: /usr/local/lib/libYARP_OS.a(Bottle.cpp.o): relocation R_X86_64_32S against `_ZTVN4yarp2os6BottleE' can not be used when making a shared object; recompile with -fPIC
/usr/local/lib/libYARP_OS.a: could not read symbols: Bad value

The yarp installed is the version:

YARP version 2.3.60

Thank you in advance,
Marco Damonte

Check and correct port names for force/torques and IMU.

In the README we target Gazebo version 1.9, but the new build fails with Gazebo 1.9.1, because for the IMU we are using GetId that was introduced in Gazebo 2.0 ( https://travis-ci.org/EnricoMingo/gazebo_yarp_plugins/builds/13287149 , compare http://gazebosim.org/api/1.9.1/classgazebo_1_1sensors_1_1Sensor.html with http://gazebosim.org/api/2.0.0/classgazebo_1_1sensors_1_1Sensor.html ).

The options are:

• Avoid using GetId
• Switch to 2.0 as target version

In the CMakeLists.txt the "-lboost_system" option is explicitly passed to the linker. It is necessary?

I have removed it without problem, while it was giving issue trying to compile on Travis ( https://travis-ci.org/traversaro/gazebo_yarp_plugins ).

I'm testing the Velocity control with the two different version of gazebo the I can install from the repositories (1.9 [gazebo] and 2.1 [gazebo-current])

the function is:

gazebo::physics::JointPtr joint =  this->_robot->GetJoint(joint_names[j]);
joint->SetMaxForce(0, joint->GetEffortLimit(0)*1.1); //<-- MAGIC NUMBER!!!!      
std::cout<<"j: "<<j<<" speed:" <<ref<<std::endl;
joint->SetVelocity(0,toRad(ref));

so I run:

./testmotor --robot coman --part right_arm

and type:

icmd set cmv 3
set vmo 3 -10

I obtain the cout in the function

j: 3 speed:-10

and the arm doesn't move. So I think the problem is Gazebo because the plugin does all it has to do.

Is it possible that the only thing I have to do is use Gazebo 2.0 to make it works?

P.S: Mirko tried with Gazebo 2.0 and everything works, of course we have the same version of YARP (the barbalberto one)

As already mentioned in #25, if we are refactoring the code It could be a good time also to change the name of plugin/device/classes to reflect their actual meaning and increase the readability of the repository for new users.
I expose a proposal that I came up with (inspired mainly from https://github.com/ros-simulation/gazebo_ros_pkgs/tree/hydro-devel/gazebo_plugins), but please make any comment on this.

https://github.com/robotology/gazebo_yarp_plugins/wiki/Proposal-for-names-conventions

(I am not sure about the yarp device driver naming convention, perhaps @lornat75 can provide a more informed opinion).

If we will be ok with this changes and when the times of the changes will come, I can do the dirty work of changing the names and updating all the uses across the repo (Possibly during an italian night).

As in the title, any good man that want to make some order in that "horrible" file?

Now the IMU is located as body "Waist". In the real robot it is not exactly in the same position. I have already asked for the CAD position.

Stupid issue, but the code is a mess and it can cause several problems, we better agree to a style and stick with it.
Based on the code now present in the repo, I guess I could propose:

• Indentation: 4 spaces (actual spaces, no tabs)
• Indent style: the code now present seem a mix of Allman and K&R, we can choose any style as long we stick to it.

Hi guys,
the position control in the simulator is done through an interpolation of the desired reference position using a reference speed:

double temp=ref_pos[j];
                if ( ( pos[j]-ref_pos[j] ) < -ROBOT_POSITION_TOLERANCE )
                {
                    if ( ref_speed[j]!=0 ) temp=pos[j]+ ( ref_speed[j]/1000.0 ) *robot_refresh_period* ( double ) _T_controller;
                    motion_done[j]=false;
                }
                else if ( ( pos[j]-ref_pos[j] ) >ROBOT_POSITION_TOLERANCE )
                {
                    if ( ref_speed[j]!=0 ) temp=pos[j]- ( ref_speed[j]/1000.0 ) *robot_refresh_period* ( double ) _T_controller;
                    motion_done[j]=false;
                }
                else
                {
                    motion_done[j]=true;
                }
                //std::cout<<"pos: "<<pos[j]<<" ref_pos: "<<ref_pos[j]<<" ref_speed: "<<ref_speed[j]<<" period: "<<robot_refresh_period<<" result: "<<temp<<std::endl;
                sendPositionToGazebo ( j,temp );

as you can see should be possible to "jump" this interpolation setting ref_speed[i] = 0. Now, ref_speed[i] is an element of a yarp::sig::Vector so I suppose is a double that means that also when you set it equal to 0 actually that if return true. Am I wrong? I think we should use something like

( ref_speed[j]>eps |  ref_speed[j]<-eps)

There is actually a problem when switching control modes (at the moment, from position control mode to any other control mode and back)
As Andrea (del Prete) pointed out, we should be able to reset the integrator inside the position (and velocity) PIDs inside JointController.
At the moment Gazebo does not offer this interface, which is a bummer.
From JointController.cc, the reset() function reads:

void JointController::Reset()
{
  // Reset setpoints and feed-forward.
  this->positions.clear();
  this->velocities.clear();
  this->forces.clear();
  // Should the PID's be reset as well?
}

Yes, yes you should.
As soon as this functionality is implemented in Gazebo ( https://bitbucket.org/osrf/gazebo/issue/936/jointcontroller-reset-should-reset-pids ) we should do it the right way.
For the moment, the way around it would be to call again JointController::AddJoint(), which does not check if the joint is already added, and calls PID::Init(), thus resetting them (PID::Reset()) together with their integrator (iErr term)

Setup OS

Ubuntu 13.04 system with gazebo 1.9 and Ros hydro

Get coman urdf

• clone the repository iit-coman-ros-pkg
• cd coman_urdf/script
• source /usr/share/ros-xxx/setup.sh
• ./create_bla_bla_bla coman_robot_plain.urdf.xacro

Note: we need to fix the use of ROS to get the sdf file

Compile the libyarp_gazebo

• clone the repository
• mkdir build
• cd build
• cmake ..
• make
• export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/gazebo_yarp_plugins/build

Run

• yarpserver
• gazebo yarp_gazebo.world

You should have something like this:

Robot Name: COMAN
# Joints: 23
# Links: 24
ControlBoard subdevice is fakebot
Joint Limits
COMAN::LHipSag max_pos: 45.0001 min_pos: -110.002
COMAN::LHipLat max_pos: 60.0001 min_pos: -24.9981
COMAN::LHipYaw max_pos: 50.002 min_pos: -50.002
COMAN::LKneeSag max_pos: 110.002 min_pos: -9.99811
COMAN::LAnkLat max_pos: 35.002 min_pos: -35.002
COMAN::LAnkSag max_pos: 69.9983 min_pos: -69.9983
COMAN::RHipSag max_pos: 45.0001 min_pos: -110.002
COMAN::RHipLat max_pos: 24.9981 min_pos: -60.0001
COMAN::RHipYaw max_pos: 50.002 min_pos: -50.002
COMAN::RKneeSag max_pos: 110.002 min_pos: -9.99811
COMAN::RAnkLat max_pos: 35.002 min_pos: -35.002
COMAN::RAnkSag max_pos: 69.9983 min_pos: -69.9983
COMAN::WaistSag max_pos: 29.9943 min_pos: -30.0001
COMAN::WaistLat max_pos: 50.002 min_pos: -20.002
COMAN::WaistYaw max_pos: 79.9964 min_pos: -80.0021
COMAN::LShSag max_pos: 95.0021 min_pos: -195
COMAN::LShLat max_pos: 119.977 min_pos: -17.9995
COMAN::LShYaw max_pos: 90.0002 min_pos: -90.0002
COMAN::LElbj max_pos: 0 min_pos: -135
COMAN::RShSag max_pos: 95.0021 min_pos: -195
COMAN::RShLat max_pos: 17.9995 min_pos: -119.977
COMAN::RShYaw max_pos: 90.0002 min_pos: -90.0002
COMAN::RElbj max_pos: 0 min_pos: -135
FakeBot thread started
yarp: created device <fakebot>. See C++ class fakebot for documentation.

We should support the "set home positions". Currently from the robotMotorGui it is impossible to reset the robot to the original pose.

I think, a part from adding the relevant lines in the config files, we should also implement something in the interface.

@barbalberto

As title

I opened this issue for defining how to pass information (name of the part, port opened, joint controlled) to the controlboard plugin.
@EnricoMingo I was unable to answer in Yammer that is not working for me.
Returning in topic:
What is the way implemented in ros_control? As far I can tell adding child elements to the tag is the standard way of passing information to the gazebo plugin, also for the ros plugins ( http://gazebosim.org/wiki/Tutorials/1.9/ROS_Motor_and_Sensor_Plugins ).

there are some PIDs inside the model, how do they interact with the torque? Is actuator dynamics inside?

I am using GAZEBO 2.0 and YARP version X.X.60/wrapper branch.

In https://github.com/robotology/gazebo_yarp_plugins/blob/master/src/fakebotIMUplugin.cc the magnetometer returns the orientation angles.
There is a reason or it is simply a mistake?

Since the robot start always in the zero configuration it is needed to have the possibility to specify an alternative initial configuration for the robot. The simulation will start with the robot already in this specified configuration. The initial configuration will be a tag for the gazebo_yarp_plugin.
Mirko already has found the way to start the simulation with the robot in another configuration, it follows the code:

for (int i=0;i<_robot->GetJointCount();i++)
{ 
gazebo::math::Angle a;
a.SetFromDegree(150);
_robot->GetJoints()[i]->SetAngle(0,a);
}
sleep(3);

I will assing the issue to me (and maybe Mirko) since I am debugging the COMAN with the forearm that start in a configuration in which there is a self collision.

The real velocity of the joint is influenced by the gravity. We should compensate it with a control loop, since the gazebo pids cannot.

The current velocity control implementation strategy needs to be checked.
Pros:

• works very nicely

Cons:

• needs a magic number for the SetMaxForce parameter, otherwise weird things happen (i.e., setting 0 velocity sometimes the joint starts to move if SetMaxForce is called using a big number). If we set SetMaxForce to the joint effort limit, the control does not work, if it is 1.1 times that number, it magically works again even though the actual torque which is commanded to the physics engine is clamped anyway to the effort limit. We need to investigate this better.
• there is a bug at the moment when switching from velocity to other control kinds, so that after using velocity control both torque and position control modes cannot be used (I suspect this could be solved easily by setting velocity ref to 0, max force to 0 when switching from velocity control to any other control)

Possible other implementation: using gazebo's JointController
Pros:

• it is a PID, so similar to the one implemented on the robot (I suppose?)
• control switching works

Cons:

• doesn't work at the moment: what PID values should we set?

I sawin the code that the velocity control is implemented (good work Mirko!) but I don't know how to run it in simulation. I tried to change control mode through:
icmd set cmv
but nothing happen. Can someone write a short tutorial on how to pass to velocity control running the testmotor executable?

Now port name in the simulation is "/coman/inertial". It is the same as in the real robot?

IMU is now implemented as a port, I have to convert it as an AnalogDevice.

Currently the torque and velocity feedback is exposed using hardcoded /coman/part/analog/torques:o and /coman/part/analog/speeds:o .

I guess that the clean way to implement this in a robot independent way is to implement the two functions:

• ITorqueControl::getTorques()
• IEncoders::getEncoderSpeeds

@EnricoMingo @MirkoFerrati @arocchi Are you using this ports? There is some more feature you need?

@barbalberto controlboardwrapper2 wraps correctly this two methods?

What if we delete that archive and create a folder with the files that should be copied inside ~/.gazebo/models?
So that when we change one of them, we only commit changes. Obviously we need to copy from the project folder to the models folder and viceversa, maybe we could also add a script for this.

It is called in yarp velocityMove, look for that

As for the IMU sensor, if possible we need to know the "real" position of the FT sensors in the kinematic chain (anyway I think that considering the FT sensors in the same pose of the ankle joints is ok...)

The yarp gazebo controlboard should have a way of knowing which joints are actuated and which are not in the simulated robot. The way this will be implemented will reflect the way the elastic transmission will be implemented, and will also affect the way the FT sensor reading are corrected from the bias of the sensing element inertia (see https://github.com/EnricoMingo/iit-coman-ros-pkg/issues/11 )

In the last updated version of YARP from Alberto we have a new devicedriver that is the controlboardwrapper2 (that is the same one present in the real robots COMAN and iCub). We have to update the simulator too. We have tried to make it works but we have some problems, I think a new branch will be needed. "Controlboard" still works, we have new ports for reading torques and velocity so in the future the extra theard to publish these informations in the simulator will be deleted. Alberto will be the one that will work on these migration so pls do not change the structure of the simulator from now on.

For using setVelocity you must set the maxforce with:

joint->SetMaxForce(0, 200);
joint->SetVelocity(0,toRad(ref));

instead, it should be read in the .sdf using:

<joint name="LElbj" type="revolute">
   ...
   <physics>.
    ...
    <ode>
     ...
       <max_force>200</max_force>
     ...
    </ode>
   </physics>
  </joint>

max_force is read by gazebo, but not used to set the internal variable. No idea why.

Since setting positions implies sending one gazebo message for each joint, while we expect the commands to arrive in a single "time step" on the real robot, we should pause the simulation before sending all the messages, then resume the simulation afterwards.

There is any method in coman.h that permits to read torque?

Ok, I think we are near a first "stable-enough" version so we have to stop the developing of new features / bug fixing in order to move the repository. I also think that this could be the opportunity to Mirko to refactor (after we move to Robotology) coman.cpp and coman.h. If you agree Alessio will commit its last changes and we "freeze" this version. We could open also a branch but maybe we could have problems in the future for merging so... Maybe we can start to use this simulator!

I was wondering if it is possible to remove the jointLogger.hpp code (for 0.2 version), that it is quite an hack? @MirkoFerrati

This is a BIG bug of the simulator. We need to find a way to synchronize the modules with the time in the simulation. If the RT factor is almost 1 is ok, otherwise simulation is totally non-sync. One solution could be to run the RFmodule and/or ratethread with the time published by the simulator.

We need to make the user feel like he can use the following methods and obtain a joint impedance control.
SetStiffness
SetDaming
SetPosition

In the real Coman this control is already implemented, so we don't care about how we implement this in gazebo.