Unit 6 Grasping not working

migueelnery · June 14, 2021, 10:21pm

Hi, in exercise 6.2 when I execute roslaunch simple_grasping pick_and_place.launch I got the following error after the object is detected and the robot does not move:

[ INFO] [1623707791.086471480, 983.826000000]: Manipulation plan 2 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1623707791.086763400, 983.826000000]: Manipulation plan 7 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1623707791.086848935, 983.826000000]: Manipulation plan 11 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1623707791.087136135, 983.826000000]: Manipulation plan 15 failed at stage 'reachable & valid pose filter' on thread 3
[ INFO] [1623707791.107516079, 983.837000000]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 2
[ INFO] [1623707791.130173180, 983.856000000]: Manipulation plan 6 failed at stage 'approach & translate' on thread 3
[ INFO] [1623707791.135179590, 983.860000000]: Manipulation plan 5 failed at stage 'approach & translate' on thread 1
[ WARN] [1623707791.145683310, 983.869000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 9. Try a lower max_step.
[ INFO] [1623707791.146001317, 983.870000000]: Manipulation plan 3 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1623707791.146833652, 983.871000000]: Manipulation plan 10 failed at stage 'approach & translate' on thread 2
[ WARN] [1623707791.157442878, 983.879000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[ INFO] [1623707791.158679459, 983.880000000]: Manipulation plan 0 failed at stage 'approach & translate' on thread 0
[ WARN] [1623707791.165765262, 983.885000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[ INFO] [1623707791.172154269, 983.889000000]: Manipulation plan 12 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1623707791.181615388, 983.893000000]: Manipulation plan 14 failed at stage 'approach & translate' on thread 3
[ WARN] [1623707791.182460888, 983.893000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ WARN] [1623707791.184933707, 983.893000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 3. Try a lower max_step.
[ INFO] [1623707791.187158536, 983.895000000]: Manipulation plan 8 failed at stage 'approach & translate' on thread 1
[ WARN] [1623707791.192585341, 983.897000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ WARN] [1623707791.193560827, 983.898000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[ WARN] [1623707791.201447862, 983.905000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ WARN] [1623707791.201585889, 983.905000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 3. Try a lower max_step.
[ INFO] [1623707791.201657485, 983.905000000]: Manipulation plan 16 failed at stage 'approach & translate' on thread 2
[ INFO] [1623707791.207196630, 983.910000000]: Manipulation plan 9 failed at stage 'approach & translate' on thread 1
[ WARN] [1623707791.208759564, 983.911000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ INFO] [1623707791.211285790, 983.913000000]: Manipulation plan 13 failed at stage 'approach & translate' on thread 3
[ INFO] [1623707791.212047639, 983.914000000]: Manipulation plan 4 failed at stage 'reachable & valid pose filter' on thread 2
[ WARN] [1623707791.215394703, 983.918000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ INFO] [1623707791.215489079, 983.918000000]: Manipulation plan 17 failed at stage 'approach & translate' on thread 0
[ WARN] [1623707791.218945310, 983.921000000]: All supplied grasps failed. Retrying last grasp in verbose mode.
[ INFO] [1623707791.219172671, 983.921000000]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1
[ INFO] [1623707791.219931602, 983.922000000]: Orientation constraint satisfied for link 'wrist_roll_link'. Quaternion desired: 0.259278 0.281263 -0.627287 0.678363, quaternion actual: 0.258732 0.283651 -0.627772 0.677128, error: x=0.002818, y=0.003004, z=0.003749, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1623707791.219998129, 983.922000000]: Position constraint satisfied on link 'wrist_roll_link'. Desired: 0.640203, 0.338856, 1.183325, current: 0.639639, 0.339071, 1.182638
[ INFO] [1623707791.220075236, 983.922000000]: Differences 0.000563999 -0.000215446 0.000687216
[ WARN] [1623707791.225895175, 983.927000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ INFO] [1623707791.228777867, 983.927000000]: Orientation constraint satisfied for link 'wrist_roll_link'. Quaternion desired: 0.259278 0.281263 -0.627287 0.678363, quaternion actual: 0.264025 0.281867 -0.628336 0.675304, error: x=0.007870, y=0.007940, z=0.002936, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1623707791.228986305, 983.927000000]: Position constraint satisfied on link 'wrist_roll_link'. Desired: 0.640203, 0.338856, 1.183325, current: 0.639966, 0.337954, 1.183449
[ INFO] [1623707791.229817084, 983.927000000]: Differences 0.000237413 0.000901181 -0.000123408
[ WARN] [1623707791.237117237, 983.929000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ INFO] [1623707791.237878372, 983.929000000]: Orientation constraint satisfied for link 'wrist_roll_link'. Quaternion desired: 0.259278 0.281263 -0.627287 0.678363, quaternion actual: 0.260180 0.284854 -0.626844 0.676928, error: x=0.002777, y=0.007044, z=0.002544, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1623707791.237952729, 983.929000000]: Position constraint satisfied on link 'wrist_roll_link'. Desired: 0.640203, 0.338856, 1.183325, current: 0.640710, 0.338788, 1.182677
[ INFO] [1623707791.238111078, 983.929000000]: Differences -0.000507383 6.72366e-05 0.000648808
[ WARN] [1623707791.243945588, 983.933000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ INFO] [1623707791.244501654, 983.933000000]: Orientation constraint satisfied for link 'wrist_roll_link'. Quaternion desired: 0.259278 0.281263 -0.627287 0.678363, quaternion actual: 0.256562 0.281476 -0.630616 0.676218, error: x=0.000982, y=0.003634, z=0.008848, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1623707791.244583100, 983.933000000]: Position constraint satisfied on link 'wrist_roll_link'. Desired: 0.640203, 0.338856, 1.183325, current: 0.640700, 0.339474, 1.183757
[ INFO] [1623707791.244639229, 983.933000000]: Differences -0.000496605 -0.000618717 -0.000431762
[ WARN] [1623707791.250935855, 983.939000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ INFO] [1623707791.254245737, 983.942000000]: Orientation constraint satisfied for link 'wrist_roll_link'. Quaternion desired: 0.259278 0.281263 -0.627287 0.678363, quaternion actual: 0.258757 0.281946 -0.630291 0.675488, error: x=0.001618, y=0.000326, z=0.008330, tolerance: x=0.010000, y=0.010000, z=0.010000
[ INFO] [1623707791.254893815, 983.942000000]: Position constraint satisfied on link 'wrist_roll_link'. Desired: 0.640203, 0.338856, 1.183325, current: 0.639930, 0.339556, 1.183495
[ INFO] [1623707791.255766953, 983.942000000]: Differences 0.000273425 -0.000700028 -0.000169281
[ WARN] [1623707791.262485339, 983.948000000]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 6. Try a lower max_step.
[ INFO] [1623707791.262564184, 983.948000000]: Manipulation plan 17 failed at stage 'approach & translate' on thread 2
[ INFO] [1623707791.263019476, 983.948000000]: Pickup planning completed after 0.143000 seconds
[ERROR] [1623707791.265799, 983.949000]: Pick failed in the planning stage, try again...
[ INFO] [1623707791.946623349, 984.465000000]: Planning attempt 1 of at most 1

These are my controllers, everything seems to be okay:

Thanks in advance!

albertoezquerro · June 15, 2021, 8:57am

Hello @migueelnery ,

I see you have not configured the Perception part in the MoveIt package. It could be related to this.

migueelnery · June 15, 2021, 10:15pm

Hi @albertoezquerro , thanks for your reply.

I have configured the Perception but I still got the same error. The robot can’t plan to the picking pose.

albertoezquerro · June 17, 2021, 8:54am

Hello @migueelnery ,

I’ve sent you an e-mail. Please check it.

migueelnery · June 17, 2021, 11:16pm

Hi @albertoezquerro, I saw your files and it works now!

There was an error in my fetch.srdf file. I was defining the wrong group for endeffector:

<end_effector name=“gripper” parent_link=“wrist_roll_link” group=“arm”/>

I changed for:

<end_effector name=“gripper” parent_link=“wrist_roll_link” group=“gripper” parent_group=“arm”/>

and now robot can grasp the cube.

Thank you!

kevinnoventa · April 24, 2022, 7:38am

Hi, I had the same issue with this errors :

Did I setup the perception wrong? because in my rviz like this :

albertoezquerro · April 25, 2022, 8:14am

Hello @kevinnoventa ,

Checking the images you shared, it looks like your error is related to the MoveIt controller’s configuration. Please make sure that your controllers are properly configured as explained in the course (check the ros_controllers.yaml file).

kevinnoventa · April 25, 2022, 4:53pm

hi @albertoezquerro ,

I have checked ros_controllers.yaml file, and there was something wrong in my code. I already changed it, but the robot still failed to pickup the object

# Simulation settings for using moveit_sim_controllers
moveit_sim_hw_interface:
  joint_model_group: arm
  joint_model_group_pose: start
# Settings for ros_control_boilerplate control loop
generic_hw_control_loop:
  loop_hz: 300
  cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
  joints:
    - l_wheel_joint
    - r_wheel_joint
    - torso_lift_joint
    - bellows_joint
    - head_pan_joint
    - head_tilt_joint
    - shoulder_pan_joint
    - shoulder_lift_joint
    - upperarm_roll_joint
    - elbow_flex_joint
    - forearm_roll_joint
    - wrist_flex_joint
    - wrist_roll_joint
    - l_gripper_finger_joint
    - r_gripper_finger_joint
  sim_control_mode: 1  # 0: position, 1: velocity
# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50
controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - upperarm_roll_joint
      - elbow_flex_joint
      - forearm_roll_joint
      - wrist_flex_joint
      - wrist_roll_joint
  - name: gripper_controller
    action_ns: gripper_action
    default: True
    parallel : True
    type: GripperCommand
    joints:
      - l_gripper_finger_joint
      - r_gripper_finger_joint

is this the correct settings for perception?

albertoezquerro · April 26, 2022, 8:06am

Hello @kevinnoventa ,

What does it mean that it failed to pickup the object? The arm doesn’t move at all? Does it reach the object but the gripper doesn’t open/close? Does the gripper try to grasp it?

The perception configuration seems right to me.