Moveit Commander unable to plan with subframe as end effector link. Error link not found
setendeffectorlink(collisionobject/subframe) throws Error: Link 'collisionobject/subframe' not found in model 'ur10erobot' when planning
I am using the Moveit Python interface and am trying to set the end effector link to the subframe of an attached collision object.
The moveitconfig has been regenerated with the latest version of the setup assistant and the ResolveConstraintFrames planning adapter is present. My workflow is the following: create the attached collision object and set the mesh, id, link name, subframe name and subframe pose. I add the object to the scene, check it is in the scene and set the end effector link to the subframe. When trying to plan a pose I get the following error: Link 'endeffector/tcp' not found in model 'ur10e_robot' Relevant code below. Am I missing something?
def attach_endeffector(self,
name: str,
filename: str = '/dev_ws/src/trajectory_tools/end_effector/ur_ee.stl'
) -> bool:
pose = PoseStamped()
pose.header = Header()
pose.header.frame_id = self.move_group.get_end_effector_link()
pose.pose = Pose(position=Point(0, 0, 0),
orientation=Quaternion(0, 0, 1, 0))
ee = moveit_msgs.msg.AttachedCollisionObject()
ee.object = self.make_mesh(name, pose, filename=filename)
ee.object.subframe_names = ['tcp']
ee.object.subframe_poses = [Pose(
position=Point(0.184935, 0, 0.06),
orientation=Quaternion(0, 0, 1, 0))]
ee.link_name = self.move_group.get_end_effector_link()
self.scene.attach_object(ee)
return self.wait_for_state_update(name, object_is_attached=True)
When I first encountered the error I added a getting planning scene service call followed by apply planning scene to ensure the correct planning scene is being used. When inspecting the result the response of the service call the attached collision object and subframe name and pose are all present.
self.ee_name = 'end_effector'
rospy.sleep(0.2)
self.setup_scene()
self.attach_endeffector(self.ee_name)
rospy.sleep(0.2)
rospy.loginfo('waiting for get_planning_scene service')
rospy.wait_for_service('/get_planning_scene', 30)
self.get_planning_scene_service = rospy.ServiceProxy('/get_planning_scene',GetPlanningScene)
planning_scene_req = moveit_msgs.msg.PlanningSceneComponents()
planning_scene_req.components = 0
resp = self.get_planning_scene_service(planning_scene_req)
self.scene.apply_planning_scene(resp)
rospy.sleep(0.2)
self.move_group.set_end_effector_link(f'{self.ee_name}/tcp')
rospy.loginfo(
f'end effector link set to {self.move_group.get_end_effector_link()}')
Asked by vinceH on 2022-09-20 10:59:11 UTC
Comments