External torque estimation, URDF refining #41

marcoesposito1988 · 2015-06-30T10:30:59Z

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 Publish (possibly) interesting data provided from FRI #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.

Not working yet

marcoesposito1988 · 2015-06-30T14:52:52Z

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 · 2015-07-13T15:30:20Z

@marcoesposito1988 how are you doing with this?

marcoesposito1988 · 2015-07-14T13:16:12Z

@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 · 2015-07-14T13:41:19Z

@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 · 2015-07-15T14:04:17Z

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 · 2015-07-16T09:02:08Z

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 · 2015-07-16T12:40:34Z

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

Agree

carlosjoserg · 2015-07-16T14:42:43Z

lwr_controllers/src/arm_state_controller.cpp

+                for (unsigned i = 0; i < joint_handles_.size(); i++) {
+                    (*joint_position_)(i) = joint_handles_[i].getPosition();
+                    (*joint_velocity_)(i) = joint_handles_[i].getVelocity();
+                    (*joint_acceleration_)(i) = 0; 


The velocity is obtained by derivation of the position, and then smooth filtering, why don't you do the same for the acceleration?

Because I'm lazy 😇

I added it. Does it look correct?

You mean like the whole approach?

Yes, sure, I don't exactly how the KDL solver works, it looks like it gives you all wrenches at all links directly, wonderful.

carlosjoserg · 2015-07-17T07:55:51Z

lwr_controllers/src/arm_state_controller.cpp

+                int ret = id_solver_->CartToJnt(*joint_position_, 
+                                                *joint_velocity_, 
+                                                *joint_acceleration_, 
+                                                *joint_wrenches_,


But I'm reviewing still...

According to the api, this joint_wrenches_ is an input, right? Where do you update it?

I suppose you are using joint_wrenches_= 0 always, so it is just to estimate the torque at the joints from the model, is that it?

If so, I think that the class you should use is ChainDynParam, I couldn't find a forward dynamic solver.

Yes, I use the inverse dynamics solver to find the expected joint torques given the current position, velocity and acceleration of the robot. Then I subtract the expected from the measured joint torques to find the estimated external joint torques.

So joint_wrenches is input, and I set it to zero to find the expected joint torques when there is no object applying a force on the robot.

I think that the inverse dynamics solver is a higher-level solver than ChainDynParam, and is closer to what we want. Or do you see any problem with this approach?

No, it's fine, I was just wondering that if you never consider external forces (joint_wrenhes_ = 0), perhaps there was no need to use the solver, because you are not actually solving neither a FD nor an ID problem, also because the motion is measured not planned, but the numbers are the same as in an ID problem.

So, as you wish, no problem, let me know when to merge.

Though, I'd change the var name in case we ever use it, since it is not joint wrenches as if the joint generated the wrenches, but wrenches at the segments, I believe expressed in the corresponding joint frame from this, it doesn't say that, only:
f_ext The external forces (no gravity) on the segments Output parameters:
anyways, it is zero in our case.

Ok. I'm still playing with it because today I get very crappy performance out of the controller. I don't know if I changed something wrong; in the meantime I realised that I was computing the acceleration in a very wrong way 😇 so I fixed it

And I might have found the reason.. I think that the numerical stability was due to using the damped pseudo-inverse of the Jacobian. Somehow the change did not make it into the git commit.

I would say that we can test it a bit more, and then merge it.

marcoesposito1988 · 2015-08-25T15:09:07Z

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 · 2016-04-19T17:14:36Z

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 · 2016-10-10T13:12:21Z

Closing this one in favor of #52

Thanks @kjyv for the reference

marcoesposito1988 added 5 commits June 25, 2015 14:36

Started working on ArmStateController

c9b21d4

Compiling ArmStateController

acbd69d

Not-exploding ArmStateController

654ee12

Updated URDF link mass

c8b1007

WIP: estimation of cartesian wrench on end effector

6f31382

Not working yet

Transform est_ext_wrench to tool reference frame

80f5a42

Using jacobian transpose pseudo-inverse

73e4211

Added external torque in base reference frame

030653e

carlosjoserg reviewed Jul 16, 2015
View reviewed changes

Derive acceleration through exp smoothing

233a33e

carlosjoserg reviewed Jul 17, 2015
View reviewed changes

marcoesposito1988 added 2 commits July 20, 2015 14:58

Fix bad copy-paste!

b962cff

Use damped SVD decomposition for better stability at singularities

5c824d6

carlosjoserg added the in review label Jul 27, 2015

marcoesposito1988 mentioned this pull request Aug 25, 2015

Arm state controller merge #52

Closed

marcoesposito1988 added 3 commits September 8, 2015 19:13

Optional rviz for moveit in single_lwr.launch

6645055

Fix parenthesis in lwr_description/utils.xacro

d5d8fb8

Fixed damping initial value to avoid vibrations

15074f4

Fixed compilation error on 16.04

21fc0b4

carlosjoserg closed this Oct 10, 2016

carlosjoserg removed the in review label Oct 10, 2016

marcoesposito1988 deleted the arm_state_controller branch January 24, 2017 10:04

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

External torque estimation, URDF refining #41

External torque estimation, URDF refining #41

marcoesposito1988 commented Jun 30, 2015

marcoesposito1988 commented Jun 30, 2015

carlosjoserg commented Jul 13, 2015

marcoesposito1988 commented Jul 14, 2015

marcoesposito1988 commented Jul 14, 2015

carlosjoserg commented Jul 15, 2015

marcoesposito1988 commented Jul 16, 2015

carlosjoserg commented Jul 16, 2015

carlosjoserg Jul 16, 2015

marcoesposito1988 Jul 16, 2015

carlosjoserg Jul 17, 2015

carlosjoserg Jul 17, 2015

carlosjoserg Jul 17, 2015

carlosjoserg Jul 17, 2015

marcoesposito1988 Jul 20, 2015

carlosjoserg Jul 20, 2015

marcoesposito1988 Jul 20, 2015

marcoesposito1988 Jul 20, 2015

carlosjoserg Jul 20, 2015

marcoesposito1988 commented Aug 25, 2015

kjyv commented Apr 19, 2016

carlosjoserg commented Oct 10, 2016

External torque estimation, URDF refining #41

External torque estimation, URDF refining #41

Conversation

marcoesposito1988 commented Jun 30, 2015

marcoesposito1988 commented Jun 30, 2015

carlosjoserg commented Jul 13, 2015

marcoesposito1988 commented Jul 14, 2015

marcoesposito1988 commented Jul 14, 2015

carlosjoserg commented Jul 15, 2015

marcoesposito1988 commented Jul 16, 2015

carlosjoserg commented Jul 16, 2015

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

marcoesposito1988 commented Aug 25, 2015

kjyv commented Apr 19, 2016

carlosjoserg commented Oct 10, 2016