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 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 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 main(): ### Parameters ### ENV_FILE = "../../trajopt/data/pr2_table.env.xml" MANIP_NAME = "rightarm" N_STEPS = 10 LINK_NAME = "r_gripper_tool_frame" INTERACTIVE = True #joints_start_end = np.array([ # [-0.95, -0.38918253, -2.43888696, -1.23400121, -0.87433154, -0.97616443, -2.10997203], # [-0.4, -0.4087081, -3.77121706, -1.2273375, 0.69885101, -0.8992004, 3.13313843] #]) #joints_start_end = np.array([[0.34066373, -0.49439586, -3.3 , -1.31059503 , -1.28229698, -0.15682819, -116.19626995], # [ 0.5162424 , -0.42037121 , -3.7 , -1.30277208 , 1.31120586, -0.16411924 ,-118.57637204]]) #joints_start_end = np.array([[ -1.83204054 , -0.33201855 , -1.01105089 , -1.43693186 , -1.099908, -2.00040616, -116.17133393], #[ -0.38176851 , 0.17886005 , -1.4 , -1.89752658 , -1.93285873, -1.60546868, -114.70809047]]) joints_start_end = np.array([[ 0.33487707, -0.50480484, -3.3, -1.33546928, -1.37194549, -0.14645853, -116.11672039 ], [ 4.71340480e-01, -4.56593341e-01, -3.60000000e+00, -1.33176173e+00, 1.21859723e+00, -9.98780266e-02, -1.18561732e+02 ]]) ################## joints_start_end[:, 2] = np.unwrap(joints_start_end[:, 2]) joints_start_end[:, 4] = np.unwrap(joints_start_end[:, 4]) joints_start_end[:, 6] = np.unwrap(joints_start_end[:, 6]) joints_start = joints_start_end[0, :] joints_end = joints_start_end[1, :] ### Env setup #### env = rave.RaveGetEnvironment(1) if env is None: env = rave.Environment() env.StopSimulation() atexit.register(rave.RaveDestroy) env.Load(ENV_FILE) robot = env.GetRobots()[0] manip = robot.GetManipulator(MANIP_NAME) robot.SetDOFValues(joints_start, manip.GetArmIndices()) ################## result_traj = move_arm_straight(env, manip, N_STEPS, LINK_NAME, joints_start, joints_end, interactive=INTERACTIVE) print 'Showing original straight-line trajectory' env.SetViewer('qtcoin') env.UpdatePublishedBodies() import time time.sleep(2) play_traj(mu.linspace2d(joints_start, joints_end, N_STEPS), env, manip) raw_input('press enter to continue') print 'Showing optimized trajectory' play_traj(result_traj, env, manip) raw_input('press enter to continue')
return request if __name__ == "__main__": ### Parameters ### ENV_FILE = "wamtest1.env.xml" MANIP_NAME = "arm" LINK_NAME = "wam7" #INTERACTIVE = True ################## ### Env setup #### env = rave.RaveGetEnvironment(1) if env is None: env = rave.Environment() env.StopSimulation() env.Load(ENV_FILE) robot = env.GetRobots()[0] manip = robot.GetManipulator(MANIP_NAME) ################## T_w_mug = env.GetKinBody("mug-table-cluttered").GetLinks()[0].GetTransform() xyz_targ = (T_w_mug[:3,3] + np.array([0,0,.15])).tolist() quat_targ = np.array([0,1,0,0]).tolist() request = move_arm_to_grasp(xyz_targ, quat_targ, LINK_NAME, MANIP_NAME) s = json.dumps(request) print "REQUEST:",s #trajoptpy.SetInteractive(False);
def __init__(self, envid, fixed_frame='base_link', rate=10., logger=TextColors()): """ C{RaveStateUpdater} constructor. It uses a C{tf.TransformListener} instance to query TF. By default, after creating it will start updating the environment at the given rate. Every time it updates, check for new frame ids in TF. The body names and the TF frame ids must have similarities and when possible should be the same. We check C{frame_id in body_name} and C{body_namein frame_id} this way if C{frame_id='left/base_link'} and C{body_name='denso_left'}, we will split the C{frame_id} into C{['left','base_link']} and will be able to match them. @type envid: int @param envid: Id of the OpenRAVE environment to be updated @type fixed_frame: string @param fixed_frame: Name of the TF frame to be considered as the fixed frame.It is also used to locate the corresponding OpenRAVE body name. @type rate: float @param rate: Rate at which we will query TF for new transforms. @type logger: Object @param logger: Logger instance. When used in ROS, the recommended C{logger=rospy}. """ self.env = orpy.RaveGetEnvironment(envid) try: self.listener = tf.TransformListener() except rospy.ROSInitException: logger.logerr( 'time is not initialized. Have you called rospy.init_node()?') return # TODO: Wait until we hear something from TF rospy.sleep(5.0) # Subscribe to the joint_states topics that match a robot in OpenRAVE topics = rosgraph.Master('/rostopic').getPublishedTopics('/') self.js_topics = [] self.js_robots = [] for topic_info in topics: topic_name = topic_info[0] msg_class = roslib.message.get_message_class(topic_info[1]) if ('joint_states' in topic_name) and (msg_class is JointState): namespace = topic_name.replace('/', '').replace('joint_states', '') robot = self._find_rave_body(namespace) if robot is not None: self.js_topics.append(topic_name) self.js_robots.append(robot.GetName()) self.js_msgs = [None] * len(self.js_topics) for topic in self.js_topics: rospy.Subscriber(topic, JointState, self._cb_joint_states, callback_args=topic) # Look for the body that corresponds to the reference frame self.fixed_frame = self._get_tf_reference_frame(fixed_frame) if self.fixed_frame is None: logger.logerr( 'Failed to find an unique frame in the TF tree. Found frames: {0}' .format(self.listener.getFrameStrings())) return # Make the origin coincide with the reference frame body self.ref_body = self._find_rave_body(self.fixed_frame) if self.ref_body is None: body_names = [body.GetName() for body in self.env.GetBodies()] logger.logerr( 'Failed to find the reference body in OpenRAVE. Found bodies: {0}' .format(body_names)) return criros.raveutils.move_origin_to_body(self.ref_body) # Create a publisher that will report the affected OpenRAVE objects self.pub = rospy.Publisher('/openrave/state_updater', String, queue_size=3) # Start a thread to update all the objects found in TF and OpenRAVE self.rate = rate self.elapsed_time = 0.0 self.update_rave_environment() # Update env at least once self.timer = rospy.Timer(rospy.Duration(1.0 / rate), self.update_rave_environment) rospy.on_shutdown(self.stop)
def prm_connect(q1, q2, collision_checker, source='', return_start_set_and_path_idxs=False): global prm_vertices global prm_edges is_goal_region = False is_multiple_goals = False if type(q2) is AARegion: is_goal_region = True else: is_multiple_goals = isinstance(q2, list) # and len(q2[0]) == len(q1) is_single_goal = not is_goal_region and not is_multiple_goals if prm_vertices is None or prm_edges is None: prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb')) collision_checker_is_set = isinstance(collision_checker, set) no_collision_checking = collision_checker_is_set and len( collision_checker) == 0 ## Base case checks if not no_collision_checking: env = openravepy.RaveGetEnvironment(1) robot = env.GetRobot('pr2') non_prm_config_collision_checker = collision_fn(env, robot) if non_prm_config_collision_checker(q1): print "Collision at init config" if return_start_set_and_path_idxs: return None, None else: return None if is_single_goal and non_prm_config_collision_checker(q2): print "Collision at goal config" if return_start_set_and_path_idxs: return None, None else: return None if is_multiple_goals: q2_original = q2 q2 = [ q_goal for q_goal in q2_original if not non_prm_config_collision_checker(q_goal) ] if len(q2) == 0: if return_start_set_and_path_idxs: return None, None else: return None if is_single_goal and are_base_confs_close_enough( q1, q2, xy_threshold=0.8, th_threshold=50.): if return_start_set_and_path_idxs: return [q1, q2], None else: return [q1, q2] if is_multiple_goals: for q_goal in q2: if are_base_confs_close_enough(q1, q_goal, xy_threshold=0.8, th_threshold=50.): if return_start_set_and_path_idxs: return [q1, q_goal], None else: return [q1, q_goal] ### ## Defines a goal test function if is_multiple_goals: close_points = [] def is_connected_to_goal(prm_vertex_idx): q = prm_vertices[prm_vertex_idx] goals = q2 if len(q.squeeze()) != 3: raise NotImplementedError # if source == 'sampler': # diff = utils.base_conf_diff(q, q2[0]) # xydist = np.linalg.norm(diff[0:2]) # if xydist < 0.8: # close_points.append(q) return any( are_base_confs_close_enough( q, g, xy_threshold=0.8, th_threshold=360.) for g in goals) elif is_goal_region: def is_connected_to_goal(prm_vertex_idx): q = prm_vertices[prm_vertex_idx] goal_region = q2 return goal_region.contains_point(q) else: def is_connected_to_goal(prm_vertex_idx): q = prm_vertices[prm_vertex_idx] return are_base_confs_close_enough(q, q2, xy_threshold=0.8, th_threshold=52.) ##### def heuristic(q): return 0 def is_collision(q): col = q in collision_checker if collision_checker_is_set else non_prm_config_collision_checker( prm_vertices[q]) return col ### making a set of qinit idxs start = set() for idx, q in enumerate(prm_vertices): q_close_enough_to_q1 = are_base_confs_close_enough(q, q1, xy_threshold=0.8, th_threshold=360.) if q_close_enough_to_q1: if not is_collision(idx): start.add(idx) ##### # todo can I get this set start? It can optimize v_manip computation path_idxs = find_prm_path(start, [is_connected_to_goal], heuristic, is_collision, source)[0] # distances = [utils.base_pose_distance(q2[0], prm_vtx) for prm_vtx in prm_vertices] # if source == 'sampler': # import pdb;pdb.set_trace() if path_idxs is not None: path = [q1] + [prm_vertices[i] for i in path_idxs] if is_single_goal: path += [q2] elif is_multiple_goals: path += [get_goal_config_used(path, q2)] if return_start_set_and_path_idxs: return path, (start, path_idxs) else: return path else: if return_start_set_and_path_idxs: return None, (start, path_idxs) else: return None
def prm_connect(q1, q2, collision_checker): global prm_vertices global prm_edges is_goal_region = False is_multiple_goals = False if type(q2) is AARegion: is_goal_region = True else: is_multiple_goals = isinstance(q2, list) # and len(q2[0]) == len(q1) is_single_goal = not is_goal_region and not is_multiple_goals collision_checker_is_set = isinstance(collision_checker, set) if prm_vertices is None or prm_edges is None: prm_vertices, prm_edges = pickle.load(open('./prm.pkl', 'rb')) no_collision_checking = collision_checker_is_set and len( collision_checker) == 0 if not no_collision_checking: env = openravepy.RaveGetEnvironment(1) robot = env.GetRobot('pr2') non_prm_config_collision_checker = collision_fn(env, robot) if non_prm_config_collision_checker(q1): #print "initial config in collision" return None if is_single_goal and non_prm_config_collision_checker(q2): #print "goal config in collision" return None if is_multiple_goals: q2_original = q2 q2 = [ q_goal for q_goal in q2_original if not non_prm_config_collision_checker(q_goal) ] if len(q2) == 0: return None if is_single_goal and are_base_confs_close_enough( q1, q2, xy_threshold=0.8, th_threshold=50.): return [q1, q2] if is_multiple_goals: for q_goal in q2: if are_base_confs_close_enough(q1, q_goal, xy_threshold=0.8, th_threshold=50.): return [q1, q_goal] if is_multiple_goals: def is_connected_to_goal(prm_vertex_idx): q = prm_vertices[prm_vertex_idx] goals = q2 if len(q.squeeze()) != 3: raise NotImplementedError return any( are_base_confs_close_enough( q, g, xy_threshold=0.8, th_threshold=50.) for g in goals) elif is_goal_region: def is_connected_to_goal(prm_vertex_idx): q = prm_vertices[prm_vertex_idx] goal_region = q2 return goal_region.contains_point(q) else: def is_connected_to_goal(prm_vertex_idx): q = prm_vertices[prm_vertex_idx] return are_base_confs_close_enough(q, q2, xy_threshold=0.8, th_threshold=50.) def heuristic(q): return 0 def is_collision(q): return q in collision_checker if collision_checker_is_set else collision_checker( prm_vertices[q]) # start = {i for i, q in enumerate(prm_vertices) if np.linalg.norm((q - q1)[:2]) < .8} start = set() for idx, q in enumerate(prm_vertices): q_close_enough_to_q1 = are_base_confs_close_enough(q, q1, xy_threshold=0.8, th_threshold=50.) if q_close_enough_to_q1: if not is_collision(idx): start.add(idx) path = find_prm_path(start, [is_connected_to_goal], heuristic, is_collision)[0] if path is not None: path = [q1] + [prm_vertices[i] for i in path] if is_single_goal: path += [q2] elif is_multiple_goals: path += [get_goal_config_used(path, q2)] return path else: return None