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 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"