About using soft_limits

[guihomework]

The Shadow hand has mechanical limits larger than urdf limits. For instance, due to external forces, the xFJ3 could become negative when the lower limit is 0.0.
This could lead to problems with MoveIt! thinking some joints are out of bound, or in trajectory controllers expecting joints targets within the range but starting way-point is out of range already.

We started to think about using the optional soft_limits and have the urdf store the hard limits (mechanical) http://wiki.ros.org/urdf/XML/joint
There is a discussion started there moveit/moveit#867 to evaluate if MoveIt! should support that.
Of course a lot of tools rely on the joint limits, and this might become an issue if the hard limits are set there.

@toliver What do you think regarding the shadow hand and specifically the control side? The controller common code https://github.com/shadow-robot/sr_core/blob/kinetic-devel/sr_mechanism_controllers/src/sr_controller.cpp#L101 could try read the optional soft limits and revert to the urdf joint limits if not found, to be backward compatible.

[Rocketmagnet]

The hand was designed before MoveIt came along. Originally, we intended the spec of the hand to cover the full mechanical range of the joints. However, in some circumstances, the hand struggled to reach the spec angles (e.g. if it was holding a weight or something). So we decided to spec a smaller range, which we were confident the hand would always be able to reach.

But then MoveIt came along, and was surprised that this robot is not like other robots, with strictly defined joint limits. Personally, I think it is MoveIt that should change, and should be more accepting of different types of robots, including those with non-strictly defined joint limits.

[guihomework]

Sounds reasonable that the hand has smaller controllable limits, but how can MoveIt! (or other software reading the hand description) guess, that -5 degrees on xFJ3 is actually valid, the hand can be there, although not in a controllable fashion ?

Hence I would suggest to define the hard limits somewhere, but use the soft limits as they are currently, for the controllers to restrict their control range to the one you are confident works always.

Shadow has its own controllers, so should be easy to implement without relying on ros_controllers package to update their way of handling these limits. The urdf could then be update to describe the two limits, hard and soft limits.

[Rocketmagnet]

Couldn't MoveIt be modified to accept two ranges: A controllable range, and an acceptable range?

[guihomework]

This is exactly what we would like to do, and I have great hopes that MoveIt! will indeed handle two ranges, but the URDF needs to contain these two ranges at all. Shadow would need to change their URDF and then handle these two ranges (or at least read the correct one) in its controllers. for instance

 <joint name="${prefix}${joint_prefix}J3" type="revolute">
      <parent link="${parent}" />
      <child link="${prefix}${link_prefix}proximal" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <axis xyz="1 0 0" />
      <limit lower="${-20.0*M_PI/2}" upper="${M_PI/2}" effort="2"
      velocity="2.0" />
      <dynamics damping="0.1" />
     <safety_controller k_velocity="10" k_position="15" soft_lower_limit="0.0" soft_upper_limit="${M_PI/2}" />
</joint>

There are already tools that parse the existing soft_limits and this is a very small change in the controller to read one if available, and use the other one if not (backward compatibility). Not sure what other software would be affected though. Probably the sr_gui_joint_slider, but the rqt_joint_trajectory_controller GUI already handles the soft limits and would not create slider beyond the soft limits.

I think I will give it a try on our hands to see what breaks, but it is still interesting to know what the ROS developer at your side think about it.

If this works I can provide a PR, even on the software that affect my work. The rest would be left to shadow developers to adjust the same way I did.

[toliver]

@guihomework sorry for my late answer.

What you're proposing makes sense, and it would be the right way to do it.

I have noticed an issue though, that we would need to address. Currently the function get_limits() returns the maximum value so effectively it doesn't limit the value that comes from the controllers.

The moment you add the safety element to the URDF, this function will return an effort limit, computed taking into account the current position and velocity. This will produce a slightly different behaviour than the one you have currently.

This would also mean that the effort limits that we have in the URDF, that are supposed to be in Nm (assuming that they represent a torque limit), would be inadequate now, as the commanded_effort that we are limiting is expressed in the raw uncalibrated units of the strain gauges.

And in the case of Shadow, we normally use commands that are PWM, not effort, so again we would need to define a different number for the limit.

One way around this would be to remove the application of the limit here, considering that we are already applying limits elsewhere. the problem is that this is not ros_control friendly, as the ros_control controllers don't apply limits and these are supposed to be applied in the write() function. A variable in the Ros_ethercat class could allow to select if we want to apply limits or not.

Having a variable in the xacros that let's you switch between different units (Nm, strain gauges, PWM) would be another soution, although a bit cumbersome.

[toliver]

An easier workaround that we have applied sometimes is to increase the tolerance of the following moveit parameters to avoid moveit's complaints (given the fact that we assume that the robot being within limits is our controller's responsibility, not moveit's).

https://github.com/shadow-robot/sr_interface/blob/kinetic-devel/sr_multi_moveit/sr_multi_moveit_config/launch/trajectory_execution.launch.xml#L16

https://github.com/shadow-robot/sr_interface/blob/kinetic-devel/sr_multi_moveit/sr_multi_moveit_config/launch/ompl_planning_pipeline.launch.xml#L14

[guihomework]

@toliver what you explain is completely understandable, and you spotted indeed an issue with this effort limits. I wanted to limit the commanded position with this safety limits, but they indeed influence the effort limit at the same time.

I am wondering if the URDF could not be generated taking care of the PWM_CONTROL env variable (not sure this is still existing) to set the correct effort limits (even if not calibrated). One can even use the new xacro functions to read the max_pwm or max_force from the yaml as there is a yaml loader for xacro and python access to whatever we want inside.

For the purpose of one article, I did some rough calibration of one gauge, and got a ratio of 10 (100 units is 10 N), I could probably live with this factor somewhere, either in the controller (sending shadow units for effort, and sr_robot_lib doing the conversion before writing) or in the sr_robot_lib (controllers sending Nm with the correct ratio then). For PWM this is more tricky,

I have already a work-around, instead of allowing larger limits in moveit, I allow to be out of bound in the trajectories for the first point(s) as long as the follow-up waypoints move towards bringing them within limits. Then the low-level controller saturates the input anyway to the working range (position controller saturate min at 0 not in negative, so should be fine)

So I cannot come with a clean way yet, when I have time I will discuss this again, to try find a clean way.