Fixes franka_gripper_sim GripperCommand action

[rickstaa]

This commit makes sure that users can also send gripper widths between 0.0 and 0.08. In the old version the gripper would only open and close based on whether the set gripper width was smaller or bigger than the current width.

I explained the problem in #172.

[rickstaa]

Please check the MoveIt gripper control execution timeout section of this comment #161 (comment) before merging this commit. Although, this comment does not directly affect this pull request, I think it is good to be aware of issues that exist with the current implementation of the GRASP state.

[rickstaa]

@gollth I applied the changes you requested. I'm now looking at the comments you made about the PID gains in #172.

[rickstaa]

@gollth I went ahead and apply your suggestion in #172 for the change of namespace. If you want, we can merge rickstaa#6 into #173 or create a new pull request. The related doc change can be found in frankaemika/docs#13.

[rickstaa]

@gollth There still appears to be an issue with the state machine when the PID gains you gave in #172. It still does not transfer from the GRASP to the HOLDING state because the velocities are sometimes higher than the current kGraspRestingThreshold (i.e. 0.001). Setting the kGraspRestingThreshold to 0.005 seems to solve the problem.

franka_ros/franka_gazebo/include/franka_gazebo/franka_gripper_sim.h

Line 36 in 0a9563c

const double kGraspRestingThreshold = 0.001;

In the related comment (#172 (comment)), during testing you use the config/franka_hw_sim.yaml to increase both the resting_threshold and consecutive_samples values. Due to that I was not sure whether you wanted to update the franka_gripper_sim kGraspRestingThreshold parameter or the config/franka_hw_sim.yaml.

Please let me know where you want to add these values or apply them yourself using gh pr checkout 173.

[rickstaa]

I did one last investigation into the behaviour under different PID gains. I did this both for the move and gripper_action action services. The results can be found in the next section.

Gripper_action step response

Old PID gains

New PID gains

Move action sinusoid response

Old PID gains

New PID gains

Conclusion

From these experiments, you can see that although with the new PID gains the resting velocities are lower, the overall behaviour is a lot worse. This is expected since no derivative gain is added to smooth out the motion and no integral gain to get rid of a steady-state error. Unfortunately, I therefore think the old gains are closer to the optimal gains.

For the state machine to transfer from GRASPING to HOLDING with the old PID gains, you however need to set a kGraspRestingThreshold of 0.11 which is not close to being stationary.

Further tuning

I tried to tune the gains manually again, but I keep getting gains close to the old ones (i.e., P: 100-500, I: 30-60, D: 10-22). The old gains (i.e., 100-25-20 seem to give good smooth control results, but sadly high velocity oscillations. I however can not get rid of the oscillations without making the movement less smooth.

Furthermore, I also tried using MATLAB to tune the PID gains (see https://www.youtube.com/watch?v=ENhFepqjFOY and the zip below) this however gives me a wide range of gains. I think this is since I'm not directly controlling the joint, but sending commands to the action server. As a result a lot of the control commands get cancelled.

gripper_pid_tunner.zip

[rickstaa]

@gollth To ensure the velocities are not resulting from the arm controller, I also checked the old PID gains while running the cartesian_impedance controller (see GIF below). At this stage, I still think the problem we are experiencing has one of the following causes (ordered from most likely to least likely):

• Incorrect PID gains.
• Incorrect dynamic finger properties (i.e. inertias, masses, joint friction).
• Upstream issues in the gazebo simulator.

Incorrect PID gains

For my investigation, see #173 (comment).

Incorrect dynamics

I quickly investigated the effect of the dynamic parameters.

property	Effect
Damping	Increasing damping increases vel oscillations. Lowering has no noticeable effect.
Friction	Increasing friction lowers vel oscillations but prevents movement.
Inertia	Doesn't affect the vel oscillations too much (i.e., for the values I tested).
Mass	The vel oscillations are lowered when the mass is increased.

I interchanged the inertias you provided by the ones used in the justagist/franka_panda_description repository. If we keep the mass the same (i.e. 15g), we see no significant effect on the velocity oscillations. If we, however, increase the mass to 100 g, the velocity oscillations decrease.

To me, 100 g looks a bit much, where 15 g seems too little. However, I do not know the exact weight since I have not detached the finger from the hand and can not find the hand data sheet online (see the results for 38 g in the gif below).

To conclude, I think the oscillations are caused by an interplay between the PID gains and the dynamic properties. Maybe you can verify the finger weight such that we can try to retune the PID gains again. If that doesn't help, we can create a question on the ROS or gazebo forms to see if somebody encountered this behaviour on other robots.

Bugs in gazebo

I checked the gazebo repo but only found issues with the velocity calculation when a joint is in contact with an object (i.e. gazebosim/gazebo-classic#622 and gazebosim/gazebo-classic#634). Further, given the fact that we can actually see the fingers shake, I don't think this problem is caused by a bug in Gazebo.

[rickstaa]

@marcbone Thanks for your comment. I see what you mean the gripper.grasp method gets called after the gripper.move therefore invoking the grasp behaviour implemented in the libfranka library (see https:/frankaemika/libfranka/blob/f1f46fb008a37eb0d1dba00c971ff7e5a7bfbfd3/src/gripper.cpp#L73-L74):

franka_ros/franka_gripper/src/franka_gripper.cpp

Lines 54 to 55 in ac1e07d

	return gripper.grasp(target_width, default_speed, goal->command.max_effort, grasp_epsilon.inner,
	grasp_epsilon.outer);

In that case, closing this pull request makes sense since we want the simulation to mimic the actual robot behaviour. However, there is one downside with this: users can not set a specific with using MoveIt. They have to use the franka_gripper/move action instead.

Ideally, there would be another franka_gripper/move action (e.g. franka_gripper/moveit_move) that has the control_msgs/GripperCommand msg-type. MoveIt could then use this action server to set the gripper width to a specific width.

@rhaschke Do you think not having the ability to set a specific width using MoveIt is a significant problem?

[rhaschke]

@marcbone, so the real hand doesn't allow to drive the gripper to a specific position (different from fully opened / closed)?
If that's not possible, I would consider this as a severe limitation of the gripper and asking for a this feature!

In any case, the simulation should, of course, reflect real robot behavior.

[rickstaa]

@rhaschke On the real robot, I normally set the gripper to a specific position using the franka_gripper::MoveAction(width, speed) action. This action is found on the franka_gripper/move namespace (see the docs.

The main problem is that MoveIt, to my knowledge, only works with the control_msgs.msg.GripperCommand msg while the franka_gripper/move service uses the franka_gripper/Move message.

franka_ros/franka_gripper/action/Move.action

Lines 1 to 6 in ac1e07d

	float64 width # [m]
	float64 speed # [m/s]
	---
	bool success
	string error
	---

As a result, people can not set the gripper width using MoveIt since the franka_gripper/gripper_action transitions into the grasp state and therefore only opens/closes the gripper. They have to use the franka_gripper/move action service directly to do this.

[rhaschke]

Indeed, MoveIt can only interact with GripperCommand actions or TrajectoryJointControllers. If Franka provides another action type, a proxy would be needed to translate corresponding actions.

[rhaschke]

Is there a trajectory controller available for the gripper?

[marcbone]

We can offer you to change the gripper command so that if the max_effort is 0 we use the gripper.move() command instead. Would that work for you? You could then move gripper to a desired width and would still be able to grasp an object with a certain force.

There is no trajectory controller available for the gripper since all gripper commands are blocking.

[rhaschke]

That sounds reasonable.

[rickstaa]

@marcbone I think your proposed solution is more intuitive and gives us a way to implement both the move and grasp behaviour at the MoveIt side.

@rhaschke When @marcbone implements his solution. Until we implement a solution at our end I think we have to increase the max_effort that is set in the moveit_simple_controller_manager to a higher number to keep the current grasping behaviour (see https:/ros-planning/moveit/blob/4db626d9c3b5d6296b012188a4a0bfe4bddf6bce/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/gripper_controller_handle.h#L111-L113).

[rickstaa]

I will open an issue on https:/ros-planning/moveit when @marcbone solution has been merged into the devel branch. @marcbone can you maybe close #172 when you implemented your proposed solution. This way I know when we can start development at the MoveIt side.

[rhaschke]

@rickstaa, I think you can start development at the MoveIt side already. Currently, a non-zero gripper effort can be specified via the trajectory. I think we should add the possibility to

• specify that as a ROS parameter in the GripperCommand declaration
• allow switching controllers for different (types of) trajectories
  Allow specification of controller names for trajectories moveit/moveit#1832

[rickstaa]

@rhaschke I created moveit/moveit#2956. I can not give an ETA when I have time to implement this but please add me under assignees so that I don't forget.

[rhaschke]

We can offer to change the gripper command so that if the max_effort is 0 we use the gripper.move() command instead.

Thinking about the issue a little deeper, I think that your GripperAction misbehaves. Please correct me, @marcbone, if I'm wrong.
The GripperCommand msg provides two fields:

float64 position
float64 max_effort

As far as I understand, the current implementation kind of ignores the position argument of that message, but either attempts to fully open or fully close the hand depending on whether the current position is smaller or larger than the commanded position.
That's a reasonable approach for grasping. However, it doesn't apply to free-space motion, but exactly leads to the observed oscillations around the commanded position (#173 (comment)).
IMO, the gripper action should realize position-control as long as measured efforts are smaller than max_effort. Otherwise, it should switch to force-control and maintain the given (grasp) force.

[gollth]

There still appears to be an issue with the state machine when the PID gains you gave in #172.

For the record, it was really a problem with poorly tuned gains for the gripper sim. With drastically reduced I + D gains the behavior is much more stable:

[gollth]

IMO, the gripper action should realize position-control as long as measured efforts are smaller than max_effort. Otherwise, it should switch to force-control and maintain the given (grasp) force.

Good idea @rhaschke! However we tried to stick as close to the implementation of the franka_gripper as possible.

[rickstaa]

@gollth I will answer #172 (comment) here to keep the discussion in one place.

In the current form, the gripper action still cannot grasp objects (see gif).

This is kind of expected when you pass a gripper command position of 0.0. When you check the result of the action or the documentation of franka_gripper you will find out why. For the same reason, grasping works in your example above.

I indeed expected the gripper_action to fail based on the current codebase. As you are aware, it fails is caused by the ternary expression, which I tried to address in #173:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Line 437 in 414bcb1

Config{.width_desired = goal->command.position * 2.0 < width ? 0 : kMaxFingerWidth,

As discussed earlier, this ternary expression makes it so that MoveIt can only open or close the gripper through the gripper_action service. As a result, the width is set to 0.0 when closing the gripper, and the following code is executed because the gripper width is not within the epsilon range:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 467 to 468 in 414bcb1

	std::lock_guard<std::mutex> lock(this->mutex_);
	this->state_ = State::IDLE;

This behaviour can also be observed in the grasp service when we set the width to 0.0 while keeping the epsilon at 0.005.

rostopic pub --once /franka_gripper/grasp/goal              franka_gripper/GraspActionGoal              "goal: { width: 0.0, epsilon:{ inner: 0.005, outer: 0.005 }, speed: 0.1, force: 5.0}"

Good idea @rhaschke! However, we tried to stick as close to the implementation of the franka_gripper as possible.

I fully understand that you want to stick as close to implementing the real franka_gripper as possible, and I also think that the grasp action is implemented correctly. I, however, think the gripper_action in the current form is not very useful on the real and simulated robot when looking from the MoveIt perspective. This is because it does not allow us to set a specific with or grasp an object.

I think @marcbone's suggestion together with moveit/moveit#2956 would solve the gripper_width problem for which I created #172.

We can offer you to change the gripper command so that if the max_effort is 0 we use the gripper.move() command instead. Would that work for you? You could then move the gripper to the desired width and would still be able to grasp an object with a certain force.

There is no trajectory controller available for the gripper since all gripper commands are blocking.

Additionally, @rhaschke suggestion would allow us also to grasp objects. I will leave the actual implementation to you, but I just wanted to sketch which problems we are experiencing from the MoveIt side.

[rickstaa]

I just noticed that we could make MoveIt grasp by setting the /franka_gripper/gripper_action/width_tolerance to the gripper width (i.e. 0.08). I, however, still think that the gripper_action is a bit unintuitive and could be improved. The downside is that using this ROS parameter, we can OR grasp OR move since it is only read at startup:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 29 to 30 in 414bcb1

	nh.param<double>("gripper_action/width_tolerance", this->tolerance_gripper_action_,
	kDefaultGripperActionWidthTolerance);

Together with @marcbone suggestion, this would allow MoveIt to both move and grasp. @rhaschke are you okay with using that ROS parameter for MoveIt?

[rickstaa]

Correction, it appears that setting the /franka_gripper/gripper_action/width_tolerance does not work when it is set to 0.08 since the check is performed too early when the gripper width is still in the start position:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 466 to 469 in 414bcb1

	if (not inside_tolerance) {
	std::lock_guard<std::mutex> lock(this->mutex_);
	this->state_ = State::IDLE;
	}

Setting it to a value greater than the gripper width (i.e. 0.1) works but feels like a rather peculiar workaround. Further, since the direction is not included in the GripperAction STATE::GRASPING transition:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Line 439 in 414bcb1

.force_desired = goal->command.max_effort,

We have to set the max_effort to be negative:

rostopic pub --once /franka_gripper/gripper_action/goal control_msgs/GripperCommandActionGoal "goal: {command:{position: 0.0, max_effort: -5.0}}"

[rickstaa]

@gollth, @marcbone, @rhaschke maybe we can continue our discussion in #172?

[gollth]

Why adjust the tolerance instead of the actual desired grasping position? When you want to plan a grasping motion you should have an idea of the size you want to grasp no?

[rickstaa]

Why adjust the tolerance instead of the actual desired grasping position?

@gollth Oh, I see. Sorry for being unclear in my previous comment. My comment above was about how I can hack the gripper_action so that it supports both movement and grasping. That is why I adjusted the width_tolerance since the ternary expression prohibits me from setting a specific width.

When you want to plan a grasping motion you should have an idea of the size you want to grasp no?

This is true when you have information about the grasping object, and you can use this information to get a grasping Pose. I also have use-cases where I don't care about the object width, but I want the gripper to close while making sure that it keeps applying a force without the force exceeding the max_effort. A mechanism, as explained by @rhaschke earlier, would help in this case.

To conclude, I think the current implementation of the grasping service is perfect since it allows the user to specify a specific grasp. I, however, think that the grasp_action service (on both the real and simulated robot), which MoveIt uses should allow the following behaviours:

• When you send a gripper_width, and a max_effort of 0.0, the move_action should be used.
• When you send a gripper_width and a max_effort that is greater than 0.0, the gripper should try to go to the gripper_width and only move into the HOLDING state when the effort exceeds the max_effort.

By doing this, users can now both move and grasp in MoveIt (on the real and simulated robot). I also think this makes more sense given the structure of the GripperCommand message. I also don't think this change removes functionalities since you already supply a grasping and move service.

[rickstaa]

There still appears to be an issue with the state machine when the PID gains you gave in #172.

For the record, it was a problem with poorly tuned gains for the gripper sim. With drastically reduced I + D gains, the behaviour is much more stable:

@gollth, I tested your new gains, and I'm sorry, but they do not work correctly on the simulated robot. They seem to work when the hand is horizontal since there is no gravity being applied to the fingers. However, when I turn the gripper by some degrees (i.e. 90 in the example below), the low gains cannot move the fingers up (see gif 1).

gif 1:

I improved the behaviour by increasing both the P, I and D values again (see gif 2/3). The velocities now stay in the 0.004 range when the gripper is vertical and 0.0002 when the gripper is horizontal. I did not spend more than 5 min tuning these gains, so there are probably better gains that can decrease the velocity variance while still giving the correct behaviour. I, however, hope my gains serve as a starting point to find the solution to this very delicate tuning problem.

gif 2:

gif 3:

[rickstaa]

@gollth I just realized that I checked the move service while the problems are only experienced when using the grasp service. The issues are still present when using the grasp service (see gif below).

It is, however strange that these velocities are not present when using the moving service. The oscillations are, therefore, closely related to the logic you use for the GRASP state. This is because the current logic assumes that the gripper comes into contact with another object which helps dampen out the velocity oscillations of the fingers.