def generate_random_entity(id=None): entity_info = EntityInfo() if not id: entity_info.id = str(hash(entity_info)) entity_info.type = random.choice(["random_from_magicmock", "human", "coke", "fanta"]) # entity.data = mock.MagicMock() entity_info.data = "" entity = from_entity_info(entity_info) entity._pose = random_kdl_frame() return entity
def test_grammar(): # Export a (default) robot env. This is necessary because the action server # loads knowledge on construction of actions. # It is desirable to improve this in the future. os.environ["ROBOT_ENV"] = "robotics_testlabs" knowledge = load_knowledge('challenge_demo') # Construct a Mockbot object and add a number of static entities robot = Mockbot() robot.ed._static_entities = { "couch_table": from_entity_info(EntityInfo(id="couch_table")), "operator": from_entity_info(EntityInfo(id="operator")), } robot.ed._dynamic_entities = dict() test_grammar(robot, knowledge.grammar, knowledge.grammar_target)
def __init__(self): allowed_planning_time = 0.0 ARM_AND_LINEAR_GROUP_NAME = "arm_and_linear" ARM_GROUP_NAME = "arm_torso" GRIPPER_GROUP = "gripper" REFERENCE_FRAME = "base_link" PLANNER = "RRTConnectkConfigDefault" #PLANNER = "RRTstarkConfigDefault" MOVE_GROUP_SERVICE = "/move_group/plan_execution/set_parameters" result = False upright_constraints = None robot = None arm = None arm_and_linear = None database = None scene = None grasp_generator = None place_generator = None marker_publisher = None self.robot = RobotCommander() self.arm = self.robot.get_group("arm_torso") self.gripper = self.robot.get_group(GRIPPER_GROUP) self.scene = PlanningSceneInterface() self.pickup_object = RecognizedObject("part") self.table = EntityInfo()
def execute(self, userdata=None): arm = self.arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" if self.item_designator: arm.occupied_by = self.item_designator.resolve() else: if self.item_label != "": handed_entity = EntityInfo(id=self.item_label) arm.occupied_by = handed_entity else: rospy.logerr( "No grabbed entity designator and no label for dummy entity given" ) return "failed" if arm.handover_to_robot(self.timeout): arm.send_gripper_goal('close') rospy.sleep(2.0) return "succeeded" else: rospy.logwarn("Eventhough no force is felt the gripper will close") arm.send_gripper_goal('close') rospy.sleep(2.0) return "timeout"
def execute(self, userdata=None): arm = self.arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" if self.item_designator: arm.occupied_by = self.item_designator.resolve() else: if self.item_label != "": handed_entity = EntityInfo(id=self.item_label) arm.occupied_by = handed_entity else: rospy.logerr("No grabbed entity designator and no label for dummy entity given") return "failed" if arm.handover_to_robot(self.timeout): return "succeeded" else: return "timeout"
def execute(self, userdata=None): arm = self._arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" gravitation = 9.81 try_current = 0 measure_force = MeasureForce(self._robot) # To be able to determine the weight of the object the difference between the default weight and the weight with # the object needs to be taken. Note that initially the weight with the object is set to the default weight to # be able to enter the while loop below. arm_weight = measure_force.get_force() arm_with_object_weight = arm_weight weight_object = numpy.linalg.norm(numpy.subtract(arm_weight, arm_with_object_weight)) / gravitation while weight_object < self._minimal_weight and try_current < self._try_num: if try_current == 0: self._robot.speech.speak("Let me try to pick up the garbage") else: self._robot.speech.speak("I failed to pick up the trash, let me try again") rospy.loginfo("The weight I felt is %s", weight_object) try_current += 1 # This opening and closing is to make sure that the gripper is empty and closed before measuring the forces # It is necessary to close the gripper since the gripper is also closed at the final measurement # arm.send_gripper_goal('open') # arm.wait_for_motion_done() # arm.send_gripper_goal('close', max_torque=1.0) # arm.wait_for_motion_done() arm_weight = measure_force.get_force() rospy.loginfo("Empty weight %s", arm_weight) # Open gripper arm.send_gripper_goal('open') arm.wait_for_motion_done() # Go down and grab try: arm.move_down_until_force_sensor_edge_up(timeout=7) except TimeOutException: rospy.logwarn("No forces were felt, however no action is taken!") pass # self._robot.torso.send_goal("grab_trash_down") # self._robot.torso.wait_for_motion_done() # rospy.sleep(3) arm.send_gripper_goal('close', max_torque=1.0) arm.wait_for_motion_done() # Go up and back to pre grasp position self._robot.torso.send_goal("grab_trash_up") self._robot.torso.wait_for_motion_done() arm_with_object_weight = measure_force.get_force() rospy.loginfo("Full weight %s", arm_with_object_weight) weight_object = numpy.linalg.norm(numpy.subtract(arm_weight, arm_with_object_weight)) / gravitation rospy.loginfo("weight_object = {}".format(weight_object)) # Go back and pull back arm self._robot.head.look_up() self._robot.head.wait_for_motion_done() self._robot.base.force_drive(-0.5, 0, 0, 2.0) arm.send_joint_goal('handover') arm.wait_for_motion_done() arm.send_joint_goal('reset') arm.wait_for_motion_done() self._robot.head.reset() # # if weight_object < self._minimal_weight: # return "failed" self._robot.speech.speak("Look at this I can pick up the trash!") handed_entity = EntityInfo(id="trash") arm.occupied_by = handed_entity return "succeeded"