Open traversaro opened 9 years ago
cc @hu-yue
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).
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 ).
iMotorEncoders
and the IEncoders
interfacesOnce 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
.
actuatedJoints
and elasticJoint
summed up. iMotorEncoders (currently not implemented) should instead always return the position, velocity and acceleration of the actuatedJoint
.Currently we are using the Gazebo::JointController to close the Position and Velocity PID around a (single) Gazebo Joint.
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. 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 .
cc people that could be interested in SEA simulation, even if now they are busy with DRC: @barbalberto @EnricoMingo @arocchi
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
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.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.
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
@hu-yue :DDDDD @EnricoMingo
rotor
link. 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
@hu-yue so I guess you are tryng to tackle the Modify behaviour of iMotorEncoders and the IEncoders interfaces
@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 iEncoders
does 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.
@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?
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.
@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.
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) . . .
@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:
@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.
Yes I just received the mail, thanks! When I'm done I'll do it.
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)
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?sendPositionToGazebo
,sendVelocityToGazebo
and sendTorqueToGazebo
without using JointController
, but in sendTorqueToGazebo
the SEA support is not implemented yet.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)iMotorEncoders
behaviour to check.Everything need to be tested.
getNumberOfMotorEncoders
always match getAxes
. This is quite an hidded assumption, but I prefer to stick to how the interfaces are used in real robots. Last update as per commit b21b042cbfe96941688979d6ae36fdc20b68a48b:
getNumberOfMotorEncoders
matches getAxes
as @traversaro said, and in the non SEA joints it behaves like iEncoders
.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.
@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.
@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.
cc @nunoguedelha
cc @gabrielenava
Any news on this? @traversaro
Not at the moment, but @gabrielenava was reviewing the state of the art on SEA/elastic joints simulation.
@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 =)
@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 :-(
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 theIEncoders
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
andchild_link
with proper inertia values):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 jointmotor_joint
.IEncoders::getEncoder
should return the sum of position of the jointmotor_joint
andelastic_joint
(because theelastic_joint
position is actually the spring deflection).Probably also other method should be modified.