search

robotology / gazebo-yarp-plugins

Plugins to interface Gazebo with YARP.
33 stars 49 forks source link

SEA joints simulation using gazebo-yarp-plugins #192

Open traversaro opened 9 years ago

traversaro commented 9 years ago

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:

Probably also other method should be modified.

traversaro commented 9 years ago

cc @hu-yue

traversaro commented 9 years ago

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
# 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.

traversaro commented 9 years ago

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.

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 commented 9 years ago

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

hu-yue commented 9 years ago

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

<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.

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.

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 commented 9 years ago

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 notifications@github.com:

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 commented 9 years ago

@hu-yue :DDDDD @EnricoMingo

EnricoMingo commented 9 years ago

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 notifications@github.com:

@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 commented 9 years ago

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

hu-yue commented 9 years ago

@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 commented 9 years ago

@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 commented 9 years ago

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 commented 9 years ago

@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 commented 9 years ago

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:

#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 commented 9 years ago

@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:

traversaro commented 9 years ago

@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 commented 9 years ago

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

hu-yue commented 9 years ago

So to summarize the current situation:

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 unsolved issues in order of priority:

Everything need to be tested.

traversaro commented 9 years ago
hu-yue commented 9 years ago

Last update as per commit b21b042cbfe96941688979d6ae36fdc20b68a48b:

traversaro commented 9 years ago

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 commented 9 years ago

@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 commented 9 years ago

@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 commented 9 years ago

cc @nunoguedelha

traversaro commented 7 years ago

cc @gabrielenava

arocchi commented 7 years ago

Any news on this? @traversaro

traversaro commented 7 years ago

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

hu-yue commented 7 years ago

@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 commented 7 years ago

@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 :-(