[bug] Gazebo gripper control is broken
Opened this issue ยท 16 comments
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:
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
- Follow the documentation to install
libfranka
library. - Create a new catkin workspace.
- Clone franka_ros package in this workspace.
- Clone https://github.com/rickstaa/panda_moveit_config/tree/adds_gazebo_simulation in this workspace.
- Build the workspace following the steps explained in the documentation.
- Start the Panda simulation using the
roslaunch panda_moveit_config demo_gazebo.launch
. - Set the ros namespace `export ROS_NAMESPACE=/panda
- Run the script below to send a MoveIt gripper command.
- 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:
-
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.
- Gripper PID gains need improvement for the
-
Gripper Action Behavior:
- The
/franka_gripper/gripper_action/goal
action should be enhanced. It should enable position control when measured efforts are belowmax_effort
and switch to force control otherwise to maintain the desired grasp force.
- The
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.
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
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
Please correct me if this is not the indented way how this gripper command action is supposed to work, though.
@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
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
.
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.
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.
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 theFrankaGripperSim::onGripperActionGoal
initially pushes it toGRASPING
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?
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.
@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}"
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
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?
@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.
Stone made of stone material, sounds legit =D
To summarize. My bug report is related to closely related issues:
- First, it looks like the current implementation of the
gripper_action
and thusgrasp
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.
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:
-
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.
- Gripper PID gains need improvement for the
-
Gripper Action Behavior:
- The
/franka_gripper/gripper_action/goal
action should be enhanced. It should enable position control when measured efforts are belowmax_effort
and switch to force control; otherwise, maintain the specified grasp force.
- The
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 settingmax_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).
- Grip er action should achieve position control if efforts are below
- 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, thegripper_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.