def test_unknown_robot_link(self, box_setup): """ :type box_setup: GiskardTestWrapper """ ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.robot_link = u'asdf' box_setup.add_collision_entries([ce]) box_setup.send_and_check_goal(MoveResult.UNKNOWN_OBJECT)
def avoid_collision(self, min_dist, robot_link=u'', body_b=u'', link_b=u''): collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.AVOID_COLLISION collision_entry.min_dist = min_dist collision_entry.robot_link = str(robot_link) collision_entry.body_b = str(body_b) collision_entry.link_b = str(link_b) self.set_collision_entries([collision_entry])
def test_attached_collision_avoidance(self, box_setup): """ :type box_setup: GiskardTestWrapper """ pocky = 'http://muh#pocky' box_setup.attach_box(pocky, [0.1, 0.02, 0.02], box_setup.r_tip, [0.05, 0, 0]) ces = [] ce = CollisionEntry() ce.type = CollisionEntry.ALLOW_COLLISION ce.robot_link = pocky ce.body_b = 'box' ces.append(ce) box_setup.add_collision_entries(ces) p = PoseStamped() p.header.frame_id = box_setup.r_tip p.header.stamp = rospy.get_rostime() p.pose.position.y = -0.11 p.pose.orientation.w = 1 box_setup.set_and_check_cart_goal(box_setup.default_root, box_setup.r_tip, p) box_setup.check_cpi_geq(box_setup.get_l_gripper_links(), 0.048)