def visualize_path(path): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') dim_path = len(path[0]) if dim_path == 3: robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) elif dim_path == 11: manip = robot.GetManipulator('rightarm_torso') robot.SetActiveDOFs(manip.GetArmIndices(), DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) else: assert path[0].shape[0] == robot.GetActiveDOF(), 'robot and path should have same dof' env = robot.GetEnv() if len(path) > 1000: path_reduced = path[0:len(path) - 1:int(len(path) * 0.1)] else: path_reduced = path for idx, conf in enumerate(path_reduced): is_goal_config = idx == len(path_reduced) - 1 if is_goal_config: draw_robot_at_conf(conf, 0.5, 'path' + str(idx), robot, env) else: draw_robot_at_conf(conf, 0.9, 'path' + str(idx), robot, env) raw_input("Continue?") remove_drawn_configs('path', env)
def remove_drawn_configs(name, env=None): if env is None: assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironments()[0] for body in env.GetBodies(): if body.GetName().find(name) != -1: env.Remove(body)
def one_arm_place_object(place_action): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') manip = robot.GetManipulator('rightarm_torso') robot.SetActiveDOFs(manip.GetArmIndices(), DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) place_config = place_action['q_goal'] set_active_config(place_config) release_obj() open_gripper(robot)
def visualize_placements(placements, obj=None): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironments()[0] if obj is None: obj = env.GetRobots()[0].GetGrabbed()[0] if type(obj) == unicode or type(obj) == str: obj = env.GetKinBody(obj) for idx, conf in enumerate(placements): draw_obj_at_conf(conf, 0.7, 'place' + str(idx), obj, env) raw_input("Continue?") remove_drawn_configs('place', env)
def fold_arms(): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') leftarm_manip = robot.GetManipulator('leftarm') rightarm_manip = robot.GetManipulator('rightarm') FOLDED_LEFT_ARM = [0.0, 1.29023451, 0.0, -2.12, 0.0, -0.69800004, 0.0] set_config(robot, FOLDED_LEFT_ARM, leftarm_manip.GetArmIndices()) set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM), rightarm_manip.GetArmIndices())
def set_body_transparency(body, transparency): env = openravepy.RaveGetEnvironments()[0] if type(body) == unicode or type(body) == str: body = env.GetKinBody(body) for link in body.GetLinks(): for geom in link.GetGeometries(): geom.SetTransparency(transparency)
def set_robot_config(base_pose, robot=None): if robot is None: env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') base_pose = np.array(base_pose) base_pose = clean_pose_data(base_pose.astype('float')) robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) base_pose = np.array(base_pose).squeeze() """ while base_pose[-1] < 0: try: factor = -int(base_pose[-1] /(2*np.pi)) except: import pdb;pdb.set_trace() if factor == 0: factor = 1 base_pose[-1] += factor*2*np.pi while base_pose[-1] > 2*np.pi: factor = int(base_pose[-1] /(2*np.pi)) base_pose[-1] -= factor*2*np.pi if base_pose[-1] < if base_pose[-1] > 1.01: base_pose[-1] = 1.01 elif base_pose[-1] < 0.99: base_pose[-1] = 0.99 """ # print base_pose robot.SetActiveDOFValues(base_pose)
def get_body_xytheta(body): if not isinstance(body, openravepy.KinBody): env = openravepy.RaveGetEnvironments()[0] body = env.GetKinBody(body) Tbefore = body.GetTransform() body_quat = get_quat(body) th1 = np.arccos(body_quat[0]) * 2 th2 = np.arccos(-body_quat[0]) * 2 th3 = -np.arccos(body_quat[0]) * 2 quat_th1 = quat_from_angle_vector(th1, np.array([0, 0, 1])) quat_th2 = quat_from_angle_vector(th2, np.array([0, 0, 1])) quat_th3 = quat_from_angle_vector(th3, np.array([0, 0, 1])) if np.all(np.isclose(body_quat, quat_th1)): th = th1 elif np.all(np.isclose(body_quat, quat_th2)): th = th2 elif np.all(np.isclose(body_quat, quat_th3)): th = th3 else: print "This should not happen. Check if object is not standing still" import pdb; pdb.set_trace() if th < 0: th += 2 * np.pi assert (0 <= th < 2 * np.pi) # set the quaternion using the one found set_quat(body, quat_from_angle_vector(th, np.array([0, 0, 1]))) Tafter = body.GetTransform() assert (np.all(np.isclose(Tbefore, Tafter))) body_xytheta = np.hstack([get_point(body)[0:2], th]) body_xytheta = body_xytheta[None, :] clean_pose_data(body_xytheta) return body_xytheta
def randomly_place_region(body, region, n_limit=None): env = openravepy.RaveGetEnvironments()[0] if env.GetKinBody(get_name(body)) is None: env.Add(body) orig = get_body_xytheta(body) # for _ in n_limit: i = 0 while True: set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point( body, np.array([uniform(*range) for range in cspace] + [region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(env, body): return if n_limit is not None: i += 1 if i >= n_limit: set_obj_xytheta(orig, body) return
def two_arm_place_object(place_action): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') try: place_base_pose = place_action['q_goal'] except KeyError: place_base_pose = place_action['base_pose'] leftarm_manip = robot.GetManipulator('leftarm') rightarm_manip = robot.GetManipulator('rightarm') set_robot_config(place_base_pose, robot) release_obj() set_config(robot, FOLDED_LEFT_ARM, leftarm_manip.GetArmIndices()) set_config(robot, mirror_arm_config(FOLDED_LEFT_ARM), rightarm_manip.GetArmIndices())
def convert_to_kin_body(obj): env = openravepy.RaveGetEnvironments()[0] if isinstance(obj, openravepy.KinBody): obj = obj else: obj = env.GetKinBody(obj) return obj
def make_actions_executable(self): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironment(1) for a in self.A: if a.type == 'two_arm_pick' and type( a.discrete_parameters['object']) == str: a.discrete_parameters['object'] = env.GetKinBody( a.discrete_parameters['object'])
def get_color_of(body): env = openravepy.RaveGetEnvironments()[0] if type(body) == unicode or type(body) == str: obj = env.GetKinBody(body) else: obj = body return get_color(obj)
def get_global_pose_from_relative_pose_to_body(body, rel_robot_pose): if not isinstance(body, openravepy.KinBody): env = openravepy.RaveGetEnvironments()[0] body = env.GetKinBody(body) t_body = body.GetTransform() t_pose = get_transform_from_pose(rel_robot_pose, 'robot') t_pose_global = get_xytheta_from_transform(np.dot(t_body, t_pose)) return t_pose_global
def visualize_values_in_two_arm_domains(self, entity_values, entity_names): is_place_node = entity_names[0].find('region') != -1 if is_place_node: return max_val = np.max(entity_values) min_val = np.min(entity_values) for entity_name, entity_value in zip(entity_names, entity_values): entity = openravepy.RaveGetEnvironments()[0].GetKinBody(entity_name) set_color(entity, [0, (entity_value - min_val) / (max_val - min_val), 0])
def compute_occ_vec(key_configs): env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') occ_vec = [] with robot: for config in key_configs: set_robot_config(config, robot) collision = env.CheckCollision(robot) * 1 occ_vec.append(collision) return np.array(occ_vec)
def set_obj_xytheta(xytheta, obj): if isinstance(xytheta, list) or isinstance(xytheta, tuple): xytheta = np.array(xytheta) env = openravepy.RaveGetEnvironments()[0] if type(obj) == unicode or type(obj) == str: obj = env.GetKinBody(obj) xytheta = xytheta.squeeze() set_quat(obj, quat_from_angle_vector(xytheta[-1], np.array([0, 0, 1]))) set_xy(obj, xytheta[0], xytheta[1])
def set_rightarm_torso(config, robot=None): if robot is None: env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') manip = robot.GetManipulator('rightarm_torso') robot.SetActiveDOFs(manip.GetArmIndices(), DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) set_active_config(config, robot)
def visualize_pick_and_place(pick, place): env = openravepy.RaveGetEnvironments()[0] obj = pick.discrete_parameters['object'] if type(obj) == unicode or type(object) == str: obj = env.GetKinBody(obj) saver = CustomStateSaver(env) visualize_path(pick.low_level_motion) two_arm_pick_object(obj, pick.continuous_parameters) visualize_path(place.low_level_motion) saver.Restore()
def two_arm_pick_object(obj, pick_action): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') if type(obj) == str or type(obj) == unicode: obj = robot.GetEnv().GetKinBody(obj) try: base_pose = pick_action['q_goal'] except KeyError: base_pose = pick_action['base_pose'] set_robot_config(base_pose, robot) if 'g_config' in pick_action.keys(): g_config = pick_action['g_config'] leftarm_manip = robot.GetManipulator('leftarm') rightarm_torso_manip = robot.GetManipulator('rightarm_torso') set_config(robot, g_config[0], leftarm_manip.GetArmIndices()) set_config(robot, g_config[1], rightarm_torso_manip.GetArmIndices()) grab_obj(obj)
def get_robot_xytheta(robot=None): if robot is None: env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') with robot: robot.SetActiveDOFs([], DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) robot_xytheta = robot.GetActiveDOFValues() robot_xytheta = robot_xytheta[None, :] clean_pose_data(robot_xytheta) return robot_xytheta
def randomly_place_region(body, region): env = openravepy.RaveGetEnvironments()[0] if env.GetKinBody(get_name(body)) is None: env.Add(body) while True: set_quat(body, quat_from_z_rot(uniform(0, 2 * PI))) aabb = aabb_from_body(body) cspace = region.cspace(aabb) if cspace is None: continue set_point(body, np.array([uniform(*range) for range in cspace] + [ region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body)) if not body_collision(env, body): return
def get_pick_base_pose_and_grasp_from_pick_parameters(obj, pick_parameters, obj_xyth=None): if not isinstance(obj, openravepy.KinBody): env = openravepy.RaveGetEnvironments()[0] obj = env.GetKinBody(obj) assert len(pick_parameters) == 6 grasp_params = pick_parameters[0:3] ir_params = pick_parameters[3:] pick_base_pose = get_absolute_pick_base_pose_from_ir_parameters( ir_params, obj, obj_xyth) return grasp_params, pick_base_pose
def execute_pick(self): env = openravepy.RaveGetEnvironments()[0] if isinstance(self.discrete_parameters['object'], openravepy.KinBody): obj_to_pick = self.discrete_parameters['object'] else: obj_to_pick = env.GetKinBody(self.discrete_parameters['object']) if self.type == 'two_arm_pick_two_arm_place': two_arm_pick_object(obj_to_pick, self.continuous_parameters['pick']) else: one_arm_pick_object(obj_to_pick, self.continuous_parameters['pick'])
def get_rightarm_torso_config(robot=None): if robot is None: env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') with robot: manip = robot.GetManipulator('rightarm_torso') robot.SetActiveDOFs(manip.GetArmIndices(), DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis, [0, 0, 1]) robot_config = robot.GetActiveDOFValues() robot_config = robot_config[None, :] clean_rightarm_torso_base_pose(robot_config[0, :]) return robot_config
def Restore(self): assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironment(self.env_id) robot = env.GetRobot(self.robot_name) currently_holding = len(robot.GetGrabbed()) > 0 if currently_holding: held_obj = robot.GetGrabbed()[0] release_obj() for obj_name, obj_pose in zip(self.object_poses.keys(), self.object_poses.values()): set_obj_xyztheta(obj_pose, env.GetKinBody(obj_name)) set_robot_config(self.robot_base_pose, robot) robot.SetDOFValues(self.robot_dof_values) if self.is_holding: held_obj = env.GetKinBody(self.held_object) grab_obj(held_obj)
def execute(self): env = openravepy.RaveGetEnvironments()[0] if self.type == 'two_arm_pick': obj_to_pick = utils.convert_to_kin_body( self.discrete_parameters['object']) if 'q_goal' in self.continuous_parameters and type(self.continuous_parameters['q_goal']) == list and\ len(self.continuous_parameters['q_goal']) > 1: try: two_arm_pick_object( obj_to_pick, {'q_goal': self.continuous_parameters['q_goal'][0]}) except: import pdb pdb.set_trace() else: two_arm_pick_object(obj_to_pick, self.continuous_parameters) elif self.type == 'two_arm_place': two_arm_place_object(self.continuous_parameters) elif self.type == 'two_arm_pick_two_arm_place': obj_to_pick = utils.convert_to_kin_body( self.discrete_parameters['object']) two_arm_pick_object(obj_to_pick, self.continuous_parameters['pick']) two_arm_place_object(self.continuous_parameters['place']) elif self.type == 'one_arm_pick': obj_to_pick = utils.convert_to_kin_body( self.discrete_parameters['object']) one_arm_pick_object(obj_to_pick, self.continuous_parameters) elif self.type == 'one_arm_place': one_arm_place_object(self.continuous_parameters) elif self.type == 'one_arm_pick_one_arm_place': obj_to_pick = utils.convert_to_kin_body( self.discrete_parameters['object']) one_arm_pick_object(obj_to_pick, self.continuous_parameters['pick']) one_arm_place_object(self.continuous_parameters['place']) else: raise NotImplementedError
def Restore(self): if self.env_id is None: return assert len(openravepy.RaveGetEnvironments()) == 1 env = openravepy.RaveGetEnvironment(self.env_id) robot = env.GetRobot(self.robot_name) currently_holding = len(robot.GetGrabbed()) > 0 if currently_holding: held_obj = robot.GetGrabbed()[0] release_obj(robot, held_obj) for obj_name, obj_pose in zip(self.object_poses.keys(), self.object_poses.values()): try: set_obj_xytheta(obj_pose, env.GetKinBody(obj_name)) except: print "Object ", obj_name, 'does not exist in the environment' continue set_robot_config(self.robot_base_pose, robot) robot.SetDOFValues(self.robot_dof_values) if self.is_holding: held_obj = env.GetKinBody(self.held_object) grab_obj(robot, held_obj)
def get_body_with_name(obj_name): env = openravepy.RaveGetEnvironments()[0] return env.GetKinBody(obj_name)
def set_active_config(conf, robot=None): if robot is None: env = openravepy.RaveGetEnvironments()[0] robot = env.GetRobot('pr2') robot.SetActiveDOFValues(conf.squeeze())