External torque estimation, URDF refining

[marcoesposito1988]

This pull request contains:

• an ArmStateController which publishes the estimated torque acting on the robot joints, based on the dynamic model contained in the URDF (to be extended as in issue #38)
• A refining of the links mass, in order to improve the external torque estimation

The current status is a precision in estimating external torques around +/-1.5Nm, with no tool attached to the robot. In other words, the maximum torque that is estimated for the robot with nothing attached in various positions is +/-1.5Nm. This should be a good starting point, since the internal torque estimation of the Kuka controller also has a comparable precision.

[marcoesposito1988]

Added (not working yet) estimation of cartesian wrench on end effector based on the estimated external torque and the jacobian.

This should theoretically work, but does not (yet). The reason may be that the dynamic model is still totally wrong, and even if the estimated external torques look good, they lead to totally faulty computations.

[carlosjoserg]

@marcoesposito1988 how are you doing with this?

[marcoesposito1988]

@carlosjoserg: apparently getting closer. I am playing with weights to test the force estimation, and the results are less absurd after the last commit.

I am trying to understand if there is still a systematic error, and if this can be removed. Of course the dynamic model is just a guess, so there could be endless improving on that side. But I hope that the maths and the code are right.

[marcoesposito1988]

@carlosjoserg: from these preliminary tests (done by attaching a ~1Kg mass with a rope) I get quite consistent results.. at least, as consistent as they can be given a ~1N noise on each joint. In particular the noise of the first joints is multiplied by the length of the arm, so this can have a very big impact on the performance.

For debugging I also included the estimated external force in the robot base reference frame, and I compared the result to the estimated weight-force of the attached object to the force along Z. The value never exited the range 12-16N. Most importantly, this even held at singular positions (where the internal torque_tcp_est variable completely got crazy).

If you are interested I can also commit the estimation in the base reference frame.

[carlosjoserg]

Of course the dynamic model is just a guess, so there could be endless improving on that side.

But the dynamic model shouldn't affect for a wrench estimation at the TCP if the robot is still, right? Or are you estimating while moving?

Most importantly, this even held at singular positions (where the internal torque_tcp_est variable completely got crazy).

This is nice.

If you are interested I can also commit the estimation in the base reference frame.

I don't know how yet, but it would be nice if one can just ask the estimated wrench at any frame in the arm, not necessarily the TCP, and also express everything at any requested reference frame as well. Oly that probably this would need a tf listener.

[marcoesposito1988]

As of now I am only experimenting while keeping the robot still, yes. But I might use this with the robot moving as well in the future. What I meant is that the round numbers we put in the URDF might differ for very small quantities from the actual values, which may lead to bigger error in the final estimation.

Still, the more I play with this, the happier I am. The torque still looks a bit overestimated, but the cartesian force looks really proper.

Computing at arbitrary reference frames would require a TF listener and maybe a service; I would rather put this in an external node. Still, we might publish the force in the two most important frames, i.e. tool and base. What do you think?

[carlosjoserg]

Still, we might publish the force in the two most important frames, i.e. tool and base. What do you think?

Agree

[marcoesposito1988]

I think this is the best we can get. Compared to the internal KRL variable named "torque_tcp_est", forces are a bit underestimated, but the controller is way less noisy when no external force is acting on the robot. It also does not get crazy at singularities.

So I would vote for merging.

[kjyv]

You may want to have a look at the masses in the following document, they improve the estimation for me over the current values and also over the ones suggested in this PR. http://www.coppeliarobotics.com/contributions/LBR4p_dynamic_model.pdf (i.e. 1.6, 2.7, 2.7, 2.7, 2.7, 1.7, 1.6, 0.3) I compared measured torques with predicted ones for a few trajectories.

[carlosjoserg]

Closing this one in favor of #52

Thanks @kjyv for the reference