Proper joint torque feedback

osrf-migration commented 9 years ago

Original report (archived issue) by Silvio Traversaro (Bitbucket: traversaro).

I have several questions regarding how to properly implement joint torque feedback in Gazebo. I don't know if the bug tracker is the right place to post this questions, but given that I will get in the internal details of Gazebo and this question could lead to some new physics-related test, I prefer to post here instead of posting in Gazebo Answers. To keep the discussion simple I will refer only to revolute joints, but the concepts can be generalized to other types of joints. I also disregard joint stiffness, mainly because I don't have a clear idea in my mind on how it is implemented in Gazebo. Sorry if I probably write obvious notions for stating my point, but I prefer to clearly define concepts as much as possible to avoid misunderstandings.

At this moment, Gazebo is not simulating any kind of motor dynamics, but it is simulating joint friction (currently only viscous friction model, with an additional Coulomb/dry friction tem is being implemented in issue 381 (#381) ).

Disregarding torque limits, we can then define the torques applied by any controller as:

the motor torque (i.e. the the quantity setted by the JointController with the Joint::SetForce() method).

This motor torque is then equal to the difference (or addiction, depending on how you define the different components) between:

the joint torque (i.e. the torque that, for a 1-DOF fixed base robot with no gravity and no external force, we find on the left side of the dynamics equation I \ddot{q} = \tau) and
the friction torque (i.e. the torque opposing the motion due to friction).

Currently the joint torque feedback is implemented in gazebo_ros_control (and also in our gazebo-yarp-plugins) using the Joint::GetForce() method. This method, as far as I was able to test (the documentation says that it is not implemented, so I guess it is simply not updated), just returns the last value set through Joint::SetForce(). Using the notation used in this issue, this function returns the motor torques. However, for proper force control, we are interested in joint torque.

To see why we want to control the joint torques let's take as an example a joint controlled with a classical impedance control scheme: \tau = K(q_m-q_d), where:

\tau is the controlled torque (Nm)
K is the impedance stiffness (Nm/[rad])
q_m is the measured position of the joint ([rad])
q_d is the desired position of the joint ([rad])

Let's imagine that a Coulomb friction of 1Nm is acting on the joint, and the impedance stiffness is set to 1 Nm/[rad]. If the controlled torque is the motor torque, we will have a deadband of 1 [rad] around the desired position, where no actual joint torque is exerted on the robot. This because in the deadband all the commanded torque are lower then 1 Nm, and so they are counterbalanced by the Coulomb friction torque.

What usually happens in real robots is that an inner loop for controlling joint torque by commanding appropriate motor torques, using some kind of joint torque feedback (SEA springs, strain gauges, ...). At the best of my knowledge, there is no clean way to get joint torques in Gazebo. A possible solution that should work is to use the output of Joint::GetForceTorque() and to project it on the joint axis, but I still have to test it.

Even if this works, do you think it could make sense to have a proper method for the physics::Joint class for returning joint torques?

osrf-migration commented 9 years ago

Original comment by Silvio Traversaro (Bitbucket: traversaro).

Edited issue description

osrf-migration commented 9 years ago

Original comment by Stefan Kohlbrecher (Bitbucket: Stefan_Kohlbrecher).

@traversaro: Did you implement the GetForceTorque() approach? We're currently also looking into the issue and I have not found a way of getting joint torque apart from using GetForceTorque() and wrapping computation for projection on the joint axis around it.

osrf-migration commented 9 years ago

Original comment by Silvio Traversaro (Bitbucket: traversaro).

@Stefan_Kohlbrecher Actually I did not progress further on this issue.

Currently we are simulating the low level torque control with just a simple call to Joint::SetForce(), even if in our real robot (the iCub humanoid) we have a joint torque control with feedforward friction compensation and a feedback control closed on a measurement of the joint torque.

Anyway I worked a bit on implementing some new tests for the Joint::GetForceTorque() as part of the implementation of fixed joints (in https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/1747/support-for-fixed-joints/diff). Unfortunatly all this tests are in conditions where the bodies don't move. To actually asses the behaviour of Joint::GetForceTorque() for getting joint torques the easiest way would be to set up a simple test scenario of a moving joint, for example a classical pendulum rigidly connected to the world in zero gravity. In that case we could control the joint to spin at constant velocity, and in this simple case we would know the analytical values for viscous friction, coulomb friction (and consequently motor torque and joint torque) and we could check the value returned by Joint::GetForceTorque(). Unfortunatly at the moment I don't have time to do this in the short term, but I would be happy to help if you want try to implement such a test.

Anyway I saw that there is an ongoing gazebo_design document related to simulate transmission and motor properties at #!/osrf/gazebo_design/branch/transmission . I don't know if OSRF folks are already happy to receive a feedback on it, but it is definitly relevant to this issue.

FYI, we are also tryng to understand to which extend we could simulate SEA joint torque control with the existing gazebo infrastructure, our (downstream) issue relevant to this topic is : https://github.com/robotology/gazebo-yarp-plugins/issues/192 .

osrf-migration commented 8 years ago

Original comment by Jonathan Vorndamme (Bitbucket: vorndamme).

I tried using

#!c++

Joint::GetForceTorque()

on the drcsim atlas robot version 5 without controllers to get the joint torques, but it returns 0 for all joints except the last x-joints in every limb, where it return ridiculously high values. So this does not look to me, as if it would work.

osrf-migration commented 8 years ago

Original comment by Jonathan Vorndamme (Bitbucket: vorndamme).

I had to configure all joints to provide feedback (via Joint::SetProvideFeedback(true)) to get return values for all joints. I looked in the implementation of GetForceTorque() for odeHingeJoints, and i think, there is an error in the implementation, because the joint torque is calculated in the child link frame and subtracted in the parent link frame without transformation.

Also, when testing the approach, i recognized, that SetForce() sometimes has no effect (see https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/issues/1848/setforce-on-joint-has-no-effect (#1848)).

osrf-migration commented 8 years ago

Original comment by Silvio Traversaro (Bitbucket: traversaro).

@vorndamme can you link the line of code in which you think you found that problem?

It would not surprise me if there is a bug like that: the test case present in the moment are not checking really dynamic situations. I think that a simple test scenario of a moving joint, for example a classical pendulum rigidly connected to the world in zero gravity would be the best way of debug the problem.

osrf-migration commented 8 years ago

Original comment by Jonathan Vorndamme (Bitbucket: vorndamme).

it's https://github.com/osrf/gazebo/blob/7cb38ac8f823c49c3aafa5007236010d8970d98f/gazebo/physics/ode/ODEJoint.cc in line 656 and 657,

#!c++

 wrenchAppliedWorld.body2Torque =
        this->GetForce(0u) * this->GetLocalAxis(0u);

the wrench according to the setted force is calculated in child link frame as it uses GetLocalAxis(), the negative value is set for the parent joint in line 663

#!c++

wrenchAppliedWorld.body1Torque = -wrenchAppliedWorld.body2Torque;

and in line 825 this wrench is added to the overall result

#!c++

this->wrench = this->wrench - wrenchAppliedWorld;

but the result is in the respective jointframes (see lines 711-717

#!c++

// rotate resulting body2Force in world frame into link frame
      this->wrench.body2Force = childPose.rot.RotateVectorReverse(
        -this->wrench.body2Force);

      // rotate resulting body2Torque in world frame into link frame
      this->wrench.body2Torque = childPose.rot.RotateVectorReverse(
        -this->wrench.body2Torque);

and lines 770-776

#!c++

      // rotate resulting body1Force in world frame into link frame
      this->wrench.body1Force = parentPose.rot.RotateVectorReverse(
        -this->wrench.body1Force);

      // rotate resulting body1Torque in world frame into link frame
      this->wrench.body1Torque = parentPose.rot.RotateVectorReverse(
        -this->wrench.body1Torque);

), which means in line 825 we add a torque expressed in child frame to a torque expressed in parent frame.

osrf-migration commented 8 years ago

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

pull request #2110

osrf-migration commented 8 years ago

Original comment by Silvio Traversaro (Bitbucket: traversaro).

For the sake of precision: pull request #2110 is a PR that is solving a bug that was reported in this issue, but it is not solving the issue itself. I will write a brief recap for people that does not want to read all the issue.

TL;DR: This issue is about the simulation of joint torque sensors that measure the torque exerted by the output shaft of the transmission to the robot, i.e. the joint torque. This sensors are usually implemented by measuring the deformation of an elastic element mounted between the trasmission and the robot. This deformation can be measured using a pair of encoders (as in Valkyrie [1] or iCub [3] ) or using straingauge sensors (as in the DLR LWR [2]). Depending on the elasticity of the deformed element and its effect on the robot dynamics, it could make sense to explicitly model the elastic element in the simulation as a joint elasticity. In this case, the joint torque can then be obtained from simulation using the measure of the two simulated encoders [4][5]. If the elastic element is too stiff to be efficiently simulated, it would then make to get the joint torques directly from the physics engine as the difference between the torque given as input to the joint (the motor torque) and the friction torques.

[1] : http://www.me.utexas.edu/~hcrl/publications/JFR-Valkyrie-Actuator-Control-Final.pdf

[2] : http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=1014788

[3] : http://pasa.lira.dist.unige.it/pasapdf/1245_Parmiggiani_etal2012.pdf

[4] : https://github.com/robotology/gazebo-yarp-plugins/issues/192

[5] : https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/735/create-spring-damper-in-joints-issue-845/diff#comment-671487

osrf-migration commented 7 years ago

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

set version to "all"

osrf-migration commented 7 years ago

Original comment by Ian McMahon (Bitbucket: IanTheEngineer).

+1 on this issue. I've actually been attempting to track down an answer on this topic for the past few weeks [1]. I too want to simulate SEA's to properly compute the amount of torque experienced by each joint, so I'm thrilled to find this issue and your links, @traversaro. @nkoenig and @scpeters, any thoughts on how to add SEA joint-torque feedback as a Gazebo plugin, or as part of the gazebo_ros_control's robot_hw_sim hardware_interface?

[1] : http://answers.gazebosim.org/question/15987/understanding-gazebophysicsjointgetforce-return-wrench/

gazebosim / gazebo-classic

Proper joint torque feedback #1321