def move_robot(robot_name, to, object):
ProcessModule.perform(
MotionDesignator([
('type', 'moving'),
('target', moving_targets[robot_name][to][object][0]),
('orientation', moving_targets[robot_name][to][object][1])
]))
def park_arms(arms):
print("Parking arms {}.".format(arms))
left_arm = [('left-arm', 'park')] if arms in [Arms.LEFT, Arms.BOTH] else []
right_arm = [('right-arm',
'park')] if arms in [Arms.RIGHT, Arms.BOTH] else []
ProcessModule.perform(
MotionDesignator([('type', 'move-arm-joints')] + left_arm + right_arm))
def park_arms(robot_name):
# Parking description
park_desc = [('type', 'move-arm-joints'), ('left-arm', 'park')]
if robot_name != 'donbot' and robot_name != 'hsr':
park_desc.append(('right-arm', 'park'))
# Perform Parking with MotionDesignator
ProcessModule.perform(MotionDesignator(park_desc))
def place(arm, btr_object, target):
print("Placing {} with {} at {}.".format(btr_object, arm, target))
motion_arm = "left" if arm is Arms.LEFT else "right"
ProcessModule.perform(
MotionDesignator([('type', 'place'), ('object', btr_object),
('arm', motion_arm), ('target', target)]))
if filter_contact_points(
btr_object.prop_value("bullet_obj").contact_points_simulated(),
[0, 1, 2]):
raise PlanFailure()
def pick_up(arm, btr_object):
print("Picking up {} with {}.".format(btr_object, arm))
motion_arm = "left" if arm is Arms.LEFT else "right"
# TODO: Hack to detach from kitchen.. (Should go into process module maybe)
try:
btr_object.prop_value('bullet_obj').detach(
BulletWorld.current_bullet_world.get_objects_by_name("kitchen")[0])
except KeyError:
print("Not attached to anything!")
ProcessModule.perform(
MotionDesignator([('type', 'pick-up'), ('object', btr_object),
('arm', motion_arm)]))
def close_container(object_designator, arm):
object_type = object_designator.prop_value('type')
if object_type in ["container", "drawer"]:
motion_type = "closing-prismatic"
elif object_type in ["fridge"]:
motion_type = "closing-rotational"
else:
raise NotImplementedError()
joint, handle = get_container_joint_and_handle(object_designator)
arm = "left" if arm == Arms.LEFT else "right"
environment = object_designator.prop_value('part-of')
ProcessModule.perform(
MotionDesignator([('type', motion_type), ('joint', joint),
('handle', handle), ('arm', arm),
('part-of', environment)]))
def detect(object_designator):
print("Detecting object of type {}.".format(
object_designator.prop_value("type")))
det_object = ProcessModule.perform(
MotionDesignator([('type', 'detecting'),
('object', object_designator.prop_value("type"))]))
if det_object is None:
raise PlanFailure("No object detected.")
detected_obj_desig = object_designator.copy([("pose",
det_object.get_pose()),
("bullet_obj", det_object)])
detected_obj_desig.equate(object_designator)
return detected_obj_desig
def place_item():
joints1 = [
-0.34272141100183123, 0.05644136829402769, -1.524766011073404,
-0.7687056842847906, 67.41096823133962, -1.6232834354990002,
-31.599376507705365
]
joints2 = [
-0.3636967851931485, -0.04329604604612444, 0.1373117642504671,
-0.3598731953377081, 98.97627247347805, -1.619118262120206,
-0.18285877641116244
]
ProcessModule.perform(
MotionDesignator([('type', 'setting_arm_joints'), ('arm', '1'),
('positions', joints1)]))
rospy.sleep(3)
ProcessModule.perform(
MotionDesignator([('type', 'setting_gripper'), ('gripper', '1'),
('position', 1)]))
rospy.sleep(3)
ProcessModule.perform(
MotionDesignator([('type', 'setting_arm_joints'), ('arm', '1'),
('positions', joints2)]))
arm = targets[object_types[i]][
1] if robot_description.i.name != 'donbot' else 'left'
move_object(object_types[i], position_target, arm,
robot_description.i.name)
else:
# Spawn object to be manipulated from hsr
robot_name = 'hsr'
hsr_cereal = Object("hsr_cereal", "cereal",
"../../resources/breakfast_cereal.stl",
[0.2, 1.6, 0.5])
# Park arms
park_arms(robot_name)
# Move to object
move_robot(robot_name, 'hsr_cereal')
# Look at object
ProcessModule.perform(
MotionDesignator([('type', 'looking'), ('target', 'down')]))
# Detect object
det_obj = ProcessModule.perform(
MotionDesignator([('type', 'detecting'), ('object', 'cereal')]))
# Open Gripper
ProcessModule.perform(
MotionDesignator([('type', 'opening-gripper'), ('gripper', 'left')]))
# Pick up detected object
ProcessModule.perform(
MotionDesignator([('type', 'pick-up'), ('object', det_obj),
('arm', 'left')]))
# Close Gripper
ProcessModule.perform(
MotionDesignator([
('type', 'closing-gripper'),
('gripper', 'left'),
def grab_item():
with par as state:
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('pose', goal),
('frame', 'map')]))
while True:
p = pose.get_value().pose.position
if math.hypot(goal.position.x - p.x, goal.position.y - p.y) < 0.1:
joints1 = [
-0.0859596170472463, -0.11647065453147773,
-2.041268906810807, -0.6065624875352394, 93.16130975194685,
-1.3021992881147604, 0.3898056299396988
]
joints2 = [
-0.059014966406225944, 0.2889082169850691,
-1.447956049242228, -0.46179177615171163,
92.38934037659165, -1.3021992881147604, 0.3898056299396988
]
joints3 = [
-0.3636967851931485, -0.04329604604612444,
0.1373117642504671, -0.3598731953377081, 98.97627247347805,
-1.619118262120206, -0.18285877641116244
]
ProcessModule.perform(
MotionDesignator([('type', 'setting_arm_joints'),
('arm', '1'), ('positions', joints1)]))
rospy.sleep(3)
ProcessModule.perform(
MotionDesignator([('type', 'setting_arm_joints'),
('arm', '1'), ('positions', joints2)]))
rospy.sleep(3)
ProcessModule.perform(
MotionDesignator([('type', 'setting_gripper'),
('gripper', '1'), ('position', 0)]))
rospy.sleep(3)
ProcessModule.perform(
MotionDesignator([('type', 'setting_arm_joints'),
('arm', '1'), ('positions', joints3)]))
break
rospy.sleep(0.1)
if r_gripper_state.get_value() < 0.01:
ProcessModule.perform(
MotionDesignator([('type', 'setting_gripper'), ('gripper', '1'),
('position', 1)]))
raise
def reset_robot():
with par as state:
ProcessModule.perform(
MotionDesignator([('type', 'setting_gripper'), ('gripper', '0'),
('position', 1)]))
ProcessModule.perform(
MotionDesignator([('type', 'setting_gripper'), ('gripper', '1'),
('position', 1)]))
ProcessModule.perform(
MotionDesignator([('type', 'setting_arm_joints'), ('arm', '0'),
('positions', [
0.863628806499912, -0.35204634320186196,
0.062598299461055, -1.5624834948006732,
12.337630347710876, -2.015093455343633,
-109.33697662946868
])]))
ProcessModule.perform(
MotionDesignator([('type', 'setting_arm_joints'), ('arm', '1'),
('positions', [
-0.5229603971359181, -0.3527442747622196,
0.11999344300251935, -1.6681661141106483,
94.62327918044838, -2.0109160876170535,
0.11287135350578059
])]))
ProcessModule.perform(
MotionDesignator([('type', 'setting_torso'), ('position', 0.3)]))
state.wait_for()
goal = Pose()
goal.orientation.w = 1.0
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('pose', goal),
('frame', 'map')]))
94.62327918044838, -2.0109160876170535,
0.11287135350578059
])]))
ProcessModule.perform(
MotionDesignator([('type', 'setting_torso'), ('position', 0.3)]))
state.wait_for()
goal = Pose()
goal.orientation.w = 1.0
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('pose', goal),
('frame', 'map')]))
print('Initializing demo...')
ProcessModule.perform(
MotionDesignator([('type', 'subscribing_pose'), ('fl', pose)]))
ProcessModule.perform(
MotionDesignator([('type', 'subscribing_gripper'), ('gripper', '1'),
('fl', r_gripper_state)]))
reset_robot()
goal = Pose()
look_goal = Pose()
def grab_item():
with par as state:
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('pose', goal),
('frame', 'map')]))
def open_gripper(gripper):
print("Opening gripper {}".format(gripper))
ProcessModule.perform(
MotionDesignator([('type', 'opening-gripper'), ('gripper', gripper)]))
def navigate(target, orientation=[0, 0, 0, 1]):
print("Moving to {}. Orientation: {}.".format(target, orientation))
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('target', target),
('orientation', orientation)]))
def close_gripper(gripper):
print("Closing gripper {}.".format(gripper))
ProcessModule.perform(
MotionDesignator([('type', 'closing-gripper'), ('gripper', gripper)]))
def move_object(object_type, target, arm, robot_name):
# Get Gripper frame
gripper = robot_description.i.get_tool_frame(arm)
# Move to sink
with par as s:
park_arms(robot_name)
move_robot(robot_name, 'sink', object_type)
# Access object if needed
if object_type == "spoon":
ProcessModule.perform(
MotionDesignator([
('type', 'accessing'),
('drawer-joint', 'sink_area_left_upper_drawer_main_joint'),
('drawer-handle', 'sink_area_left_upper_drawer_handle'),
('arm', 'left'), ('distance', 0.3), ('part-of', kitchen)
]))
if robot_name == "boxy":
park_arms("boxy")
ProcessModule.perform(
MotionDesignator([('type', 'moving'),
('target', [-0.09, 0.61, 0]),
('orientation', [0, 0, 0, 1])]))
# Look at object
ProcessModule.perform(
MotionDesignator([('type', 'looking'), ('target', object_type)]))
# Detect object
# Try to detect object via camera, if this fails...
det_obj = ProcessModule.perform(
MotionDesignator([('type', 'detecting'), ('object', object_type)]))
block_new = None
if det_obj:
block = btr.blocking(det_obj, BulletWorld.robot, gripper)
block_new = list(filter(lambda obj: obj.type != "environment", block))
else:
# ... the robot grasps the object by using its knowledge of the environment.
det_obj = ProcessModule.perform(
MotionDesignator([('type', "world-state-detecting"),
('object', object_type)]))
# If something is in the way, move it first and then move back to the sink.
if block_new:
move_object(block_new[0].type, targets[block_new[0].type][0], arm,
robot_name)
move_robot(robot_name, 'sink', object_type)
if det_obj.type == "spoon":
kitchen.detach(det_obj)
# Pick up the object
ProcessModule.perform(
MotionDesignator([('type', 'pick-up'), ('object', det_obj),
('arm', arm)]))
park_arms(robot_name)
# Move to island
move_robot(robot_name, 'island', object_type)
# Look at target (also quickfix for not colliding with kitchen if robot has odom frame :/ )
ProcessModule.perform(
MotionDesignator([('type', 'looking'),
('target', targets[object_type][0])]))
# Place object if target pose of object is reachable for the robots manipulator
if btr.reachable_pose(target, robot, gripper, threshold=0.05):
ProcessModule.perform(
MotionDesignator([('type', 'place'), ('object', det_obj),
('target', target), ('arm', arm)]))
park_arms(robot_name)
print("placed: ", object_type)
# if not btr.stable(det_obj):
# raise btr.ReasoningError
targets[object_type][2] = True
def look_at(target):
print("Looking at {}.".format(target))
ProcessModule.perform(
MotionDesignator([('type', 'looking'), ('target', target)]))
def move_object(object_type, target, arm):
gripper = "l_gripper_tool_frame" if arm == "left" else "r_gripper_tool_frame"
with par as s:
ProcessModule.perform(
MotionDesignator([('type', 'move-arm-joints'),
('left-arm', 'park'), ('right-arm', 'park')]))
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('target', [0.65, 0.7, 0]),
('orientation', [0, 0, 0, 1])]))
if object_type == "spoon":
ProcessModule.perform(
MotionDesignator([
('type', 'accessing'),
('drawer-joint', 'sink_area_left_upper_drawer_main_joint'),
('drawer-handle', 'sink_area_left_upper_drawer_handle'),
('arm', 'left'), ('distance', 0.3), ('part-of', kitchen)
]))
ProcessModule.perform(
MotionDesignator([('type', 'looking'), ('target', [1.3, 0.6, 1])]))
det_obj = ProcessModule.perform(
MotionDesignator([('type', 'detecting'), ('object', object_type)]))
block = btr.blocking(det_obj, BulletWorld.robot, gripper)
block_new = list(filter(lambda obj: obj.type != "environment", block))
if block_new:
move_object(block_new[0].type, targets[block_new[0].type][0],
targets[block_new[0].type][1])
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('target', [0.65, 0.7, 0]),
('orientation', [0, 0, 0, 1])]))
ProcessModule.perform(
MotionDesignator([('type', 'pick-up'), ('object', det_obj),
('arm', arm)]))
ProcessModule.perform(
MotionDesignator([('type', 'move-arm-joints'), ('right-arm', 'park')]))
ProcessModule.perform(
MotionDesignator([('type', 'moving'), ('target', [-0.3, 1, 0]),
('orientation', [0, 0, 1, 0])]))
if btr.reachable_pose(targets[object_type][0], robot, gripper):
ProcessModule.perform(
MotionDesignator([('type', 'place'), ('object', det_obj),
('target', target), ('arm', arm)]))
ProcessModule.perform(
MotionDesignator([('type', 'move-arm-joints'), ('left-arm', 'park'),
('right-arm', 'park')]))
print("placed: ", object_type)
if not btr.stable(det_obj):
raise btr.ReasoningError
targets[object_type][2] = True