search

Kinovarobotics / kinova-ros

ROS packages for Jaco2 and Mico robotic arms
BSD 3-Clause "New" or "Revised" License
365 stars 317 forks source link

MoveIt Commander Fails when attempting to move closed hands #346

Closed jimzers closed 2 years ago

jimzers commented 3 years ago

Description

After closing the fingers in a Kinova Arm (spherical, 7dof, gen2), any attempts to plan arm movement fails.

Version

ROS distribution : Melodic, Ubuntu 18.04

Branch and commit you are using : melodic-devel

Steps to reproduce

  1. close the fingers (manually, or through programmatic control)
  2. attempt to call any MoveIt planning library to move (joint states, poses, fails for both)

Error output:

[ INFO] [1627678956.287731414]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1627678956.287814394]: Planning attempt 1 of at most 1
[ INFO] [1627678956.288449221]: Found a contact between 'j2s7s300_link_finger_tip_3' (type 'Robot link') and 'j2s7s300_link_finger_tip_1' (type 'Robot link'), which constitutes a collision. Contact information is not stored.
[ INFO] [1627678956.288479992]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1627678956.288506448]: Start state appears to be in collision with respect to group arm
[ WARN] [1627678956.388444145]: Unable to find a valid state nearby the start state (using jiggle fraction of 0.050000 and 100 sampling attempts). Passing the original planning request to the planner.
[ INFO] [1627678956.389021432]: Planner configuration 'arm[PRMstarkConfigDefault]' will use planner 'geometric::PRMstar'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1627678956.389108097]: arm[PRMstarkConfigDefault]: problem definition is not set, deferring setup completion...
[ WARN] [1627678956.389316252]: arm[PRMstarkConfigDefault]: Skipping invalid start state (invalid state)
[ERROR] [1627678956.389391040]: arm[PRMstarkConfigDefault]: There are no valid initial states!
[ INFO] [1627678956.389420876]: No solution found after 0.000262 seconds
[ WARN] [1627678956.399064865]: Goal sampling thread never did any work.
[ INFO] [1627678956.399181680]: Unable to solve the planning problem
[ INFO] [1627678956.588260830]: Received event 'stop'

Code example (if necessary)

    # compute the path
    (plan, fraction) = self.group.compute_cartesian_path(
        waypoints,  # waypoints to follow
        0.002,  # eef_step
        420.0)  # jump_threshold

    # execute the path
    rospy.loginfo('starting...')
    self.group.execute(plan, wait=True)

Expected behavior

Should expect a path to be calculated.

Any other information

A fix might involve removing the individual collision meshes from the Kinova hand and simplifying it into a single box mesh... However I wasn't able to find a distinct <visual> or <collision> tag in the .xacro files.

felixmaisonneuve commented 3 years ago

Hi @jimzers,

This is because moveit has joint limits and it is possible for the actual gripper (on the real robot) to go beyond those limits. We do not use <collision> tags. Instead, we defined a limited angle range for each joint.

You can find those limits in kortex_description/grippers/robotiq_2f_{85/140}/urdf/robotiq_arg2f_{85/140}_model_macro.xacro. In this file, you will find xacro:macro tags and you will need to edit some of the limits.

For example, you will need to edit the finger_joint joint limits from 

<limit lower="0" upper="0.8" velocity="2.0" effort="1000" />

to 

<limit lower="0" upper="0.82" velocity="2.0" effort="1000" />

You might also need to change other elements, I'm thinking of right_outer_knuckle_joint (that seems to also limit the movement to 0.8).

Finally, you will probably need to modify the "hard limits" in kortex_description/grippers/robotiq_2f_{85/140}/config/joint_limits.yaml.

I have not tested on a real arm, so I don't know what value you should use, but at 0.82 it seems to me the fingers are overlapping a bit, so you should be good to manually close the gripper and use moveit after. With that value however, it might be possible that fully closing the gripper with moveit does not work because the actual gripper is unable to get to the position.

Let me know how it goes, Felix

felixmaisonneuve commented 3 years ago

Hi @jimzers,

Have you tried the modifications I suggested? If so, did it solve your problem? I will wait a couple weeks and close this issue if I don't get an answer.

Regards, Felix