def test_link_b_set_but_body_b_not(self, box_setup): """ :type box_setup: GiskardTestWrapper """ ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.link_b = u'asdf' box_setup.add_collision_entries([ce]) box_setup.send_and_check_goal(MoveResult.INSOLVABLE)
def test_unknown_link_b(self, box_setup): """ :type box_setup: GiskardTestWrapper """ ce = CollisionEntry() ce.type = CollisionEntry.AVOID_COLLISION ce.body_b = u'box' ce.link_b = 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_allow_collision(self, box_setup): """ :type box_setup: GiskardTestWrapper """ p = PoseStamped() p.header.frame_id = box_setup.r_tip p.header.stamp = rospy.get_rostime() p.pose.position = Point(0.15, 0, 0) p.pose.orientation = Quaternion(0, 0, 0, 1) collision_entry = CollisionEntry() collision_entry.type = CollisionEntry.ALLOW_COLLISION collision_entry.body_b = u'box' collision_entry.link_b = u'base' box_setup.wrapper.set_collision_entries([collision_entry]) 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.0) box_setup.check_cpi_leq(box_setup.get_r_gripper_links(), 0.0)