How can I pick an object close to another

[WiserOrb]

Description

I'm trying to pick a small cylinder ('object1') next to another ('object2') using the grasp function, however the planning fail because the hand collides with 'object2'. I've tried to add 'object2' to the list of allowed_touch_objects, but it doesn't solve my problem.

My environment

• ROS Distro: tested on Melodic and Noetic
• OS Version: Ubuntu 18.04 for Melodic, 20.04 for Noetic
• Installed from binary, following the tutorials

Steps to reproduce

I use the panda simulator from the tutorials First I launch it

roslaunch panda_moveit_config demo.launch

Then I execute a small pick test

import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import rospy
from trajectory_msgs.msg import JointTrajectoryPoint
import tf
from math import pi, fabs, cos, sqrt
tau = 2.0 * pi


#initialization
moveit_commander.roscpp_initialize("")
rospy.init_node("pick_place", anonymous=True)
robot      = moveit_commander.RobotCommander()
scene      = moveit_commander.PlanningSceneInterface()

group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)

joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -tau / 8
joint_goal[2] = 0
joint_goal[3] = -tau / 4
joint_goal[4] = 0
joint_goal[5] = tau / 6  # 1/6 of a turn
joint_goal[6] = 0
move_group.go(joint_goal, wait=True)
move_group.stop()

#objects
obj_pose = geometry_msgs.msg.PoseStamped()
obj_pose.header.frame_id = robot.get_root_link()
obj_pose.pose.orientation.w = 1
obj_pose.pose.position.x = 0.5
obj_pose.pose.position.y = 0
obj_pose.pose.position.z = 0

scene.add_cylinder("object1", obj_pose, 0.01, 0.02)

obj_pose.pose.position.x = 0.56
scene.add_cylinder("object2", obj_pose, 0.01, 0.02)

rospy.sleep(2) #waiting for synchronization

#grasp
grasp = moveit_commander.Grasp()

#grasp pose
grasp.grasp_pose.header.frame_id = robot.get_root_link()
quat = tf.transformations.quaternion_from_euler(pi, 0, pi*3/4)
grasp.grasp_pose.pose.orientation = geometry_msgs.msg.Quaternion(*quat)
grasp.grasp_pose.pose.position.x = 0.5
grasp.grasp_pose.pose.position.y = 0
grasp.grasp_pose.pose.position.z = 0.11 #tool height

#grasp pre approach
grasp.pre_grasp_approach.direction.header.frame_id = robot.get_root_link()
grasp.pre_grasp_approach.direction.vector.z = -1.0
grasp.pre_grasp_approach.min_distance = 0.08
grasp.pre_grasp_approach.desired_distance = 0.10

#grasp post retreat
grasp.post_grasp_retreat.direction.header.frame_id = robot.get_root_link()
grasp.post_grasp_retreat.direction.vector.z = 1.0
grasp.post_grasp_retreat.min_distance = 0.08
grasp.post_grasp_retreat.desired_distance = 0.1

#open hand pose
grasp.pre_grasp_posture.joint_names = ["panda_finger_joint1", "panda_finger_joint2"]
pos = JointTrajectoryPoint(positions = [0.04,0.04])
pos.time_from_start = rospy.Duration(0.5)
grasp.pre_grasp_posture.points.append(pos)

#closed hand pose
grasp.grasp_posture.joint_names = ["panda_finger_joint1", "panda_finger_joint2"]
pos = JointTrajectoryPoint(positions = [0.01,0.01])
pos.time_from_start = rospy.Duration(0.5)
grasp.grasp_posture.points.append(pos)

#adding object2 to allowed objects
grasp.allowed_touch_objects = ['object2']


move_group.pick('object1', grasp)

Backtrace or Console output

Rviz output

[ INFO] [1650887406.966656664]: Collision checking is considered complete (collision was found and 0 contacts are stored)
[ INFO] [1650887406.967059417]: Found a contact between 'object2' (type 'Object') and 'panda_hand' (type 'Robot link'), which constitutes a collision. Contact information is not stored.

Use gist.github.com to copy-paste the console output or segfault backtrace using gdb.

[welcome[bot]]

Thanks for reporting an issue. Because we're a volunteer community, providing a pull request with suggested changes is always welcomed.

[corot]

Two years later....

@WiserOrb, I'm having the same problem. I have noticed that allowed_touch_objects on Grasp.msg and PlaceLocation.msg is never read. Only the same argument on Pick and Place action goals is used. Looks like correctly. However, I still have sporadic collisions between the gripper and one of the objects I have passed, no idea why.