Bring lwr_controllers back to working condition (WIP)

[marcoesposito1988]

Changes made:

• OneTaskInverseKinematics: don't rely on kdltree.getNumberOfJoints() because of new stiffness joints in URDF, but divide it by two
• make the kinematic chain start at box instead of world (as it was in old URDF)
• put a fixed joint between world and box, so that the entire robot doesn't wiggle
• restore the controllers argument to single_lwr.launch

Still, the robot doesn't move. What can be missing? Actually I haven't been able to move the robot in Gazebo with lwr_controllers since long before the Merge.

[carlosjoserg]

Still, the robot doesn't move. What can be missing? Actually I haven't been able to move the robot in Gazebo with lwr_controllers since long before the Merge.

I think all lwr_controllers, at least the OneTaskInverseKinematics for sure, are effort_controllers, they need an EffortJointInterface to be specified in the transmission, that is, here, that's the motivation behind #19

[marcoesposito1988]

Yes, it looks way better with the chain! I will start working on a xacro parameter for the transmission. Thanks!

BTW, maybe a bit more documentation about each controller in lwr_controllers might be useful, or at least a reference to understand what it does. I didn't manage to get much information about it on my own. And also about the status (deprecated, working only in simulation, working on real robot).

And maybe we could give them standard interfaces (at least for OneTaskInverseKinematics).

[marcoesposito1988]

So I removed the world link.

But in controllers.yaml I put lwr_base_link as root, otherwise the joint box->base is also in the chain and the whole robot vibrates.

Is it ok?

[carlosjoserg]

Vibrates? For the OneTaskInverseKinematics I suppose? It shouldn't matter, the base link is fixed to the box. Do you have a video?

And maybe we could give them standard interfaces

What do you mean? What I think it can be done is to templatize them, like the joint_trajectory_controller.

a bit more documentation about each controller in lwr_controllers might be useful

I agree, but I didn't follow that work closely. However, I believe the controller names are known in common control textbooks.

[marcoesposito1988]

With "vibrates" I mean that the whole robot (box included) vibrates on the z axis. It looks like the box is considered by gazebo as a mobile object, not fixed in space.

With standard interface I meant to give them a followJointTrajectory interface, yes. But just when that makes sense; I see some controllers accept a Pose as destination, other don't. What I wanted to ask you is how you use controllers that apparently don't take any input (e.g. backstepping).

[carlosjoserg]

It looks like the box is considered by gazebo as a mobile object

It is a mobile object. One thing that can be done is increase mass and inertia of the box. It could also be fixed easily, but I prefer any element spawned into gazebo to be mobile for generality.

how you use controllers that apparently don't take any input

I see. I think those are the last controllers that Enrico developed and therefore are still to be completed, that is, to give them a command interface. For instance, the backstepping controller can potentially receive a Cartesian trajectory as a setpoint

[marcoesposito1988]

I think that the vibration was an undesired effect, deriving from including the first link (box) in the kinematic chain. Gazebo simulated white noise on the orientation of the "joint" between the world and the box.

With the current configuration (root: lwr_base_link, tip: lwr_7_link), I get this output:

[ INFO] [1433931715.884252439, 0.373000000]: Number of segments: 8
[ INFO] [1433931715.884273073, 0.373000000]: Number of joints in chain: 7

Which looks fine, I guess.

[marcoesposito1988]

Oh sorry, that output comes from default_lwr_hw_sim.cpp.

Looks like there is also a problem there. It just assumes the first segment in a KDL tree to be the base.

[carlosjoserg]

Nope, it gets the root segment from the robot_description param. In the single_lwr_example, it should be getting the box and not the base.

[marcoesposito1988]

Yes, but is this assumption safe for any URDF?

[carlosjoserg]

Hard to say. I would say no. Only one non-fixed joint between the root and the first joint, and everything is wrong.

That is hard to solve within the code, perhaps it would require external parameters.

An option could be to use the joint names to extract the robot chain.

[marcoesposito1988]

I am still trying to get an effort controller to work in Gazebo (as I plan to test a custom one), but without luck so far. As an experiment, I changed the transmission to EffortJointInterface, I added back an effort_controllers/JointTrajectoryController and selected it into the moveit config's controllers.yaml. No complains anywhere, but the robot just doesn't move.

May it be because in lwr_hw_sim the stiffness command is used to compute the effort? What happens when there is no stiffness controller, is it zero?

[carlosjoserg]

This is what's being sent to the simulation. All interfaces are exposed, that is, effort, position and stiffness.

And these are the initial values for all command interfaces. but "What happens when there is no stiffness controller", then this effort should be zero, and I believe we found the problem. The position command is zero but the current position is not, so you are having a spring pulling you back to 0 in all joints with stiffness 2500 (#sigh, sorry about that).

One option could be to initialize the stiffness to 0, but then when using the real robot, it will start in a gravity compensation-like mode, as you mention here #14, and if you move the robot, and later increase the stiffness, it will go to the reference position drastically.

As always, in my opinion, the best solution is to resolve #19 by creating the four interfaces (4 + 3 reasonable modes to work with the arm actually, the 3 means to cancel the gravity term in the impedance-based modes), so you can initialize and configure controllers properly according to the selected one.

[enricocorvaglia]

Just for the record, regarding the controllers that don't take any input, it's because they have a trajectory hardcoded in them (like circle or 8-figure) for trajectory-tracking experiments. I didn't have the time to manage those trajectories as input parameters, but I believe that with little tweaks/modifications you will be able to implement it.

Best Enrico