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)
