[bug] Gazebo gripper control is broken

[rickstaa]

Old bug report

In the current version, when working with the gazebo simulation, the gripper width that is sent to the /panda/franka_gripper/gripper_action/goal topic is not handled correctly. The problem is caused by the following code:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Line 436 in 054af53

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

Due to this, the desired gripper with now becomes 0 when the gripper width is smaller than the current gripper width and 0.08 when it is bigger than the current gripper width. Consequently, users can now only open/close the gripper.

I created #173 to address this problem.

Steps to reproduce

1. Follow the documentation to install libfranka library.
2. Create a new catkin workspace.
3. Clone franka_ros package in this workspace.
4. Clone https://github.com/rickstaa/panda_moveit_config/tree/adds_gazebo_simulation in this workspace.
5. Build the workspace following the steps explained in the documentation.
6. Start the Panda simulation using the roslaunch panda_moveit_config demo_gazebo.launch.
7. Set the ros namespace `export ROS_NAMESPACE=/panda
8. Run the script below to send a MoveIt gripper command.
9. See that you can not set the gripper to a specific width, since it will only close and open.

"""Small script to see that the gazebo GripperCommand action does not set the gripper
width.
"""

import sys

import moveit_commander
import moveit_msgs.msg
import rospy

GRIPPER_WIDTH = 0.08

if __name__ == "__main__":

    # Initiate MoveIt commander and node
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node("test_gripper_width", anonymous=True)

    # Create commanders
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()

    # Load arm and hand groups
    hand_move_group = moveit_commander.MoveGroupCommander("hand")

    # Display the trajectory
    display_trajectory_publisher = rospy.Publisher(
        "/move_group/display_planned_path",
        moveit_msgs.msg.DisplayTrajectory,
        queue_size=20,
    )

    # Send the gripper width command
    hand_move_group.set_joint_value_target([GRIPPER_WIDTH/2, GRIPPER_WIDTH/2])
    (hand_plan_retval, plan, _, error_code) = hand_move_group.plan()
    retval = hand_move_group.execute(plan, wait=True)

    # Calling ``stop()`` ensures that there is no residual movement
    hand_move_group.stop()

Updated bug report

The gazebo gripper actions are not functioning as expected. This makes them less useful than they could be. The simulated gripper has improved since this bug report was initially posted (see the Old bug report item above). Two critical bugs persist:

1. Gripper PID Gains:

   • Gripper PID gains need improvement for the /franka_gripper/gripper_action/goal Action to set the gripper width accurately when the gripper is vertical.

2. Gripper Action Behavior:

   • The /franka_gripper/gripper_action/goal action should be enhanced. It should enable position control when measured efforts are below max_effort and switch to force control otherwise to maintain the desired grasp force.

Proposed Improvements

Addressing these issues will enhance the usability of the Panda simulation for RL research. Specifically, it allows users to specify a gripper width with a max force, enabling the gripper to apply that force to encountered objects.

[gollth]

Hi @rickstaa,

thanks for working on this and the whole moveit integration. The problem with this approach is the way how the grasping works. You probably already noticed the statemachine logic inside franka_gripper_sim. The core transitioning mechanic happens here:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 147 to 191 in 1922fc9

	if (w_d - tolerance.inner < width and width < w_d + tolerance.outer) {
	// Goal reached, update statemachine
	if (state == State::MOVING) {
	// Done with move motion, switch to idle again
	transition(State::IDLE, Config{.width_desired = this->config_.width_desired,
	.speed_desired = 0,
	.force_desired = 0,
	.tolerance = this->config_.tolerance});
	}
	if (state == State::HOMING) {
	if (this->config_.width_desired == 0) {
	// Finger now open, first part of homing done, switch direction
	setConfig(Config{.width_desired = kMaxFingerWidth,
	.speed_desired = this->config_.speed_desired,
	.force_desired = this->config_.force_desired,
	.tolerance = this->config_.tolerance});
	} else {
	// Finger now closed again, homing finished
	setState(State::IDLE);
	}
	}
	}
	if (state == State::GRASPING) {
	// Since the velocity signal is noisy it can easily happen that one sample is below the
	// threshold To avoid abortion because of noise, we have to read at least N consecutive number
	// of samples before interpreting something was grasped (or not)
	static int speed_threshold_counter = 0;
	double speed = this->finger1_.getVelocity() + this->finger2_.getVelocity();
	if (std::abs(speed) <= this->speed_threshold_) {
	speed_threshold_counter++;
	} else {
	speed_threshold_counter = 0;
	}
	if (speed_threshold_counter >= this->speed_samples_) {
	// Done with grasp motion, switch to holding, i.e. keep position & force
	transition(State::HOLDING, Config{.width_desired = width,
	.speed_desired = 0,
	.force_desired = this->config_.force_desired,
	.tolerance = this->config_.tolerance});
	speed_threshold_counter = 0;
	}
	}

Right now, the target width w_d (which originates from the goal of the GripperCommand message) is checked each cycle to be within certain tolerances. If it is, the statemachine will switch eventually to State::IDLE, not State::HOLDING (in which forces are exerted during static contact with an object).

With your solution, as soon as the fingers reach the desired width +/- threshold they would stop and don't hold the object tight!

Our original solution with the ternary operator was to specify widths beyond the desired width (i.e. 0 or 8cm) and use the width from the goal only after the grasp was established to check if the action succeeded or failed:

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 457 to 469 in 1922fc9

	double width_d = goal->command.position * 2.0;
	width = this->finger1_.getPosition() + this->finger2_.getPosition(); // recalculate
	bool inside_tolerance = width_d - this->tolerance_gripper_action_ < width and
	width < width_d + this->tolerance_gripper_action_;
	result.position = width;
	result.effort = 0;
	result.stalled = static_cast<decltype(result.stalled)>(false);
	result.reached_goal = static_cast<decltype(result.reached_goal)>(inside_tolerance);
	if (not inside_tolerance) {
	std::lock_guard<std::mutex> lock(this->mutex_);
	this->state_ = State::IDLE;
	}
	action_gc_->setSucceeded(result);

Please correct me if this is not the indented way how this gripper command action is supposed to work, though.

[rickstaa]

@gollth Thanks again for your quick and concise answer! I apologize in advance for the lengthy discussion below. I tried to be as brief as possible, but it amounts to a long read.

With your solution, as soon as the fingers reach the desired width +/- threshold they would stop and don't hold the object tight!

I tried debugging my code, but I could not reproduce this behaviour. I can not find any code that transfers the state machine to IDLE state when the FrankaGripperSim::onGripperActionGoal initially pushes it to GRASPING state (see the gif below).

franka_ros/franka_gazebo/src/franka_gripper_sim.cpp

Lines 149 to 168 in 1922fc9

	if (state == State::MOVING) {
	// Done with move motion, switch to idle again
	transition(State::IDLE, Config{.width_desired = this->config_.width_desired,
	.speed_desired = 0,
	.force_desired = 0,
	.tolerance = this->config_.tolerance});
	}
	if (state == State::HOMING) {
	if (this->config_.width_desired == 0) {
	// Finger now open, first part of homing done, switch direction
	setConfig(Config{.width_desired = kMaxFingerWidth,
	.speed_desired = this->config_.speed_desired,
	.force_desired = this->config_.force_desired,
	.tolerance = this->config_.tolerance});
	} else {
	// Finger now closed again, homing finished
	setState(State::IDLE);
	}
	}

I, however, discovered two other problems that are present in my solution. First, please allow me to explain my motivation.

I opened this issue to make sure the gripper behaviour in the simulation is as close as possible to that of the real robot. If I remember correctly, when using MoveIt on the real robot, I could set both a specific width and grasp objects (please correct me if I'm wrong, I haven't tried this behaviour in the last months). Also, when I checked the franka_ros codebase, and I noticed that the franka_gripper node handles the GripperAction differently (See the expandable block below for more info). I, therefore, think giving users the ability to specify a gripper width and grasp objects would bring the simulation closer to the real robot.

My investigation

I quickly checked the codebase of the franka_gripper package. To my current understanding, in the franka_gripper node, when the desired width is smaller than the current width, the move action is executed. The gripper is closed until it is within the kSamePositionThreshold.

franka_ros/franka_gripper/src/franka_gripper.cpp

Line 52 in 1922fc9

return gripper.move(target_width, default_speed);

In this case, the gripper keeps moving, even when an object is encountered, applying the force needed to lift the object. The ' grasp ' action is executed when the gripper width is more significant than the current width.

franka_ros/franka_gripper/src/franka_gripper.cpp

Line 54 in 1922fc9

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

I can't look into the FCI, but I think this action tries to apply the requested force.

Current problems

Instabilities in the gazebo velocities due to arm or hand PID Gains

I think this is related to our discussion in #161 and possibly #160. When the arm shakes a bit due to the non-perfect PID gains of its controllers, the finger joint velocities are affected. As a result, they do not become lower than the kGraspRestingThreshold (see gif below). As a result, the state machine often gets stuck in the GRAPSING state. I noticed that the problem goes away when the derivative gain is lowered to 0.0. Doing this, however, can lead to overshoots. Tuning the gripper PID gains might therefore mitigate the effect. However, I'm unsure if this is due to numerical instabilities in the gazebo simulation or wrongly tuned gripper gains.

In the old codebase, this behaviour goes unnoticed since the gripper only moves between the open and closed positions due to the ternary expression. In these positions, the gripper is in contact with the external world, which damps out the instabilities caused by the high derivative gain.

MoveIt doesn't send the max_effort

I noticed that MoveIt currently doesn't allow users to set the
max_effort value of the GripperCommand message (see the move_group code API) when they use the move_group class. As a result, the command.max_effort is set to 0, which means that the robot will apply no force in the HOLDING state. Therefore, in the current form, the robot is likely inadequate to hold heavy objects in the simulation.

This is not a problem with your API but a feature request that should be opened on the MoveIt repository.

[gollth]

Hi @rickstaa,

tried debugging my code, but I could not reproduce this behaviour. I can not find any code that transfers the state machine to IDLE state when the FrankaGripperSim::onGripperActionGoal initially pushes it to GRASPING state (see the gif below).

Yes your are absolutely right, I was confusing things in my mind. Sorry about that. So #173 should work and bring franka_gripper_sim and franka_gripper more closely together. We will discuss over there a strategy how to merge that.

Regarding the problem you have with getting stuck in GRASPING state. I agree that this has to do gazebo gains. With the default thresholds the fingers seem to oscillate around and over the kGraspRestingThreshold. Luckily you can change them via ROS parameter ;) I fiddled around a bit with the threshold and the gains and the following works for me Plan & Execute in RViz Motion Planning:

# File: franka_gazebo/config/franka_hw_sim.yaml
franka_gripper:
  type:    franka_gazebo/FrankaGripperSim
  arm_id:  $(arg arm_id)

  finger1:
    gains: { p: 1000, i: 0, d: 0 }

  finger2:
    gains: { p: 1000, i: 0, d: 0 }

  grasp:
    resting_threshold: 0.05
    consecutive_samples: 10

(I'm think we should even move the resting_threshold and consecutive_samples parameter outside of grasp namespace, since they are also relevant for gripper_command)

So I suggest we discuss in #173 about how to merge that and hopefully you can continue with these gains with the MoveIt update. How does that sound?

[rickstaa]

As stated in #180 (comment) I think the behaviour that was pointed out by @rhaschke in #173 (comment) would be the desired behaviour to solve this issue.

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.

[rickstaa]

@gollth, @marcbone Thanks a lot for improving the grasping behaviour! I performed some grasping tests using the new development branch, and I think you are close to solving this issue. I, however, found the following problems.

Move action

The move action gives some peculiar behaviour when moving the gripper beyond the stone object boundary. This is not too problematic since the move action is not meant to grasp objects. I, however, wanted to make you aware of this issue since it might confuse people when they try out the tutorial.

In the gif below, I first close the gripper using the move action. When I do this, the stone moves up and down relative to the gripper while the gripper is not moving. This is probably caused by the properties of your stone object or its interaction with the pick_tray since when I use one of my objects that also work with other robots; The behaviour is not there (see gif2). In both cases, as expected, the object slides out of the gripper.

Stone:

<?xml version='1.0'?>
<sdf version='1.7'>
<model name='stone'>
        <pose frame=''>0.000286 -0.221972 0.475172 7.5e-05 3.5e-05 0.003179</pose>
        <scale>1 1 1</scale>
        <link name='link'>
          <pose frame=''>0.000286 -0.221972 0.475172 7.5e-05 3.5e-05 0.003179</pose>
          <velocity>0 0 0 0 -0 0</velocity>
          <acceleration>0 0 -9.8 0 -0 0</acceleration>
          <wrench>0 0 -0.49 0 -0 0</wrench>
        </link>
</model>
</sdf>

My cube:

<?xml version='1.0'?>
<sdf version='1.7'>
  <model name='cube'>
    <link name='link'>
      <inertial>
        <pose>0.03 0.03 0.03 0 -0 0</pose>
        <!--NOTE: Uses Polyethylene density to calculate the mass
        see https://en.wikipedia.org/wiki/Polyethylene
        -->
        <mass>0.20735999999999996</mass>
        <inertia>
          <ixx>0.00012442</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00012442</iyy>
          <iyz>0</iyz>
          <izz>0.00012442</izz>
        </inertia>
      </inertial>
      <visual name='visual'>
        <pose>0.03 0.03 0.03 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.06 0.06 0.06</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/DarkGrey</name>
          </script>
          <shader type='pixel'/>
        </material>
      </visual>
      <collision name='collision'>
        <pose>0.03 0.03 0.03 0 -0 0</pose>
        <geometry>
          <box>
            <size>0.06 0.06 0.06</size>
          </box>
        </geometry>
        <surface>
          <!--NOTE: Used dynamic friction of clean dry plastic cube on a plastic surface
          (i.e. Polyethylene) see https://www.engineeringtoolbox.com/friction-coefficients-d_778.html
          -->
          <friction>
            <ode>
              <mu>0.2</mu>
              <mu2>0.2</mu2>
            </ode>
          </friction>
          <contact>
            <collide_bitmask>1</collide_bitmask>
            <ode>
              <kp>1e+06</kp>
              <kd>1e+06</kd>
              <max_vel>1</max_vel>
              <min_depth>0.002</min_depth>
            </ode>
            <bullet>
              <kp>1e+06</kp>
              <kd>1e+06</kd>
            </bullet>
          </contact>
        </surface>
      </collision>
    </link>
  </model>
</sdf>

Gripper action

In the current form, the gripper action still is unable to grasp objects (see gif).

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

Grasp action

The grasp action works!

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

[rickstaa]

I quickly checked and it seems that the strange behaviour is indeed caused by the definition of the stone, pick_tray and place_tray objects. The inertias and COM of these objects look very strange compared to my grasping object. I think you could improve these by using real material properties.

[gollth]

In the current form, the gripper action still is unable to 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

[gollth]

The inertias and COM of these objects look very strange compared to my grasping object. I think you could improve these by using real material properties.

Thanks @rickstaa for finding that out. Would you mind a PR?

[rickstaa]

@gollth I'm happy to create a pull request. I however have to know which materials you want to use. Maybe stone for the stone and wood for the trays.

[gollth]

Stone made of stone material, sounds legit =D

[rickstaa]

@gollth I created a draft in #206.

[rickstaa]

For future reference, part of the discussion of this topic is found in #173.

[rickstaa]

To summarize. My bug report is related to closely related issues:

• First, it looks like the current implementation of the gripper_action and thus grasp service does not work. The State Machine does not transition into the HOLDING state when high I and D gains are used since the velocities fluctuate too much. This is solved by lowering the I and D gains. The downside is that the gains are now not high enough to control the fingers when the hand is not horizontal (see #173 (comment)).
• Secondly, it is about the implementation of the gripper_action service. In the current form, MoveIt can not both grasp and move the gripper (see #173 (comment)).
  • #209 will fix this issue.

[rickstaa]

@gollth, @marcbone I updated the title to represent better the problems still present with the gripper in simulation.

[rickstaa]

Bug Report: Gripper Simulation Issues

Feedback and Investigation

I received user feedback regarding issues with the gripper simulation, as reported in GitHub issue #33. To address these concerns, I investigated whether the problems had been resolved upstream. Below are my findings:

Current Gripper Simulation Status

The simulated gripper has seen improvements since the initial report. However, two critical bugs persist:

1. Gripper PID Gains:

   • Gripper PID gains need improvement for the /franka_gripper/gripper_action/goal Action to set the gripper width accurately when the gripper is vertical.

2. Gripper Action Behavior:

   • The /franka_gripper/gripper_action/goal action should be enhanced. It should enable position control when measured efforts are below max_effort and switch to force control; otherwise, maintain the specified grasp force.

Proposed Improvements

Addressing these issues will enhance the usability of the Panda simulation for RL research. Specifically, it allows users to specify a gripper width with a max force, enabling the gripper to apply that force to encountered objects.

Gripper Simulation Investigation

Overview

Action	Horizontal Move	Vertical Move	Grasp
/franka_gripper/move/goal	✅	✅	❌
/franka_gripper/gripper_action/goal	✅	❌	➖ [1]
/franka_gripper/grasp/goal	❌	❌	✅

[1] This is possible when increasing the /franka_gripper/gripper_action/width_tolerance parameter above the max gripper width.

Move Action Investigation

The /move action can move the gripper but not grasp objects.

Move action:

• Successfully moves the gripper to a specific width using /franka_gripper/move/goal.

Grasp Action:

• Cannot be used to grasp objects because both gripper heads stop when encountering an object, leaving space between the gripper and the target.

Gripper Action Investigation

The gripper_action can move the gripper but not directly grasp objects.

Move action:

• /franka_gripper/gripper_action/goal can set a gripper width by setting max_effort to zero.

Grasp Action:

• Still cannot be used to grasp an object. The behaviour needs clarification and improvement. Suggested behaviour in [comment](#173 (comment) 255718) by @rhaschke is considered desirable.
  • Grip er action should achieve position control if efforts are below max_effort. Otherwise, it should switch to force control, maintaining the specified grasp force.
  • Additionally, the action fails when the gripper is vertical due to incorrectly tuned PID gains (GitHub issue #209).
• Users can work around this limitation by setting the /franka_gripper/gripper_action/width_tolerance parameter to a value more significant than the max gripper width (see https://frankaemika.github.io/docs/franka_ros.html#frankagrippersim). When this is done, the gripper_action correctly grasps objects. This is however suboptimal since this parameter has to be set before the franka_gripper controller is loaded.

Grasp Action

The /franka_gripper/grasp/goal cannot move the gripper to a specific width. This is as expected.

Grasp

The /franka_gripper/grasp/goal action successfully grasps objects when the correct gripper width or the inner epsilon is configured to match the gripper width.

[rickstaa]

@gollth, following my investigation, I've revised the issue body to include my latest proposed improvements. Feel free to tag me if you have any questions.