def setup_primatives(self):
#self.base_roadmap = StarRoadmap(self.initial_config, lambda q1, q2: plan_base_traj(self, q1, q2))
self.base_roadmap = MultiBiRRT(self.extract_base(self.initial_config),
q_distance_fn(self.robot), q_sample_fn(self.robot), q_extend_fn(self.robot), q_collision_fn(self.env, self.robot))
self.pick_and_places = defaultdict(list)
self.pushes = defaultdict(Graph)
class ManipulationOracle(object):
def __init__(self, problem, env, preload_databases=PRELOAD_DATABASES, debug=DEBUG):
self.problem = problem
self.env = env
if debug: print SEPARATOR
self.setup_env()
self.setup_robot(debug)
self.setup_bodies()
self.setup_regions()
self.setup_bounding_volumes()
self.setup_caches()
self.setup_primatives()
if preload_databases:
for body_name in self.get_objects():
get_grasps(self, body_name)
self.ir_database = get_custom_ir(self.robot)
if debug: print SEPARATOR, '\n', self
###############
# NOTE - can use links similarly to KinBodies. Changing the link transform doesn't affect full body, but it remains relatively attached
self.base = self.robot.GetLink('base_link')
self.torso = self.robot.GetLink('torso_lift_link')
self.gripper = self.robot.GetActiveManipulator().GetEndEffector() # NOTE - the gripper is the red thing inside the hand
#self.necessary_collision = lambda b1, p1, b2, p2: self.aabb_collision(b1, trans_from_pose(p1.value), b2, trans_from_pose(p2.value))
self.necessary_collision = lambda b1, p1, b2, p2: self.sphere_collision(b1, point_from_pose(p1.value), b2, point_from_pose(p2.value))
#self.necessary_collision = lambda b1, p1, b2, p2: self.cylinder_collision(b1, point_from_pose(p1.value), b2, point_from_pose(p2.value))
#self.necessary_collision_current = lambda b1, b2: self.aabb_collision(b1, get_trans(self.get_body(b1)), b2, get_trans(self.get_body(b2)))
#self.necessary_collision_current = lambda b1, b2: self.sphere_collision(b1, get_point(self.get_body(b1)), b2, get_point(self.get_body(b2)))
#self.necessary_collision_current = lambda b1, b2: self.cylinder_collision(b1, get_point(self.get_body(b1)), b2, get_point(self.get_body(b2)))
#################################################################
def setup_env(self):
self.env.StopSimulation()
for sensor in self.env.GetSensors():
sensor.Configure(Sensor.ConfigureCommand.PowerOff)
sensor.Configure(Sensor.ConfigureCommand.RenderDataOff)
sensor.Configure(Sensor.ConfigureCommand.RenderGeometryOff)
self.env.Remove(sensor)
self.env.SetCollisionChecker(RaveCreateCollisionChecker(self.env, COLLISION_CHECKER))
self.env.GetCollisionChecker().SetCollisionOptions(0)
self.locked = False
def setup_robot(self, debug):
self.robot = self.env.GetRobots()[0] # TODO - multiple robots and store names instead of bodies
self.robot_name = get_name(self.robot)
self.cd_model = databases.convexdecomposition.ConvexDecompositionModel(self.robot)
if not self.cd_model.load(): self.cd_model.autogenerate()
if debug: print 'Loaded convex decomposition'
self.l_model = databases.linkstatistics.LinkStatisticsModel(self.robot)
if not self.l_model.load(): self.l_model.autogenerate()
if debug: print 'Loaded link statistics'
self.l_model.setRobotWeights()
self.l_model.setRobotResolutions(xyzdelta=MIN_DELTA) # xyzdelta is the minimum Cartesian distance of an object
#self.robot.SetDOFResolutions(5*self.robot.GetDOFResolutions())
#print 'Robot resolutions:', repr(self.robot.GetDOFResolutions()), '| Robot weights:', repr(self.robot.GetDOFWeights())
robot_radius = get_radius2D(self.robot)
self.default_left_arm_config = HOLDING_LEFT_ARM
self.default_right_arm_config = mirror_arm_config(REST_LEFT_ARM)
self.robot.SetDOFValues(self.default_left_arm_config, self.robot.GetManipulator('leftarm').GetArmIndices())
self.robot.SetDOFValues(self.default_right_arm_config, self.robot.GetManipulator('rightarm').GetArmIndices())
self.robot.SetDOFValues([.15], [self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetActiveManipulator(ACTIVE_ARM)
open_gripper(self)
self.initial_config = self.get_robot_config()
# TODO - depdenent on the current robot config
self.robot_links = {get_name(link): link for link in self.robot.GetLinks() if has_geometry(link)}
#self.robot_link_radii = {name: get_radius(link)**2 for name, link in self.robot_links.iteritems()}
self.robot_link_meshes = {name: link_mesh(self, link) for name, link in self.robot_links.iteritems()}
self.robot_link_convex_meshes = {name: convex_mesh(mesh_vertices(self.robot_link_meshes[name])) for name in self.robot_links}
#self.robot_link_oobbs = {name: oobb_from_points(mesh_vertices(self.robot_link_convex_meshes[name])) for name in self.robot_links}
self.robot_parts = {
'left_hand': ['l_wrist_roll_link', 'l_gripper_r_finger_link', 'l_gripper_l_finger_tip_link',
'l_gripper_r_finger_tip_link', 'l_gripper_palm_link',
'l_gripper_l_finger_link'], #[get_name(link) for link in get_hand_links(self.robot.GetManipulator('leftarm')) if has_geometry(link)],
'left_forearm': ['l_forearm_link', 'l_wrist_flex_link', 'l_elbow_flex_link'],
'left_upper_arm': ['l_upper_arm_link', 'l_upper_arm_roll_link'],
'left_shoulder': ['l_shoulder_lift_link', 'l_shoulder_pan_link'],
'right_hand': ['r_gripper_palm_link', 'r_gripper_l_finger_tip_link', 'r_gripper_r_finger_link',
'r_wrist_roll_link', 'r_gripper_l_finger_link',
'r_gripper_r_finger_tip_link'], #[get_name(link) for link in get_hand_links(self.robot.GetManipulator('rightarm')) if has_geometry(link)],
'right_forearm': ['r_forearm_link', 'r_wrist_flex_link', 'r_elbow_flex_link'],
'right_upper_arm': ['r_upper_arm_link', 'r_upper_arm_roll_link'],
'right_shoulder': ['r_shoulder_lift_link', 'r_shoulder_pan_link'],
'body': ['base_link', 'br_caster_r_wheel_link', 'br_caster_rotation_link', 'br_caster_l_wheel_link',
'bl_caster_r_wheel_link', 'bl_caster_rotation_link', 'head_tilt_link', 'fl_caster_r_wheel_link',
'fr_caster_rotation_link', 'fr_caster_l_wheel_link', 'bl_caster_l_wheel_link', 'head_pan_link',
'fr_caster_r_wheel_link', 'torso_lift_link', 'fl_caster_rotation_link', 'fl_caster_l_wheel_link',
'laser_tilt_mount_link'] #[get_name(link) for link in get_non_manipulator_links([self.robot.GetManipulator(m) for m in ['leftarm', 'rightarm']]) if has_geometry(link)]
}
self.robot_parts['not_left'] = list(flatten(self.robot_parts[part]
for part in ['body', 'right_hand', 'right_forearm', 'right_upper_arm', 'right_shoulder']))
self.robot_parts['not_right'] = list(flatten(self.robot_parts[part]
for part in ['body', 'left_hand', 'left_forearm', 'left_upper_arm', 'left_shoulder']))
self.robot_meshes = {}
self.robot_convex_meshes = {}
for name, link_names in self.robot_parts.iteritems():
reference_link = link_names[0]
refrence_trans = np.linalg.inv(get_trans(self.robot_links[reference_link]))
self.robot_meshes[name] = merge_meshes([mesh_apply_trans(self.robot_link_meshes[link_name],
np.dot(refrence_trans, get_trans(self.robot_links[link_name]))) for link_name in link_names])
self.robot_convex_meshes = {name: convex_mesh(mesh_vertices(self.robot_meshes[name])) for name in self.robot_parts}
extrema = aabb_extrema(aabb_union([aabb_from_body(body) for body in self.env.GetBodies()])).T
if not any('floor' in get_name(body) for body in self.env.GetBodies()):
extrema += WORKSPACE_ROBOT_LENGTHS*robot_radius*np.array([[-1, -1, 0], [1, 1, 0]])
self.env_min, self.env_max = extrema
self.robot.SetAffineTranslationLimits(self.env_min, self.env_max)
#self.robot.SetAffineRotationAxisLimits(-np.array([PI, PI, PI]), np.array([PI, PI, PI])) # NOTE - doesn't change SubtractDOFValues or SetActiveDOFValues
#self.robot.SetAffineRotation3DLimits(np.array([0, 0, 0]), np.array([2*PI, 2*PI, 2*PI]))
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=self.robot, iktype=IkParameterization.Type.Transform6D,
forceikfast=True, freeindices=None, freejoints=None, manip=None)
if not ikmodel.load():
if debug: print 'Creating openrave inverse kinematics model'
ikmodel.autogenerate()
if debug: print 'Saved openrave inverse kinematics model'
else:
if debug: print 'Loaded openrave inverse kinematics model'
self.base_manip = interfaces.BaseManipulation(self.robot, plannername=None, maxvelmult=None)
self.task_manip = interfaces.TaskManipulation(self.robot, plannername=None, maxvelmult=None, graspername=None)
def setup_bodies(self):
# geometry_hash(body) calls body.GetKinematicsGeometryHash, which returns a md5
# hash unique to the particular kinematic and geometric structure of a KinBody.
self.bodies = {str(get_name(body)): body for body in self.env.GetBodies()}
self.active = {body_name for body_name in self.bodies if self.env.GetKinBody(body_name) is not None}
self.body_name_to_geom_hash = {body_name: geometry_hash(body) for body_name, body in self.bodies.items()}
self.geom_hash_to_body_name = {geometry_hash(body): body_name for body_name, body in self.bodies.items()}
self.geom_hash_to_body_names = defaultdict(list)
for body_name, body in self.bodies.items():
self.geom_hash_to_body_names[geometry_hash(body)].append(body_name)
self.objects = self.problem.object_names
self.floor_objects = self.problem.floor_object_names
self.obstacles = [name for name, body in self.bodies.items() if name not in self.get_objects() and not body.IsRobot()]
self.initial_poses = {object_name: self.get_pose(object_name) for object_name in self.get_objects()}
def setup_regions(self):
floors = [AASurface('floor', zip(self.env_min, self.env_max)[:2], BODY_Z_OFFSET, color=FLOOR_COLOR)]
self.floors = [region.name for region in floors]
self.tables = self.problem.table_names
self.sinks = self.problem.sink_names
self.stoves = self.problem.stove_names
self.goal_regions = [region.name for region in self.problem.regions]
self.regions = merge_dicts({region.name: region for region in self.problem.regions},
{name: AARegion.create_on_body(self.bodies[name], color=TABLE_COLOR) for name in self.tables},
{name: AARegion.create_on_body(self.bodies[name], color=SINK_COLOR) for name in self.sinks},
{name: AARegion.create_on_body(self.bodies[name], color=STOVE_COLOR) for name in self.stoves},
{region.name: region for region in floors})
def setup_bounding_volumes(self):
self.radii2D = {name: get_radius2D(body)**2 for name, body in self.bodies.iteritems()}
self.radii = {name: get_radius(body)**2 for name, body in self.bodies.iteritems()}
self.aabbs = {}
self.meshes = {}
for name, body in self.bodies.iteritems(): # TODO - use geom_hash instead of object names
with self.body_saver(name): # NOTE - aabb_from_body(body) != aabb_apply_trans(self.aabbs[name], get_trans(body)) is okay because constructed differently
set_trans(body, unit_trans()) # - doesn't encompass different configurations of bodies
self.aabbs[name] = aabb_from_body(body)
self.meshes[name] = mesh_from_body(body)
self.convex_meshes = {name: convex_mesh(self.get_vertices(name)) for name in self.bodies}
self.oobbs = {name: oobb_from_points(self.convex_meshes[name].vertices.T) for name in self.bodies} # NOTE - self.get_aabb(name, trans) = aabb_from_body(self.get_body(name))
# TODO - make these all caches
self.get_geometries = DeterminisiticCache(lambda name: geometries(self.get_body(name)))
self.get_geometry_types = DeterminisiticCache(lambda name: geometry_types(self.get_body(name)))
def setup_caches(self):
self.object_collision = ObjectCollisionCache(self)
self.holding_collision = HoldingCollisionCache(self)
self.traj_collision = TrajCollisionCache(self)
self.traj_holding_collision = TrajHoldingCollisionCache(self)
self.edge_collision = EdgeCollisionCache(self)
self.edge_holding_collision = EdgeHoldingCollisionCache(self)
self.random_placement_cache = RandomPlacementsCache(self)
self.grid_placement_cache = GridPlacementsCache(self)
self.region_contains = RegionContainsCache(self)
self.are_stacked = AreStackedCache(self)
self.on_edge = OnEdgeCache(self)
#self.get_aabb = GetAABBCache(self)
self.extract_base = DeterminisiticCache(lambda q: Config(base_values_from_full_config(q.value)))
#self.gripper_collision = GripperCollisionCache(self)
self.approach_collision = ApproachCollisionCache(self)
self.obj_approach_collision = ObjApproachCollisionCache(self)
self.caches = [value for value in self.__dict__.values() if isinstance(value, DeterminisiticCache)]
def setup_primatives(self):
#self.base_roadmap = StarRoadmap(self.initial_config, lambda q1, q2: plan_base_traj(self, q1, q2))
self.base_roadmap = MultiBiRRT(self.extract_base(self.initial_config),
q_distance_fn(self.robot), q_sample_fn(self.robot), q_extend_fn(self.robot), q_collision_fn(self.env, self.robot))
self.pick_and_places = defaultdict(list)
self.pushes = defaultdict(Graph)
def reset(self):
self.setup_caches()
self.setup_primatives()
#################################################################
def get_counter_objects(self): # TODO - replace objecets with this
return self.objects
def get_floor_objects(self):
return self.floor_objects
def get_objects(self):
return self.objects + self.floor_objects
def get_static_bodies(self): # NOTE - really just means not robot
return self.objects() + self.obstacles
def get_counters(self):
return self.tables + self.sinks + self.stoves
def get_floors(self):
return self.floors
def get_surfaces(self):
return self.get_counters() + self.get_floors()
#################################################################
def is_active(self, body_name):
return body_name in self.active
def set_active(self, body_name, active):
if active and not self.is_active(body_name):
self.env.Add(self.bodies[body_name])
self.active.add(body_name)
if not active and self.is_active(body_name):
self.env.Remove(self.bodies[body_name])
self.active.remove(body_name)
def set_active_state(self, active_robot=True, active_obstacles=True, active_objects=set()):
self.set_active(get_name(self.robot), active_robot)
for obst_name in self.obstacles:
self.set_active(obst_name, active_obstacles)
for obj_name in self.get_objects():
self.set_active(obj_name, obj_name in active_objects)
def get_active_objects(self):
return [object_name for object_name in self.objects if self.is_active(object_name)]
#################################################################
def compute_part_aabb(self, part_name):
return aabb_from_points(trans_transform_points(get_trans(self.robot_links[self.robot_parts[part_name][0]]),
mesh_vertices(self.robot_convex_meshes[part_name])))
#################################################################
def get_body(self, body_name):
return self.bodies[body_name]
def get_region(self, region_name):
return self.regions[region_name]
def get_geom_hash(self, body_name):
return self.body_name_to_geom_hash[body_name]
def get_body_name(self, geometry_hash):
return self.geom_hash_to_body_name[geometry_hash]
def get_aabb(self, body_name, trans=None):
return aabb_from_oobb(self.get_oobb(body_name, trans))
#if trans is None: return self.aabbs[body_name]
#return aabb_apply_trans(self.aabbs[body_name], trans)
def get_oobb(self, body_name, trans=None):
if trans is None: return self.oobbs[body_name]
return OOBB(self.oobbs[body_name].aabb, np.dot(trans, self.oobbs[body_name].trans))
def get_mesh(self, body_name, trans=None):
if trans is None: return self.meshes[body_name]
return mesh_apply_trans(self.meshes[body_name], trans)
def get_vertices(self, body_name, trans=None):
if trans is None: return mesh_vertices(self.meshes[body_name])
return mesh_vertices(mesh_apply_trans(self.meshes[body_name], trans))
def get_radius2D2(self, body_name):
return self.radii2D[body_name]
def get_radius2(self, body_name):
return self.radii[body_name]
#################################################################
def sphere_collision(self, body_name1, point1, body_name2, point2):
return length2(point1 - point2) <= self.get_radius2(body_name1) + self.get_radius2(body_name2)
def cylinder_collision(self, body_name1, point1, body_name2, point2): # NOTE - does not account for out of plane rotations
return length2((point1 - point2)[:2]) <= self.get_radius2D2(body_name1) + self.get_radius2D2(body_name2)
def aabb_collision(self, body_name1, trans1, body_name2, trans2):
return aabb_overlap(self.get_aabb(body_name1, trans1), self.get_aabb(body_name2, trans2))
#################################################################
def get_robot_config(self):
return Config(get_full_config(self.robot))
def set_robot_config(self, config):
set_full_config(self.robot, config.value)
def get_pose(self, body_name):
if not self.is_active(body_name): return None
return Pose(get_pose(self.bodies[body_name]))
def set_pose(self, body_name, object_pose=None):
if object_pose is not None:
self.set_active(body_name, True)
set_pose(self.bodies[body_name], object_pose.value)
else:
self.set_active(body_name, False)
def set_poses(self, poses):
for body_name, pose in poses:
self.set_pose(body_name, poses)
def set_all_object_poses(self, object_poses={}):
for object_name in self.get_objects():
self.set_pose(object_name, object_pose=object_poses.get(object_name, None))
#################################################################
def lock(self):
if not self.locked and is_viewer_active(self.env):
self.env.Lock()
self.locked = True
def unlock(self):
if self.locked and is_viewer_active(self.env):
self.env.Unlock()
self.locked = False
def body_saver(self, body_name):
return body_saver(self.bodies[body_name])
def robot_saver(self):
return robot_saver(self.robot)
def state_saver(self):
return ManipulationOracleStateSaver(self)
#################################################################
def grow_base_roadmap(self, q):
base_q = self.extract_base(q)
if base_q in self.base_roadmap: return True
with self.robot:
try:
CSpace.robot_base(self.robot).set_active()
set_robot_config(self.robot, self.initial_config)
with collision_saver(self.env, openravepy_int.CollisionOptions.ActiveDOFs):
return self.base_roadmap.grow(base_q) is not None
except openrave_exception: # TODO - extremely infrequently, (int)values0.size() != GetActiveDOF()?
return False
#return self.base_roadmap.grow(q) is not None
def plan_base_roadmap(self, q1, q2):
if not COMPUTE_BASE_TRAJECTORY: return tuple()
base_q1, base_q2 = self.extract_base(q1), self.extract_base(q2)
cspace = CSpace.robot_base(self.robot)
if base_q1 in self.base_roadmap and base_q2 in self.base_roadmap:
path = self.base_roadmap(base_q1, base_q2)
else:
with self.robot:
cspace.set_active()
set_robot_config(self.robot, self.initial_config)
with collision_saver(self.env, openravepy_int.CollisionOptions.ActiveDOFs):
path = self.base_roadmap(base_q1, base_q2)
if path is None: return None
return (PathTrajectory(cspace, [q.value for q in path]),)
#return self.base_roadmap(q1, q2)
#return (Trajectory.join(self.base_roadmap(q1, q2)),)
#################################################################
def get_paps(self, body_name):
return self.pick_and_places[self.get_geom_hash(body_name)]
def add_pap(self, body_name, pap):
self.pick_and_places[self.get_geom_hash(body_name)].append(pap)
#################################################################
def draw_regions(self):
for region in self.regions.values():
region.draw(self.env)
self.draw_goals()
def clear_regions(self):
for region in self.regions.values():
region.clear()
def draw_goals(self):
self.goal_handles = []
for object_name, pose in self.problem.goal_poses.items():
self.goal_handles.append(draw_point(self.env, point_from_pose(pose.value), get_color(self.get_body(object_name))))
for object_name, (surface, point) in self.problem.goal_constrained.items():
self.goal_handles.append(draw_point(self.env, point.value, get_color(self.get_body(object_name))))
for region in self.goal_regions:
self.get_region(region).draw(self.env)
def clear_goals(self):
self.goal_handles = []
for region in self.goal_regions:
self.get_region(region).clear()
def __str__(self): # TODO - only print lists if not empty
return '%s\n' \
'Collision checker: %s\n' \
'Robot: %s\n' \
'Counter objects: %s\n' \
'Floor objects: %s\n' \
'Obstacles: %s\n' \
'Tables: %s\n' \
'Sinks: %s\n' \
'Stoves: %s\n' \
'Goal regions: %s'%(self.__class__.__name__, self.env.GetCollisionChecker(), get_name(self.robot), self.objects,
self.floor_objects, self.obstacles, self.tables, self.sinks, self.stoves, self.goal_regions)
__repr__ = __str__
