SEA joints simulation using gazebo-yarp-plugins

[traversaro]

Currently yarp motor interfaces are used in robot equipped with SEA (Series Elastic Joints) to get "motor" encoder readings using the iMotorEncoders interface, as opposed to the IEncoders interface that is usually post-transmission encoder readings.

Apparently series elastic joints can be simulated in Gazebo (but this should be verified!) using a couple of joints, one active and one passive, as described in https://bitbucket.org/osrf/gazebo/pull-request/735/create-spring-damper-in-joints-issue-845/diff#comment-671487.

The relative sdf should be something like (with parent_link, rotor and child_link with proper inertia values):

<model name="se_test_model">
  <link name="parent_link"/>
  <link name="rotor"/>
  <link name="child_link"/>
  <joint name="motor_joint" type="revolute">
    <parent>parent_link</parent>
    <child>rotor</child>
  </joint>
  <joint name="elastic_joint" type="revolute">
    <parent>rotor</parent>
    <child>child_link</child>
    <axis>
      <stiffness>[series elastic element stiffness]</stiffness>
    </axis>
  </joint>
</model>

If this configuration works and actually properly simulates a SEA, we should then modify the gazebo_yarp_controlboard to support this configuration and modify the behaviour of various functions, for example:

• IMotorEncoders::getMotorEncoder should return the position of joint motor_joint.
• IEncoders::getEncoder should return the sum of position of the joint motor_joint and elastic_joint (because the elastic_joint position is actually the spring deflection).

Probably also other method should be modified.

[traversaro]

cc @hu-yue

[traversaro]

We did some preliminary testing with @hu-yue and it seems that the SEA implementation proposed in discussed in the previous post actually works.

I will then do a brief recap of which modification to gazebo_yarp_controlboard are then needed to match the SEA behavior in real robots (i.e. iCub v2.5 legs).

Load configurations for SEA joints

Currently gazebo_yarp_controlboard is used to implement a simple controller running on the top of a 1DOF Gazebo joint. It loads the controlled gazebo joint in the jointNames variable of the configuration file, as in https://github.com/robotology-playground/icub-gazebo/blob/master/icub/conf/gazebo_icub_left_leg.ini :

# Specify configuration of MotorControl devices
[left_leg]
# name of the device to be instatiated by the factory
device gazebo_controlboard
#jointNames list
jointNames l_hip_pitch l_hip_roll l_hip_yaw l_knee l_ankle_pitch l_ankle_roll  
name left_leg

• [x] We can think of an alternative way of specifying the jointNames, such that is possible to specify more complex options such as elastic joints, as in:

# Specify configuration of MotorControl devices
[left_leg]
# name of the device to be instatiated by the factory
device gazebo_controlboard
#jointNames list
jointNamesAdvanced ( ( (actuatedJoint l_hip_pitch) (type normal) ) ( (actuatedJoint l_hip_roll) (type normal) ) ( (actuatedJoint l_hip_yaw) (type normal) ) ( (actuatedJoint l_knee) (elasticJoint l_knee_elastic) (type elastic) ) ( (actuatedJoint l_ankle_pitch) (elasticJoint l_ankle_pitch_elastic) (type elastic) ) ( (actuatedJoint l_ankle_roll) (elasticJoint l_ankle_roll_elastic) (type elastic) ) )
name left_leg

(Clearly the syntax is quite verbose, perhaps the use of JSON for yarp configuration could help, but this is another story: https://github.com/robotology/yarp/issues/351 ).

Modify behaviour of iMotorEncoders and the IEncoders interfaces

Once we properly loaded the information that a given yarp controlboard is controling a "SEA" joint, we should modify the behaviour of the iEncoders and iMotorEncoders interfaces.

Currently iEncoder in normal mode returns the position, velocity and acceleration of the actuatedJoint.

• [x] For the SEA joint it should return the position, velocity and acceleration of actuatedJoints and elasticJoint summed up. iMotorEncoders (currently not implemented) should instead always return the position, velocity and acceleration of the actuatedJoint.

[traversaro]

Modify behavior of Position and Velocity PID

Currently we are using the Gazebo::JointController to close the Position and Velocity PID around a (single) Gazebo Joint.

• [ ] To match the behavior on real robots, we should command the torque at the actuatedJoint based on the measure returned by getEncoders() (i.e. the sum of the actuatedJoint and elasticJoint position). To actually achieve this, we should firstly move to use an PID implementation internal to the plugin, that set directly of the torque of the actuatedJoint, as discussed in https://github.com/robotology/gazebo-yarp-plugins/issues/165 and https://github.com/robotology/gazebo-yarp-plugins/issues/149#issuecomment-61615852 . Then it should be simple to achieve this "modified" PID.

Modify behavior of Torque control (direct Torque control and impedance position)

This would be much more tricky. On real robots, a torque sensor (sometimes based on the displacement of the SEA element itself) is used to close a PID on the desired torque. However, in Gazebo the torque is an input to the system (i.e. there is no motor dynamics). Then it would be tricky to implement a torque control similar to the one on the real robot. I don't have a clear idea on how to solve (at least for the behavior at equilibrium) this issue, for more information please check : https://bitbucket.org/osrf/gazebo/issue/1321/proper-joint-torque-feedback .

[traversaro]

cc people that could be interested in SEA simulation, even if now they are busy with DRC: @barbalberto @EnricoMingo @arocchi

[hu-yue]

I'm trying to implement the modifications necessary to simulate SEA joints.

• In the sdf file the joint with elastic elements will have syntax

<model name="se_test_model">
  <link name="parent_link"/>
  <link name="rotor"/>
  <link name="child_link"/>
  <joint name="l_knee_motor" type="revolute">
    <parent>parent_link</parent>
    <child>rotor</child>
  </joint>
  <joint name="l_knee" type="revolute">
    <parent>rotor</parent>
    <child>child_link</child>
    <axis>
      <stiffness>[series elastic element stiffness]</stiffness>
    </axis>
  </joint>
</model>

I prefer this syntax to the previous one where the motor side joint was named as the original joint name.

• First I've decided for a certain format to describe the joint names. In the .ini configuration file the new type of joint names should specified with tag jointNamesElastic if elastic joint are present:

jointNamesElastic (l_hip_pitch) (l_hip_roll) (l_hip_yaw) (l_knee, l_knee_motor) (l_ankle_pitch, l_ankle_pitch_motor) (l_ankle_roll)

The actual implementation parses this structure and stores separately the motor side joints. However this is a first easy and not sophisticated implementation. If no elastic joints are present, the normal jointNames should be used.

• iMotorEncoders has been implemented. For joints where there is no elastic joint ("motor side joint") specified, iMotorEncoders behaves exactly as iEncoders in the current implementation (returns values of the normal joints). iEncoders has not been modified due to the chosen convention for joints names.
• With the current implementation the controlled joints should be the actual actuated joints, i.e. the motor joints in case of SEA and the joint in case of normal joints.

Moving the robot joints with yarpmotorgui it seems that it's working fine, I think there are still many issue to take care about and I haven't tested the iMotorEncoders implementation yet. The Gazebo model I tried if iCubHeidelberg01 with SEA joints in the sdf (https://bitbucket.org/yue_hu/icubheidelberg01/branch/springs).

However I'm not sure how to tackle the problem of PIDs as well as the torque control.

I'll keep working on this in the next days since I need this feature, so any comment or so would help.

[EnricoMingo]

Hi Yue, I have a couple of question questions: The SEA element should not be implemented as a pure GAZEBO plugin? The plugin should not integrate the dynamic equation of the motor side? I like the syntax but the inertias parameters are no missing? May I help you with this SEA simulation? I am really interested but I always postponed because I think it will take a huge effort to be implemented only by one person. Best,

Enrico :)

2015-07-02 14:15 GMT+02:00 Yue Hu [email protected]:

I'm trying to implement the modifications necessary to simulate SEA joints.

• In the sdf file the joint with elastic elements will have syntax

parent_link rotor rotor child_link [series elastic element stiffness]

I prefer this syntax to the previous one where the motor side joint was named as the original joint name.

• First I've decided for a certain format to describe the joint names. In the .ini configuration file the new type of joint names should specified with tag jointNamesElastic if elastic joint are present:

jointNamesElastic (l_hip_pitch) (l_hip_roll) (l_hip_yaw) (l_knee, l_knee_motor) (l_ankle_pitch, l_ankle_pitch_motor) (l_ankle_roll)

The actual implementation parses this structure and stores separately the motor side joints. However this is a first easy and not sophisticated implementation. If no elastic joints are present, the normal jointNames should be used.

iMotorEncoders has been implemented. For joints where there is no elastic joint ("motor side joint") specified, iMotorEncoders behaves exactly as iEncoders in the current implementation (returns values of the normal joints). iEncoders has not been modified due to the chosen convention for joints names.

With the current implementation the controlled joints should be the actual actuated joints, i.e. the motor joints in case of SEA and the joint in case of normal joints.

Moving the robot joints with yarpmotorgui it seems that it's working fine, I think there are still many issue to take care about and I haven't tested the iMotorEncoders implementation yet. The Gazebo model I tried if iCubHeidelberg01 with SEA joints in the sdf ( https://bitbucket.org/yue_hu/icubheidelberg01/branch/springs).

However I'm not sure how to tackle the problem of PIDs as well as the torque control.

I'll keep working on this in the next days since I need this feature, so any comment or so would help.

— Reply to this email directly or view it on GitHub https://github.com/robotology/gazebo-yarp-plugins/issues/192#issuecomment-118011834 .

Ing. Mingo Enrico Hoffman Istituto Italiano di Tecnologia, Genova

[traversaro]

@hu-yue :DDDDD @EnricoMingo

• No, the physics engine are integrating the dynamic equation of the motor side. This enables the offloading of a lot of complexity from the plugin.
• The inertia parameters are specified in the rotor link.

[EnricoMingo]

Ok, so you need "something" that transfer the torque from the motor side to the link side through the spring right?

2015-07-02 14:52 GMT+02:00 Silvio Traversaro [email protected]:

@hu-yue https://github.com/hu-yue :DDDDD @EnricoMingo https://github.com/EnricoMingo

• No, the physics engine are integrating the dynamic equation of the motor side. This enables the offloading of a lot of complexity from the plugin.
• The inertia parameters are specified in the rotor link.

— Reply to this email directly or view it on GitHub https://github.com/robotology/gazebo-yarp-plugins/issues/192#issuecomment-118022690 .

Ing. Mingo Enrico Hoffman Istituto Italiano di Tecnologia, Genova

[traversaro]

@hu-yue so I guess you are tryng to tackle the Modify behaviour of iMotorEncoders and the IEncoders interfaces

[hu-yue]

@traversaro I mean Modify behavior of Position and Velocity PID, and of course Modify behavior of Torque control (direct Torque control and impedance position)

I think I've done the iMotorEncoders and iEncoders, actually iEncoders was not modified, but the question addressed in Modify behaviour of iMotorEncoders and the IEncoders interfaces is addressed in the code elsewhere so that iEncodersdoes the right thing without being modified.

@EnricoMingo Help is welcomed of course! I'm quite alone on this.

Btw, the rotor inertia should be taken care in the rotor link but in the Heidelberg model I don't have that yet, need to add it because I completely forgot about it.

[traversaro]

@hu-yue just saw the branch ( link for other readers : https://github.com/robotology/gazebo-yarp-plugins/compare/master...hu-yue:sea_joints?expand=1 ).

Yes I think it should work, we can tick Modify behaviour of iMotorEncoders and the IEncoders interfaces ! @barbalberto one thing : we are not sure how IEncoders is implemented in the Coman, do you know what getEncoders is returning for SEA joints in the Coman?

[traversaro]

With respect to the missing rotor inertia, I guess that if the inertia is missing Gazebo defaults to a mass of 1 Kg and an diagonal Inertia of 1 Kg m^2 .

For Modify behavior of Position and Velocity PID, It is inevitable to stop using the Gazebo JointController and start using running the PID in the plugin, as described in https://github.com/robotology/gazebo-yarp-plugins/issues/192 . This is because all JointController loops are closed over a joint, while in this case we are closing a loop over two joints! We can have a Skype chat if you want more info.

[hu-yue]

@traversaro Yes, that would be nice thanks. I'll contact you on skype for that.

With the actual implementation it is closed only on the actuated joint and is basically ignoring the elastic one, this need definitely to be changed.

[barbalberto]

On the Coman, the getEncoder function returns the value named GET_ENCODER_POSITION, the actual value depends on the firmware implementation. We should ask Phil for that. So the point is if the iEncoder is returning the value before or after the elasticity, right?

Some comments:

• In case of normal joints, not SEA, I think that iMotorEncoder interface returning same value as joint is misleading for 2 reasons: first as a user if a see an interface is available and it gives me numbers that are changing over time and may be reasonable, it leads me to think that the interface is implemented, works and the number are correct, which is not the case. Saying in a different way, I'd have no quick way to verify if the robot/joint I'm working on have SEA or normal joint. The other reason is that the iMotor is intended to return a different sensor from the iEncoder (that's why there is a different interface) and it is implemented when a different physical sensor is actually mounted on the robot, otherwise it is not implemented at all. So I do believe it is better to return false as a return value of the function call and all zeros as values.
• config file, it is better to have them as easy to write and modular as possible, so I prefer the @hu-yue version (btw all those parentesis are not required for a single list). If we need to specify the type as @traversaro was proposing a solution could be the following:

#jointNames list
jointNames l_hip_pitch l_hip_roll  l_hip_yaw   l_knee    l_ankle_pitch l_ankle_roll 
jointType     actuated    actuated   actuated    actuated     actuated     actuated  

Maybe we can use 'revolute' like in the sdf so it could be more similar (or 'actuated_revolute') and we can eventually extend it to 'linear'?

elasticJointNames l_hip_pitch_el l_hip_roll_el l_hip_yaw_el l_knee_el l_ankle_pitch_el l_ankle_roll_el 
elasticJointType  elastic_passive_revolute (or whatever) . . . 

[hu-yue]

@barbalberto I think the point is if iEncoder returns the values after the elasticity or not, because in the actual implementation we assume it is after the elasticity. I can change the behaviour of iMotorEncoders as suggested, I agree that probably this makes more sense.

@traversaro I tried a first implementation to use position PID by using gazebo::common::PID and SetForce() instead of the JointController (not pushed yet), it seems that for the normal iCub model this works fine, however for the Heidelberg model the robot is going crazy on the torso (and also in the legs with springs). This makes me wonder if:

• the gains of Heidelberg need to be changed (but then why did they work before)
• the implementation is wrong, or there are other issues to take care of as we talked about the last week, and it's just luck that it works on the normal iCub

[traversaro]

@hu-yue you should now be able to push the branch directly on the main repo, so it is easier if we need to do some fixes.

[hu-yue]

Yes I just received the mail, thanks! When I'm done I'll do it.

[hu-yue]

So to summarize the current situation:

• I pushed the branch sea_joints: https://github.com/robotology/gazebo-yarp-plugins/tree/sea_joints
• In case of SEA joints, the syntax to use in the configuration file is different and is as in my previous comment, which means that at the moment the brackets are still needed, but a better version can be implemented later.

jointNamesElastic (l_hip_pitch) (l_hip_roll) (l_hip_yaw) (l_knee, l_knee_motor) (l_ankle_pitch, l_ankle_pitch_motor) (l_ankle_roll)

• In this branch the iMotorEncoders has been implemented here https://github.com/hu-yue/gazebo-yarp-plugins/blob/sea_joints/plugins/controlboard/src/ControlBoardDriverMotorEncoders.cpp, however I discussed the implementation with @traversaro and we are not so convinced. Mainly the following issue does not convince me:
  • Now as @barbalberto suggested iMotorEncoders returns 0 and false on the joints where the motor side joint is not present, however, the indexing is as in the actual joints. I mean: to get/set the motor encoder value of for example l_knee_motor, the index to use is the same as l_knee. But the total number of motor encoders, as returned by the function getNumberOfMotorEncoders is the real number of motors, which is clearly smaller than the number of actuated joints. I think this is somehow misleading. I don't know how it is supposed to be in real robots?
• New implementation of sendPositionToGazebo,sendVelocityToGazebo and sendTorqueToGazebo without using JointController, but in sendTorqueToGazebo the SEA support is not implemented yet.
• ~~With the new implementation of sending commands to Gazebo, it seems that the old PID gains are not good anymore (too high) and the robot has jerky motions, with very big errors. This is probably due to the fact that in the previous implementation there were some delay that are not present in the new version. This means that probably all the robots need a re-tune of the gains.~~

The unsolved issues in order of priority:

• [ ] sendTorqueToGazebo is implemented only for normal joints but not for SEA (https://github.com/hu-yue/gazebo-yarp-plugins/blob/sea_joints/plugins/controlboard/src/ControlBoardDriver.cpp#L577)
• [x] iMotorEncoders behaviour to check.
• [ ] The problem of PID gains as in issue #119 is unsolved, in particular in the configuration files the order is still P, D, I instead of P, I, D and this is misleading.
• [ ] Specify joint type in the configuration file as suggested by @traversaro

Everything need to be tested.

[traversaro]

• configuration files : I actually like very much the configuration format proposed by @hu-yue , KISS for the moment is a good idea.
• iMotorEncoders In my opinion we should try to match the behavior on the real robot, and whenever it is implemented getNumberOfMotorEncoders always match getAxes. This is quite an hidded assumption, but I prefer to stick to how the interfaces are used in real robots.

[hu-yue]

Last update as per commit b21b042cbfe96941688979d6ae36fdc20b68a48b:

• Differently from what said before, the PID gains don't need to be changed, there was a wrong implementation before that has been corrected now.
• The behaviour of iMotorEncoders has been reverted to the previous version, i.e. now getNumberOfMotorEncoders matches getAxes as @traversaro said, and in the non SEA joints it behaves like iEncoders.

[traversaro]

The getNumberOfMotorEncoders matches getAxes was actually discussed with @randaz81 . For matching the behavior on the real robots, in which the IMotorEncoders interface is returning measurements before the gearbox, it will be also useful to add a gearbox_ratio option, that will multiply the value returned by IMotorEncoders. Anyway I think that is is easier to move this modifications and the PID cleanup to a future pull request.

It is also worth mentioning that it is planned (in a indefinite time is the future) that Gazebo will support modeling and simulation of arbitrary actuators, so eventually we could migrate this functions to use what Gazebo will offer.

[traversaro]

@hu-yue sorry for not having updated this issue in the past week. I would like to merge at least the part where we remove the use of the Gazebo JointController (even if the SEA support is not complete) but it is quite a bit change and I am a bit afraid that this could break things in Coman simulation. Unfortunately the VVV Summer School is going to start in two days.. perhaps the best idea is that we work on the sea_joints branch as a "develop" branch, and we will merge after the Summer School. I see that @mikearagao started working on its own branch for Gaze support, perhaps he can be interested to work on this "develop" branch.

[hu-yue]

@traversaro it's ok for the "develop" option, I am also leaving for conference for one week, so I will have time to keep working on it afterwards. So it's fine to merge after the summer school.

[traversaro]

cc @nunoguedelha

[traversaro]

cc @gabrielenava

[arocchi]

Any news on this? @traversaro

[traversaro]

Not at the moment, but @gabrielenava was reviewing the state of the art on SEA/elastic joints simulation.

[hu-yue]

@traversaro @gabrielenava I am still interested, I just didn't have time, but I might have time in the next months to review these stuff again, if you are working on it would be nice to have an update =)

[gabrielenava]

@hu-yue sorry for the very late reply. In the end, I just developed a Matlab simulation environment for robots with elastic joints based on mex-wholeBodyModel library. I didn't work with Gazebo; even if this might be a future activity, nothing has been scheduled jet. So I'm sorry for now there are no news at all :-(