0
$\begingroup$

Rosanswers logo

Hallo guys,

I am trying to get a pick/place with an ABB_irb2600 to work and ran into a problem: Every time the Grasp() happens the finger of my gripper (Added by myself into the ABBs urdf, see below) is moved about 20cm in x direction (of the parent link). It does not matter which of the two fingers is actuated. The problem that results from this is that the detached finger is still colliding with my environment and thus messes up the trajectory planning which in turn is failing my place() attempts.

I tried resetting the gripper into the "open" state which I introduced in the moveit setup_assistant but it is not working. Adding limits in the urdf did not yield results as well. Reducing the amount of obstacles (I try picking from one table and placing on another) helps but is still failing every other execution.

I checked out this post, but I guess it is a different Problem, because he had no collision.

I would have realy liked to supply you with pictures, but my karma is to low to upload any files.

My setup is Ubuntu 16.04 LTS with 4.15.0-52-generic kernel and i am running ROS kinetic. For visualization i use the demo.launch file from the abb_irb2600_moveit_config package supplied by ABB. For my imports look at the code below, please.

[ WARN] [1561727424.886234042]: Fail: ABORTED: No motion plan found. No execution attempted.
Is the only warning/error i get.

The pick works most of the time, but the place has only 20% success rate.

Thanks in advance!

Shawn


gripper URDF

 <link name="palm">
    <visual>
      <geometry>
        <box size="0.15 0.05 0.05"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.15 0.05 0.05"/>
      </geometry>
    </collision>
  </link>
  <!-- <link name ="sucker">
    <visual>
      <geometry>
        <cylinder length ="0.05" radius ="0.025"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length ="0.05" radius ="0.025"/>
      </geometry>
    </collision>
  </link> -->
  <link name="right_finger">
    <visual>
      <geometry>
        <box size="0.02 0.1 0.05"/>
      </geometry>
      <material name="">
        <color rgba="0.0 0.3490196 0.0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.02 0.1 0.05"/>
      </geometry>
    </collision>
  </link>
  <link name="left_finger">
    <visual>
      <geometry>
        <box size="0.02 0.1 0.05"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.02 0.1 0.05"/>
      </geometry>
    </collision>
  </link>
  <!-- Joints -->
 <joint name="link_6_to_palm" type="fixed">
    <parent link="link_6"/>
    <child link="palm"/>
    <origin xyz="0.025 0 0" rpy="0 0 1.5707963267948966"/>
  </joint>
   <joint name="palm_to_right_finger" type="prismatic">
    <parent link="palm"/>
    <child link="right_finger"/>
    <limit effort="1000.0" lower="-0.055" upper="0.0" velocity="0.5"/>
    <safety_contoller soft_lower_limit = "-0.055" soft_upper_limit = "0.0"/>
    <origin xyz="0.065 -0.075 0.0"/>

My Code

#!/usr/bin/env python

import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)

    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("ABB_arm")
    right_gripper = MoveGroupCommander("gripper")
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("table1")
    scene.remove_world_object("table2")
    scene.remove_world_object("floor")
    scene.remove_world_object("part")

    rospy.sleep(1)

    #remove the attached part to allow re-runs
    if scene.get_attached_objects(object_ids = []) == {}:
        print "no attached objects found"
    else:
        scene.remove_attached_object("palm", "part")
        print "deleted attached objects"

    rospy.sleep(1)

    #moving to start position
    right_arm.set_named_target("only_zeros")
    right_arm.go()

    print "only_zeros"

    #set the gripper to open
    right_gripper.set_named_target("open")
    right_gripper.go()

    print "open"

    rospy.sleep(1)

    # publish a demo scene
    p = PoseStamped()
    p.header.frame_id = robot.get_planning_frame()

    #add the floor
    p.pose.position.x = 0
    p.pose.position.y = 0
    p.pose.position.z = -0.1
    scene.add_box("floor", p, (2, 2, 0.2))

    # add a table
    p.pose.position.x = 1.2
    p.pose.position.y = -0.2
    p.pose.position.z = 0.4
    scene.add_box("table1", p, (0.5, 1.5, 0.8))

    # add another table
    p.pose.position.x = -1.2
    p.pose.position.y = -0.2
    p.pose.position.z = 0.4
    scene.add_box("table2", p, (0.5, 1.5, 0.8))

    # add an object to be grasped
    p.pose.position.x = 1
    p.pose.position.y = -0.12
    p.pose.position.z = 0.84
    scene.add_box("part", p, (0.08, 0.08, 0.08))

    # define the place PlaceLocation
    p.pose.position.x = -1
    p.pose.position.y = -0.12
    p.pose.position.z = 0.84

    print "scene complete"

    rospy.sleep(1)

    grasps = []

    for x in range(0, 2):
        # define grasps (poses taken from rviz)
        g = Grasp()
        pose_for_grasps = []
        pose_for_grasps.insert(0, [0.86, -0.12, 0.84, 0, 0, 0, 1])
        pose_for_grasps.insert(1, [1, -0.12, 0.98, 0.0, 0.7071, 0.0, 0.7071])
        approach_direction = []
        approach_direction.insert(0, [1, 0, 0, 0.005, 0.1])
        approach_direction.insert(1, [0, 0, 1, 0.005, 0.1])
        retreat_direction = []
        retreat_direction.insert(0, [0, 0, 1, 0.15, 0.05])
        retreat_direction.insert(1, [0, 0, 1, 0.15, 0.05])
        grasp_id = ["test0", "test1"]

        g.id = grasp_id[x]
        grasp_pose = PoseStamped()
        grasp_pose.header.frame_id = "base_link"
        grasp_pose.pose.position.x = pose_for_grasps[x][0]
        grasp_pose.pose.position.y = pose_for_grasps[x][1]
        grasp_pose.pose.position.z = pose_for_grasps[x][2]
        grasp_pose.pose.orientation.x = pose_for_grasps[x][3]
        grasp_pose.pose.orientation.y = pose_for_grasps[x][4]
        grasp_pose.pose.orientation.z = pose_for_grasps[x][5]
        grasp_pose.pose.orientation.w = pose_for_grasps[x][6]

        # set the grasp pose
        g.grasp_pose = grasp_pose

        # define the pre-grasp approach
        g.pre_grasp_approach.direction.header.frame_id = "base_link"
        g.pre_grasp_approach.direction.vector.x = approach_direction[x][0]
        g.pre_grasp_approach.direction.vector.y = approach_direction[x][1]
        g.pre_grasp_approach.direction.vector.z = approach_direction[x][2]
        g.pre_grasp_approach.min_distance = approach_direction[x][3]
        g.pre_grasp_approach.desired_distance = approach_direction[x][4]

        g.pre_grasp_posture.header.frame_id = "palm"
        g.pre_grasp_posture.joint_names = ["palm_to_left_finger"]

        pos = JointTrajectoryPoint()
        pos.positions.append(0.0)

        g.pre_grasp_posture.points.append(pos)

        # set the grasp posture
        g.grasp_posture.header.frame_id = "palm"
        g.grasp_posture.joint_names = ["palm_to_left_finger"]

        pos = JointTrajectoryPoint()
        pos.positions.append(0.2)
        pos.effort.append(0.0)
        pos.time_from_start.secs = 1

        g.grasp_posture.points.append(pos)

        # set the post-grasp retreat
        g.post_grasp_retreat.direction.header.frame_id = "base_link"
        g.post_grasp_retreat.direction.vector.x = retreat_direction[x][0]
        g.post_grasp_retreat.direction.vector.y = retreat_direction[x][1]
        g.post_grasp_retreat.direction.vector.z = retreat_direction[x][2]
        g.post_grasp_retreat.desired_distance = retreat_direction[x][3]
        g.post_grasp_retreat.min_distance = retreat_direction[x][4]

        g.allowed_touch_objects = ["part"]

        g.max_contact_force = 0

        # append the grasp to the list of grasps
        grasps.append(g)

    rospy.sleep(0.5)
    print "trying to pick"

    # pick the object
    robot.ABB_arm.pick("part", grasps)

    print "after pick"

    rospy.sleep(0.5)

    print "moving to zero"
    right_arm.set_named_target("only_zeros")
    right_arm.go()

    if scene.get_attached_objects(object_ids = []) == {}:
        print "couldn't find pick solution"
        sys.exit()
    else:
        print "trying to place"
        robot.ABB_arm.place("part", p)
        print "after place"
        right_arm.set_named_target("only_zeros")
        right_arm.go()
        print "finished pick and place"
        sys.exit()

    roscpp_shutdown()

Originally posted by Shawn on ROS Answers with karma: 18 on 2019-06-28

Post score: 0


Original comments

Comment by gvdhoorn on 2019-06-28:
Please post your edit as an answer and accept your own answer.

That will show much more clearly that your question was answered than a [closed] added to the title.

Comment by gvdhoorn on 2019-06-28:
You posted it as a comment. I've converted that to an answer.

And again: please don't close questions when they are answered.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

EDIT: I found the problem 20 mins after posting this: i set pos.positions.append(0.2) to pos.positions.append(0.01) and it solves the problem as it should. so the problem was in front of the PC once more.


Originally posted by Shawn with karma: 18 on 2019-06-28

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.