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)
