franka_ros: [bug] Gazebo gripper control is broken
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
libfrankalibrary. - 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/goalAction 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/goalaction should be enhanced. It should enable position control when measured efforts are belowmax_effortand 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.
About this issue
- Original URL
- State: open
- Created 3 years ago
- Comments: 16 (16 by maintainers)
Commits related to this issue
- Improves franka_gazebo gripper PID gains This commit improves the gripper PID gains since the old gains caused the gripper to oscillate around a given setpoint (see #172). — committed to rickstaa/franka_ros by rickstaa 3 years ago
- BREAKiNG CHANGE: Moves 'resting_threshold' and 'consecutive_samples' to base ns This commit mose the 'resting_threshold' and 'consecutive_samples' to the base ns since they also apply to the 'gripper... — committed to rickstaa/franka_ros by rickstaa 3 years ago
- BREAKiNG CHANGE: Moves 'resting_threshold' and 'consecutive_samples' to base ns This commit mose the `resting_threshold` and `consecutive_samples` to the base ns since they also apply to the `gripper... — committed to rickstaa/franka_ros by rickstaa 3 years ago
- Improves franka_gazebo gripper PID gains This commit improves the gripper PID gains since the old gains caused the gripper to oscillate around a given setpoint (see #172). — committed to rickstaa/franka_ros by rickstaa 3 years ago
- BREAKiNG CHANGE: Moves 'resting_threshold' and 'consecutive_samples' to base ns This commit mose the `resting_threshold` and `consecutive_samples` to the base ns since they also apply to the `gripper... — committed to rickstaa/franka_ros by rickstaa 3 years ago
- Improves franka_gazebo gripper PID gains This commit improves the gripper PID gains since the old gains caused the gripper to oscillate around a given setpoint (see #172). — committed to rickstaa/franka_ros by rickstaa 3 years ago
- fix(franka_gazebo): hotfix for franka_ros#172 This commit reverts to using the old gripper PID gains since these make sure that the Move action works. These gains however break the grasp action. This... — committed to rickstaa/franka_ros by rickstaa 3 years ago
- fix(franka_gazebo): hotfix for franka_ros#172 This commit reverts to using the old gripper PID gains since these make sure that the Move action works. These gains however break the grasp action. This... — committed to rickstaa/franka_ros by rickstaa 3 years ago
- fix(franka_gazebo): hotfix for franka_ros#172 This commit reverts to using the old gripper PID gains since these make sure that the Move action works. These gains however break the grasp action. This... — committed to rickstaa/franka_ros by rickstaa 3 years ago
- Merge pull request #172 in SWDEV/franka_ros from SRR-1267 to develop * commit '6aaa08926fc686b1492d8b36f985eb6f260b3de5': CHANGE: Reformat CHANGELOG ADD: franka_gazebo as dependency to franka_ros... — committed to frankaemika/franka_ros by gollth 2 years ago
Stone made of stone material, sounds legit =D
@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.