def __init__(self, arm_name, sim=None, interact=False): """ :param arm_name: "left" or "right" :param sim: OpenRave simulator (or create if None) :param interact: enable trajopt viewer """ assert arm_name == 'left' or arm_name == 'right' self.sim = sim if self.sim is None: self.sim = simulator.Simulator() self.robot = self.sim.robot self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm wrist_flex_index = self.robot.GetJointIndex(arm_name[0] + '_wrist_flex_joint') lower, upper = self.robot.GetDOFLimits() lower[wrist_flex_index], upper[ wrist_flex_index] = -np.pi / 2., np.pi / 2. self.robot.SetDOFLimits(lower, upper) if interact: trajoptpy.SetInteractive(True) self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0]) self.joint_indices = self.manip.GetArmIndices()
def move_arm_straight(env, manip, n_steps, link_name, joints_start, joints_end, xyz_start=None, quat_start=None, xyz_end=None, quat_end=None, interactive=False): ''' Finds a trajectory (of length n_steps) such that the specified link travels as close as possible to a straight line in cartesian space, constrained to the specified starting and ending joint values. xyz_start, quat_start, xyz_end, quat_end are the starting and ending cartesian poses that correspond to joints_start and joints_end. Setting them to None will compute them here. WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible! Returns: trajectory (numpy.ndarray, n_steps x dof) ''' if xyz_start is None or quat_start is None: xyz_start, quat_start = joints2xyzquat(manip, link_name, joints_start) if xyz_end is None or quat_end is None: xyz_end, quat_end = joints2xyzquat(manip, link_name, joints_end) request = move_arm_straight_request(manip, n_steps, link_name, joints_start, joints_end, xyz_start, quat_start, xyz_end, quat_end) trajoptpy.SetInteractive(interactive) prob = trajoptpy.ConstructProblem(json.dumps(request), env) result = trajoptpy.OptimizeProblem(prob) return result.GetTraj()
def move_arm_cart(env, manip, link_name, xyzs, quats, init_soln, interactive=False): assert len(xyzs) == len(quats) == len(init_soln) request = move_arm_cart_request(manip, link_name, xyzs, quats, init_soln) trajoptpy.SetInteractive(interactive) prob = trajoptpy.ConstructProblem(json.dumps(request), env) result = trajoptpy.OptimizeProblem(prob) return result.GetTraj()
def main(): args = parse_input_args() setup_log_file(args) set_global_vars(args) print 'Setting Global Vars' trajoptpy.SetInteractive(args.interactive) lfd_env, sim = setup_lfd_environment_sim(args) reg_and_traj_transferer = setup_registration_and_trajectory_transferer( args, sim) if args.subparser_name == "eval": label_demos_parallel(args, reg_and_traj_transferer, lfd_env, sim) else: raise RuntimeError("Invalid subparser name")
def main(): args = parse_input_args() setup_log_file(args) sim_env = sim_util.SimulationEnv() set_global_vars(args, sim_env) trajoptpy.SetInteractive(args.interactive) load_simulation(args, sim_env) if args.subparser_name == "eval": eval_on_holdout(args, sim_env) elif args.subparser_name == "replay": replay_on_holdout(args, sim_env) else: raise RuntimeError("Invalid subparser name")
def init_env(problemset): env = openravepy.Environment() env.StopSimulation() robot2file = {"pr2": "robots/pr2-beta-static.zae"} if args.planner == "trajopt": if args.interactive: trajoptpy.SetInteractive(True) plan_func = trajopt_plan elif args.planner == "ompl": setup_ompl(env) plan_func = ompl_plan elif args.planner in ["chomp", "chomp2"]: setup_chomp(env) plan_func = chomp_plan # chomp needs a robot with spheres chomp_pr2_file = "pr2_with_spheres.robot.xml" if problemset[ "active_affine"] == 0 else "pr2_with_spheres_fullbody.robot.xml" robot2file["pr2"] = osp.join(pbc.envfile_dir, chomp_pr2_file) env.Load(osp.join(pbc.envfile_dir, problemset["env_file"])) env.Load(robot2file[problemset["robot_name"]]) robot = env.GetRobots()[0] if args.planner == "trajopt": try: postsetup_trajopt(env) except Exception: print "can't find ros config file. skipping self collision ignore" robot.SetTransform( openravepy.matrixFromPose(problemset["default_base_pose"])) rave_joint_names = [joint.GetName() for joint in robot.GetJoints()] rave_inds, rave_values = [], [] for (name, val) in zip(problemset["joint_names"], problemset["default_joint_values"]): if name in rave_joint_names: i = rave_joint_names.index(name) rave_inds.append(i) rave_values.append(val) robot.SetDOFValues(rave_values, rave_inds) active_joint_inds = [ rave_joint_names.index(name) for name in problemset["active_joints"] ] robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"]) return env, robot, plan_func
def do_traj_ik_graph_search_opt(pr2, lr, gripper_poses): gripper_poses = [pose for i, pose in enumerate(gripper_poses) if i % 20 == 0] graph_search_res = do_traj_ik_graph_search(pr2, lr, gripper_poses) hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses] poses = rave.poseFromMatrices(hmats) quats = poses[:,:4] xyzs = poses[:,4:7] manip_name = {"r":"rightarm", "l":"leftarm"}[lr] request = get_ik_request(manip_name, "%s_gripper_tool_frame"%lr, xyzs[-1], quats[-1], graph_search_res, xyzs, quats) s = json.dumps(request) print "REQUEST:", s pr2.robot.SetDOFValues(graph_search_res[0], pr2.robot.GetManipulator(manip_name).GetArmIndices()) trajoptpy.SetInteractive(True); prob = trajoptpy.ConstructProblem(s, pr2.env) result = trajoptpy.OptimizeProblem(prob)
def main(): demofile = h5py.File(args.demos_h5file, 'r') trajoptpy.SetInteractive(False) Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] Globals.robot.SetDOFValues(PR2_L_POSTURES["side"], Globals.robot.GetManipulator("leftarm").GetArmIndices()) Globals.robot.SetDOFValues(mirror_arm_joints(PR2_L_POSTURES["side"]), Globals.robot.GetManipulator("rightarm").GetArmIndices()) init_rope_xyz, _ = load_random_start_segment(demofile) init_rope_xyz_robot = apply_tfm(init_rope_xyz[()], args.inittfmfile) table_height = init_rope_xyz_robot[:,2].mean() - .03 table_xml = make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01]) Globals.env.LoadData(table_xml) Globals.sim = ropesim.Simulation(Globals.env, Globals.robot) Globals.viewer = trajoptpy.GetViewer(Globals.env) Globals.viewer.Idle() if not args.same_as_training: for i in range(0, args.dataset_size): print "State ", i, " of ", args.dataset_size rope_nodes, demo_id = sample_rope_state(demofile) save_example_action(rope_nodes, demo_id) else: start_keys = [k for k in demofile.keys() if k.startswith('demo') and k.endswith('00')] for demo_id in start_keys: xyz_camera = demofile[demo_id]['cloud_xyz'][()] xyz_robot = apply_tfm(xyz_camera, args.inittfmfile) rope_nodes = rope_initialization.find_path_through_point_cloud( xyz_robot, perturb_peak_dist = None, num_perturb_points = 0) replace_rope(rope_nodes) Globals.sim.settle() Globals.viewer.Step() new_xyz_robot = Globals.sim.observe_cloud() save_example_action(new_xyz_robot, demo_id) time.sleep(1)
def __init__(self, arm_name, sim=None, interact=False): """ :param arm_name: "left" or "right" :param sim: OpenRave simulator (or create if None) :param interact: enable trajopt viewer """ assert arm_name == 'left' or arm_name == 'right' self.sim = sim if self.sim is None: self.sim = simulator.Simulator() self.robot = self.sim.robot self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm if interact: trajoptpy.SetInteractive(True) self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0]) self.joint_indices = self.manip.GetArmIndices()
def main(): args = parse_input_args() if args.subparser_name == "eval": eval_util.save_results_args(args.resultfile, args) elif args.subparser_name == "replay": loaded_args = eval_util.load_results_args(args.replay.loadresultfile) assert 'eval' not in vars(args) args.eval = loaded_args.eval else: raise RuntimeError("Invalid subparser name") setup_log_file(args) set_global_vars(args) trajoptpy.SetInteractive(args.interactive) lfd_env, sim = setup_lfd_environment_sim(args) reg_and_traj_transferer = setup_registration_and_trajectory_transferer(args, sim) if args.eval.action_selection == 'feature': get_features(args) if args.eval.action_selection == 'greedy': action_selection = GreedyActionSelection(reg_and_traj_transferer.registration_factory) else: action_selection = FeatureActionSelection(reg_and_traj_transferer.registration_factory, GlobalVars.features, GlobalVars.actions, GlobalVars.demos, simulator=reg_and_traj_transferer, lfd_env=lfd_env, width=args.eval.width, depth=args.eval.depth) if args.subparser_name == "eval": start = time.time() if args.eval.parallel: eval_on_holdout_parallel(args, action_selection, reg_and_traj_transferer, lfd_env, sim) else: eval_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim) print "eval time is:\t{}".format(time.time() - start) elif args.subparser_name == "replay": replay_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim) else: raise RuntimeError("Invalid subparser name")
def main(): parser = argparse.ArgumentParser() parser.add_argument("--lr", default="r") parser.add_argument("--use_cost", action="store_true") parser.add_argument("--interactive", action="store_true") parser.add_argument("--no_prompt", action="store_true") args = parser.parse_args() trajoptpy.SetInteractive( args.interactive ) # pause every iteration, until you press 'p'. Press escape to disable further plotting rospy.init_node("pr2_arm") with open('config/environment/pr2.yaml') as yaml_string: pr2_env = utils.from_yaml(yaml_string) if args.lr == 'r': min_target_pos = np.array([.5, -.5, .8]) max_target_pos = np.array([.7, .2, 1]) else: min_target_pos = np.array([.5, -.2, .8]) max_target_pos = np.array([.7, .5, 1]) while not rospy.is_shutdown(): if args.no_prompt or yes_or_no.yes_or_no("execute?"): target_pos = utils.sample_interval(min_target_pos, max_target_pos).tolist() pr2_env.pr2.update_rave() traj = planning.plan_up_trajectory(pr2_env.pr2.robot, args.lr, target_pos, use_cost=args.use_cost) bodypart2traj = {"%sarm" % args.lr: traj} pr2_trajectories.follow_body_traj(pr2_env.pr2, bodypart2traj, speed_factor=0.1)
robot = env.GetRobots()[0] init_transform = np.eye(4) init_transform[:3,3] = [-.35, 1, .92712] init_transform[:3,3] = [.1, 1, .92712] #init_transform[:3,3] = [2.6, 1, .92712] robot.SetTransform(init_transform) robot.SetDOFValues(np.zeros(robot.GetDOF())) robot.SetActiveDOFs(np.arange(robot.GetDOF()), rave.DOFAffine.Transform) # move arms to side robot.SetDOFValues([-1.3],[robot.GetJoint("l_arm_shx").GetDOFIndex()]) robot.SetDOFValues([1.3],[robot.GetJoint("r_arm_shx").GetDOFIndex()]) standing_posture = robot.GetActiveDOFValues() ################## trajoptpy.SetInteractive(True) cc = trajoptpy.GetCollisionChecker(env) cc.ExcludeCollisionPair(robot.GetLink("l_foot"), env.GetKinBody("ProjectRoom").GetLink("Floor")) cc.ExcludeCollisionPair(robot.GetLink("r_foot"), env.GetKinBody("ProjectRoom").GetLink("Floor")) n_steps = 5 x_button_press = 2.55 # if robot is at this x-coordinate, he can reach the button xyz_button = env.GetKinBody("bigredbutton").GetTransform()[:3,3] xyz_button[2] += .15; totaltraj = [] i=0
import argparse parser = argparse.ArgumentParser() parser.add_argument("--interactive", action="store_true") args = parser.parse_args() rospack = rospkg.RosPack() work_path = os.path.join( os.path.join(rospack.get_path('irl_constraints_learning')), 'src/irl_constraints_learning/path_planning') env = openravepy.Environment() env.StopSimulation() env.Load(os.path.join(work_path, "robots/baxter_structure.xml")) trajoptpy.SetInteractive( args.interactive ) # pause every iteration, until you press 'p'. Press escape to disable further plotting #env.SetViewer('qtcoin') # start the viewer (conflicts with matplotlib) robot = env.GetRobots()[0] # get the first robot ## manip = robot.GetManipulators()[0] # left_arm print robot.GetManipulators('left_arm')[0].GetArmIndices() joint_start = [0, 0, 0, 0, 0, 0, 0] robot.SetDOFValues(joint_start, robot.GetManipulators('left_arm')[0].GetArmIndices()) joint_target = [1.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5] request = { "basic_info": {
def plan(env_xml, manip_name, start, goal, traj=None, c_fn=None, eq_fn=None, ineq_fn=None, n_steps=100, lower_limit=None, upper_limit=None): env = openravepy.Environment() env.StopSimulation() env.Load(env_xml) trajoptpy.SetInteractive(False) robot = env.GetRobots()[0] # get the first robot robot.SetDOFValues(start, robot.GetManipulators(manip_name)[0].GetArmIndices()) ## if lower_limit is not None and upper_limit is not None: ## jnts = robot.GetJoints(robot.GetManipulators(limb+'_arm')[0].GetArmIndices()) ## for i, jnt in enumerate(jnts): ## ## from IPython import embed; embed(); sys.exit() ## jnt.SetLimits(lower_limit[i:i+1], upper_limit[i:i+1]) if type(goal) is not list: goal = list(goal) if traj is None: init_type = 'straight_line' traj = goal else: init_type = 'given_traj' n_steps = len(traj) if type(traj) is not list: traj = traj.tolist() # cost and constraints request = { "basic_info": { "n_steps": n_steps, "manip": manip_name, "start_fixed": True # i.e., DOF values at first timestep are fixed based on current robot state }, "costs": [ { "type": "joint_vel", # joint-space velocity cost "params": { "coeffs": [1.] } # a list of length one is automatically expanded to a list of length n_dofs }, ], "constraints": [ { "type": "joint", # joint-space target "params": { "vals": goal } # length of vals = # dofs of manip }, ], "init_info": { "type": init_type, "data": traj, "endpoint": goal } } # set qp s = json.dumps(request) # convert dictionary into json-formatted string prob = trajoptpy.ConstructProblem( s, env) # create object that stores optimization problem n_dof = len(start) # Set cost and constraints if c_fn is not None: for t in xrange(1, n_steps): prob.AddErrorCost(c_fn, [(t, j) for j in xrange(n_dof)], "ABS", "up%i" % t) ## # EQ: an equality constraint. `f(x) == 0` in a valid solution. ## if eq_fn is not None: ## for t in xrange(1,n_steps): ## prob.AddConstraint(eq_fn, [(t,j) for j in xrange(n_dof)], "EQ", "up%i"%t) # INEQ: an inequality constraint. `f(x) <= `0` in a valid solution. if ineq_fn is not None: for t in xrange(3, n_steps): if type(ineq_fn) is list: for i in range(len(ineq_fn)): prob.AddConstraint(ineq_fn[i], [(t, j) for j in xrange(n_dof)], "INEQ", "up%i" % t) else: prob.AddConstraint(ineq_fn, [(t, j) for j in xrange(n_dof)], "INEQ", "up%i" % t) t_start = time.time() result = trajoptpy.OptimizeProblem(prob) # do optimization t_elapsed = time.time() - t_start print "optimization took %.3f seconds" % t_elapsed ## from trajoptpy.check_traj import traj_is_safe ## prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem ## assert traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free ## if result.GetTraj() is None or len(result.GetTraj())==0: ## from IPython import embed; embed() #; sys.exit() ## for cost in result.GetCosts(): ## if cost[0].find('joint_vel')<0 and cost[1]>1.0: ## print "Optimization failed with high cost value {}".format(cost) ## return [] ## for cstr in result.GetConstraints(): ## if cstr[1]>0.001: ## print "Optimization failed with high constraint value {}".format(cstr) ## return [] return result.GetTraj()
def _Plan(self, robot, robot_checker, request, traj_constraints=(), goal_constraints=(), traj_costs=(), goal_costs=(), interactive=False, constraint_threshold=1e-4, sampling_func=VanDerCorputSampleGenerator, norm_order=2, **kwargs): """ Plan to a desired configuration with Trajopt. This function invokes the Trajopt planner directly on the specified JSON request. This can be used to implement custom path optimization algorithms. Constraints and costs are specified as dicts of: ``` { 'f': [float] -> [float], 'dfdx': [float] -> [float], 'type': ConstraintType or CostType 'dofs': [int] } ``` The input to f(x) and dfdx(x) is a vector of active DOF values used in the planning problem. The output is a vector of costs, where the value *increases* as a constraint or a cost function is violated or unsatisfied. See ConstraintType and CostType for descriptions of the various function specifications and their expected behavior. The `dofs` parameter can be used to specify a subset of the robot's DOF indices that should be used. A ValueError is thrown if these indices are not entirely contained in the current active DOFs of the robot. @param robot: the robot whose active DOFs will be used @param request: a JSON planning request for Trajopt @param traj_constraints: list of dicts of constraints that should be applied over the whole trajectory @param goal_constraints: list of dicts of constraints that should be applied only at the last waypoint @param traj_costs: list of dicts of costs that should be applied over the whole trajectory @param goal_costs: list of dicts of costs that should be applied only at the last waypoint @param interactive: pause every iteration, until you press 'p' or press escape to disable further plotting @param constraint_threshold: acceptable per-constraint violation error @param sampling_func: sample generator to compute validity checks @param norm_order: order of norm to use for collision checking @returns traj: trajectory from current configuration to specified goal """ import json import trajoptpy from prpy import util # Set up environment. env = robot.GetEnv() trajoptpy.SetInteractive(interactive) # Trajopt's UserData gets confused if the same environment # is cloned into multiple times, so create a scope to later # remove all TrajOpt UserData keys. try: # Validate request and fill in request fields that must use # specific values to work. assert(request['basic_info']['n_steps'] is not None) request['basic_info']['manip'] = 'active' request['basic_info']['robot'] = robot.GetName() request['basic_info']['start_fixed'] = True n_steps = request['basic_info']['n_steps'] n_dofs = robot.GetActiveDOF() i_dofs = robot.GetActiveDOFIndices() # Convert dictionary into json-formatted string and create object # that stores optimization problem. s = json.dumps(request) prob = trajoptpy.ConstructProblem(s, env) # Add trajectory-wide costs and constraints to each timestep. for t in xrange(1, n_steps): for constraint in traj_constraints: self._addFunction(prob, t, i_dofs, n_dofs, constraint) for cost in traj_costs: self._addFunction(prob, t, i_dofs, n_dofs, cost) # Add goal costs and constraints. for constraint in goal_constraints: self._addFunction(prob, n_steps-1, i_dofs, n_dofs, constraint) for cost in goal_costs: self._addFunction(prob, n_steps-1, i_dofs, n_dofs, cost) # Perform trajectory optimization. t_start = time.time() result = trajoptpy.OptimizeProblem(prob) t_elapsed = time.time() - t_start logger.debug("Optimization took {:.3f} seconds".format(t_elapsed)) # Check for constraint violations. for name, error in result.GetConstraints(): if error > constraint_threshold: raise ConstraintViolationPlanningError( name, threshold=constraint_threshold, violation_by=error) # Check for the returned trajectory. waypoints = result.GetTraj() if waypoints is None: raise PlanningError("Trajectory result was empty.") # Convert the trajectory to OpenRAVE format. traj = self._WaypointsToTraj(robot, waypoints) # Check that trajectory is collision free. p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveDOF): # Set robot DOFs to DOFs in optimization problem. prob.SetRobotActiveDOFs() checkpoints = util.GetLinearCollisionCheckPts(robot, traj, norm_order=norm_order, sampling_func=sampling_func) for _, q_check in checkpoints: self._checkCollisionForIKSolutions( robot, robot_checker, [q_check]) # Convert the waypoints to a trajectory. prpy.util.SetTrajectoryTags(traj, { Tags.SMOOTH: True }, append=True) return traj finally: for body in env.GetBodies(): for key in TRAJOPT_USERDATA_KEYS: body.RemoveUserData(key) trajopt_env_userdata = env.GetKinBody('__trajopt_data__') if trajopt_env_userdata is not None: env.Remove(trajopt_env_userdata)
prob.SetRobotActiveDOFs() t_start_verify = time() is_safe = traj_is_safe(traj, robot) t_verify += time() - t_start_verify if not is_safe: print "optimal trajectory has a collision. trying a new initialization" else: print "planning successful after %s initialization" % (i_init + 1) success = True break t_total = time() - t_start return PlanResult(success, t_total, t_opt, t_verify) trajoptpy.SetInteractive(args.interactive) env = openravepy.Environment() env.StopSimulation() env.Load("robots/pr2-beta-static.zae") loadsuccess = env.Load( osp.join(trajoptpy.data_dir, args.scenefile + ".env.xml")) assert loadsuccess robot = env.GetRobots()[0] robot.SetDOFValues(dof_vals) # import xml.etree.ElementTree as ET # cc = trajoptpy.GetCollisionChecker(env) # root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot() # disabled_elems=root.findall("disable_collisions") # for elem in disabled_elems: # linki = robot.GetLink(elem.get("link1"))
def main(): demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.execution: rospy.init_node("exec_task", disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] if not args.fake_data_segment: subprocess.call("killall XnSensorServer", shell=True) grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: new_xyz = np.squeeze(demofile[args.fake_data_segment]["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle( args.fake_data_transform[3:6]) hmat[:3, 3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :] else: Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') Globals.pr2.join_all() Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) ################################ redprint("Finding closest demonstration") if args.fake_data_segment: seg_name = args.fake_data_segment elif args.choice == "costs": f, seg_name = choose_segment(demofile, new_xyz) else: seg_name = choose_segment(demofile, new_xyz) seg_info = demofile[seg_name] # redprint("using demo %s, description: %s"%(seg_name, seg_info["description"])) ################################ redprint("Generating end-effector trajectory") if not args.choice == "costs": handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0, 1))) handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1, 1))) f = registration.tps_rpm(old_xyz, new_xyz, plot_cb=tpsrpm_plot_cb, plotting=1) handles.extend( plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0), old_xyz.max(axis=0), xres=.1, yres=.1, zres=.04)) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame" % lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj if not args.choice == "costs": handles.append( Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2, (1, 0, 0, 1))) handles.append( Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2, (0, 1, 0, 1))) # TODO plot # plot_warping_and_trajectories(f, old_xyz, new_xyz, old_ee_traj, new_ee_traj) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution == "real": Globals.pr2.update_rave() ################################ redprint("Generating joint trajectory for segment %s, part %i" % (seg_name, i_miniseg)) bodypart2traj = {} arms_used = "" for lr in 'lr': manip_name = {"l": "leftarm", "r": "rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end + 1]) if arm_moved(old_joint_traj): ee_link_name = "%s_gripper_tool_frame" % lr new_ee_traj = link2eetraj[ee_link_name] if args.execution: Globals.pr2.update_rave() new_joint_traj = plan_follow_traj( Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj[i_start:i_end + 1], old_joint_traj) # (robot, manip_name, ee_link, new_hmats, old_traj): part_name = {"l": "larm", "r": "rarm"}[lr] bodypart2traj[part_name] = new_joint_traj arms_used += lr ################################ redprint( "Executing joint trajectory for segment %s, part %i using arms '%s'" % (seg_name, i_miniseg, arms_used)) for lr in 'lr': set_gripper_maybesim( lr, binarize_gripper(seg_info["%s_gripper_joint" % lr][i_start])) #trajoptpy.GetViewer(Globals.env).Idle() if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj) # TODO measure failure condtions if not success: break redprint("Segment %s result: %s" % (seg_name, success)) if args.fake_data_segment: break
def PlanToTSR(self, robot, tsrchains, is_interactive=False, **kw_args): """ Plan using the given list of TSR chains with TrajOpt. @param robot the robot whose active manipulator will be used @param tsrchains a list of TSRChain objects to respect during planning @param is_interactive pause every iteration, until you press 'p' or press escape to disable further plotting @return traj """ import json import time import trajoptpy manipulator = robot.GetActiveManipulator() n_steps = 20 n_dofs = robot.GetActiveDOF() # Create separate lists for the goal and trajectory-wide constraints. goal_tsrs = [t for t in tsrchains if t.sample_goal] traj_tsrs = [t for t in tsrchains if t.constrain] # Find an initial collision-free IK solution by sampling goal TSRs. from openravepy import (IkFilterOptions, IkParameterization, IkParameterizationType) for tsr in self._TsrSampler(goal_tsrs): ik_param = IkParameterization(tsr.sample(), IkParameterizationType.Transform6D) init_joint_config = manipulator.FindIKSolutions( ik_param, IkFilterOptions.CheckEnvCollisions) if init_joint_config: break if not init_joint_config: raise PlanningError('No collision-free IK solutions.') # Construct a planning request with these constraints. request = { "basic_info": { "n_steps": n_steps, "manip": "active", "start_fixed": True }, "costs": [{ "type": "joint_vel", "params": { "coeffs": [1] } }, { "type": "collision", "params": { "coeffs": [20], "dist_pen": [0.025] }, }], "constraints": [], "init_info": { "type": "straight_line", "endpoint": init_joint_config.tolist() } } # Set up environment. env = robot.GetEnv() trajoptpy.SetInteractive(is_interactive) # Convert dictionary into json-formatted string and create object that # stores optimization problem. s = json.dumps(request) prob = trajoptpy.ConstructProblem(s, env) for t in xrange(1, n_steps): prob.AddConstraint(constraints._TsrCostFn(robot, traj_tsrs), [(t, j) for j in xrange(n_dofs)], "EQ", "up{:d}".format(t)) prob.AddConstraint(constraints._TsrCostFn(robot, goal_tsrs), [(n_steps - 1, j) for j in xrange(n_dofs)], "EQ", "up{:d}".format(t)) # Perform trajectory optimization. t_start = time.time() result = trajoptpy.OptimizeProblem(prob) t_elapsed = time.time() - t_start logger.info("Optimization took {:.3f} seconds".format(t_elapsed)) # Check for constraint violations. if result.GetConstraints(): raise PlanningError( "Trajectory did not satisfy constraints: {:s}".format( str(result.GetConstraints()))) # Check for the returned trajectory. waypoints = result.GetTraj() if waypoints is None: raise PlanningError("Trajectory result was empty.") # Set active DOFs to match active manipulator and plan. p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveDOF): # Set robot DOFs to DOFs in optimization problem prob.SetRobotActiveDOFs() # Check that trajectory is collision free from trajoptpy.check_traj import traj_is_safe if not traj_is_safe(waypoints, robot): return PlanningError("Result was in collision.") # Convert the waypoints to a trajectory. return self._WaypointsToTraj(robot, waypoints)
def main(): demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 if args.execution: rospy.init_node("exec_task", disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot else: Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] if not args.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle( args.fake_data_transform[3:6]) hmat[:3, 3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) else: Globals.pr2.head.set_pan_tilt(0, 1.2) Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') Globals.pr2.join_all() time.sleep(.5) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR, "rgb%i.png" % LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR, "depth%i.png" % LOG_COUNT), depth) np.save(osp.join(LOG_DIR, "xyz%i.npy" % LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s" % (seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR, "neighbor%i.txt" % LOG_COUNT), "w") as fh: fh.write(seg_name) ################################ redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0))) handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1))) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f, _ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb=tpsrpm_plot_cb, plotting=5 if args.animation else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) handles.extend( plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0) - np.r_[0, 0, .1], old_xyz.max(axis=0) + np.r_[0, 0, .1], xres=.1, yres=.1, zres=.04)) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame" % lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj handles.append( Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2, (1, 0, 0, 1))) handles.append( Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2, (0, 1, 0, 1))) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)): if args.execution == "real": Globals.pr2.update_rave() ################################ redprint("Generating joint trajectory for segment %s, part %i" % (seg_name, i_miniseg)) # figure out how we're gonna resample stuff lr2oldtraj = {} for lr in 'lr': manip_name = {"l": "leftarm", "r": "rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end + 1]) #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end if arm_moved(old_joint_traj): lr2oldtraj[lr] = old_joint_traj if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) #### ### Generate fullbody traj bodypart2traj = {} for (lr, old_joint_traj) in lr2oldtraj.items(): manip_name = {"l": "leftarm", "r": "rightarm"}[lr] old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) ee_link_name = "%s_gripper_tool_frame" % lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end + 1] new_ee_traj_rs = resampling.interp_hmats( timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) if args.execution: Globals.pr2.update_rave() new_joint_traj = planning.plan_follow_traj( Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj_rs, old_joint_traj_rs) part_name = {"l": "larm", "r": "rarm"}[lr] bodypart2traj[part_name] = new_joint_traj ################################ redprint( "Executing joint trajectory for segment %s, part %i using arms '%s'" % (seg_name, i_miniseg, bodypart2traj.keys())) for lr in 'lr': success &= set_gripper_maybesim( lr, binarize_gripper(seg_info["%s_gripper_joint" % lr][i_start])) # Doesn't actually check if grab occurred, unfortunately if not success: break if len(bodypart2traj) > 0: success &= exec_traj_maybesim(bodypart2traj) if not success: break redprint("Segment %s result: %s" % (seg_name, success)) if args.fake_data_segment: break
def smoothTrajectory(self, configStart, configsPair, constraints, maxCSpaceJump, mover, \ viewEffector, visualize): '''Runs trajectory optimization with viewing constraints. - Input configStart: Initial configuration in trajectory/current configuration of the arm. - Input configsPair: A pair of lists of configurations from which to choose the viewing points. - Input constraints: A pair of ViewConstraint objects for each viewing point. - Input maxCSpaceJump: Estimated c-space distance between waypoints in the trajectory (radians). - Input mover: ArmMover object for computing IK and checking collisions. - Input viewEffector: Name of the sensor link in OpenRAVE. - Input cloud: Point cloud (nx3 array) to add as obstacle. Removes before function returns. Ignores if None. - Input visualize: True to run the OpenRAVE visualization for the trajopt run. - Returns trajectory: List of 7-DOF configurations according to OpenRAVE ordering. - Returns essentialIndices: Indices in the trajectory that correspond to beginning, viewing (x2), and ending points. - Returns cost: The L2 length of the trajectory (radians). - Returns inCollision: True if the resulting trajectory has a collision, according to controller. - Returns meetsPosition: True if the resulting trajectory goes through the viewing points. - Returns meetsKeepout: True if the trajectory between viewing points is outside of the keepout. - Returns meetsOrient: True if the trajectory between viewing points looks at the target point. ''' trajoptpy.SetInteractive(visualize) if mover.isInCollision(configStart): raise Exception( "The current position of the arm is believed to be in collision." ) # 1. Deduce waypoint indices, trajectory length, and initialization. pointsPerRadian = int(round(1.0 / maxCSpaceJump)) initConfigs = [configStart] essentialIndices = [0] pair = motion_planner.ClosestPair(configsPair[0], configsPair[1]) selectedConfigs = [configStart, pair[0], pair[1]] for i in xrange(1, len(selectedConfigs)): nextConfig = selectedConfigs[i] prevConfig = initConfigs[-1] delta = nextConfig - prevConfig nSteps = int(round(norm(delta) * pointsPerRadian)) essentialIndices.append(essentialIndices[-1] + nSteps + 1) for j in xrange(nSteps): initConfigs.append( (prevConfig + (float(j + 1) / float(nSteps + 1)) * delta).tolist()) initConfigs.append(nextConfig.tolist()) # 2. Specify optimization problem and constraints. trajLen = essentialIndices[-1] + 1 request = { "basic_info": { "n_steps": trajLen, "manip": mover.name + "_arm", "start_fixed": True }, "costs": [{ "type": "joint_vel", "params": { "coeffs": [4] } }, { "type": "collision", "name": "cont_coll", "params": { "continuous": True, "coeffs": [4], "dist_pen": [0.02] } }], "constraints": [], "init_info": { "type": "given_traj", "data": initConfigs } } # add waypoint constraints for i in xrange(2): timeStep = essentialIndices[i + 1] request["constraints"].append({ "type": "joint", "params": { "vals": initConfigs[timeStep], "timestep": timeStep } }) # construct problem mover.robot.SetDOFValues(configStart, mover.manip.GetArmIndices()) initConfigs[0] = mover.robot.GetDOFValues().tolist( ) # wtf? Errors sometimes without this. problem = trajoptpy.ConstructProblem(json.dumps(request), mover.env) # joint limit constraints for i in xrange(1, essentialIndices[2] + 1): problem.AddConstraint( constraints[0].GetDistanceFromLowerJointLimits, [(i, j) for j in xrange(7)], "INEQ", "jointLo_{}".format(i)) problem.AddConstraint( constraints[0].GetDistanceFromUpperJointLimits, [(i, j) for j in xrange(7)], "INEQ", "jointHi_{}".format(i)) # viewing constraints for i in xrange(essentialIndices[1] + 1, essentialIndices[2]): constraint = constraints[ 0] if i < essentialIndices[2] else constraints[1] problem.AddConstraint(constraint.GetKeepoutPenalty, [(i, j) for j in xrange(7)], "INEQ", "viewKeep_{}".format(i)) problem.AddConstraint(constraint.GetOrientationPenalty, [(i, j) for j in xrange(7)], "EQ", "viewOrient_{}".format(i)) # 3. Solve optimization problem and return result. result = trajoptpy.OptimizeProblem(problem) trajectory = result.GetTraj() # check result cost = motion_planner.ComputePathCost(trajectory) collision = motion_planner.IsPathInCollision(trajectory, mover, ignoreSoftCollision=True) meetsPosition = norm(initConfigs[essentialIndices[1]]-trajectory[essentialIndices[1]]) < 0.005 \ and norm(initConfigs[essentialIndices[2]]-trajectory[essentialIndices[2]]) < 0.005 meetsKeepout = constraints[0].PathSatisfiesKeepout(\ trajectory[essentialIndices[1]+1:essentialIndices[2]]) meetsOrient = constraints[0].PathSatisfiesOrientation(\ trajectory[essentialIndices[1]+1:essentialIndices[2]]) if collision: print("Detected a collision in the trajectory") return trajectory, essentialIndices, cost, collision, meetsPosition, meetsKeepout, meetsOrient
handles.append(env.plot3(points=ppe_trans_arr[tb.state_change_index][0:3,3], pointsize=5.0, colors=array(((0,1,0))))) print ('STATE CHANGE INDEX IS',tb.state_change_index) if __name__ == "__main__": env = Environment() env.SetViewer('qtcoin') env.Reset() env.Load("/home/anto/ebolabot_ws/src/task_breakdown_openrave/src/task_breakdown_openrave/empty.env.xml") time.sleep(0.1) InitOpenRAVELogging() trajoptpy.SetInteractive(False) robot = env.GetRobots()[0] human = env.GetRobots()[1] goggles = env.GetRobots()[2] forehead = env.GetKinBody('forehead') tb = TaskBreakdown() gr = GogglesRobot() gr.Init(env, goggles) handles = [] tb.Init(robot, env, human, goggles, gr, forehead, handles) tuckarms(env, robot) ppe_loc = []
def __init__(self, interactive): env = openravepy.Environment() env.StopSimulation() print "Loading robot model" env.Load("model/model_test.xml") print "loaded robot model" print "Loading environment" env.Load("environment/env.xml") print "Environment loaded" trajoptpy.SetInteractive( True ) # pause every iteration, until you press 'p'. Press escape to disable further plotting robot = env.GetRobots()[0] print robot.GetManipulator("arm") print robot.GetManipulator('arm').GetArmIndices() joint_start = [0.0, 0.0] robot.SetDOFValues(joint_start, robot.GetManipulator('arm').GetArmIndices()) joint_target = [-np.pi / 2.0, 0.0] n_steps = 100 control_rate = 30.0 delta_t = 1.0 / control_rate max_velocity = 2.0 request = { "basic_info": { "n_steps": n_steps, "manip": "arm", # see below for valid values "start_fixed": True # i.e., DOF values at first timestep are fixed based on current robot state }, "costs": [ { "type": "joint_vel", # joint-space velocity cost "params": { "coeffs": [1] } # a list of length one is automatically expanded to a list of length n_dofs # also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1] }, { "type": "collision", "params": { "coeffs": [ 20 ], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen": [ 0.25 ] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps }, } ], "constraints": [ { "type": "joint", # joint-space target "params": { "vals": joint_target } # length of vals = # dofs of manip }, { "type": "joint_vel_limits", "name": "joint_vel_limits", "params": { "vals": [delta_t * max_velocity, delta_t * max_velocity], "first_step": 0, "last_step": n_steps - 1, #inclusive } } ], "init_info": { "type": "straight_line", # straight line in joint space. "endpoint": joint_target } } s = json.dumps(request) prob = trajoptpy.ConstructProblem( s, env) # create object that stores optimization problem t_start = time.time() result = trajoptpy.OptimizeProblem(prob) # do optimization t_elapsed = time.time() - t_start print result print "optimization took %.3f seconds" % t_elapsed prob.SetRobotActiveDOFs( ) # set robot DOFs to DOFs in optimization problem print traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
def _Plan(self, robot, request, traj_constraints=(), goal_constraints=(), traj_costs=(), goal_costs=(), interactive=False, constraint_threshold=1e-4, **kwargs): """ Plan to a desired configuration with Trajopt. This function invokes the Trajopt planner directly on the specified JSON request. This can be used to implement custom path optimization algorithms. Constraints and costs are specified as dicts of: ``` { 'f': [float] -> [float], 'dfdx': [float] -> [float], 'type': ConstraintType or CostType 'dofs': [int] } ``` The input to f(x) and dfdx(x) is a vector of active DOF values used in the planning problem. The output is a vector of costs, where the value *increases* as a constraint or a cost function is violated or unsatisfied. See ConstraintType and CostType for descriptions of the various function specifications and their expected behavior. The `dofs` parameter can be used to specify a subset of the robot's DOF indices that should be used. A ValueError is thrown if these indices are not entirely contained in the current active DOFs of the robot. @param robot: the robot whose active DOFs will be used @param request: a JSON planning request for Trajopt @param traj_constraints: list of dicts of constraints that should be applied over the whole trajectory @param goal_constraints: list of dicts of constraints that should be applied only at the last waypoint @param traj_costs: list of dicts of costs that should be applied over the whole trajectory @param goal_costs: list of dicts of costs that should be applied only at the last waypoint @param interactive: pause every iteration, until you press 'p' or press escape to disable further plotting @param constraint_threshold: acceptable per-constraint violation error @returns traj: trajectory from current configuration to specified goal """ import json import trajoptpy # Set up environment. env = robot.GetEnv() trajoptpy.SetInteractive(interactive) # Trajopt's UserData gets confused if the same environment # is cloned into multiple times, so create a scope to later # remove all TrajOpt UserData keys. try: # Convert dictionary into json-formatted string and create object # that stores optimization problem. s = json.dumps(request) prob = trajoptpy.ConstructProblem(s, env) assert (request['basic_info']['manip'] == 'active') assert (request['basic_info']['n_steps'] is not None) n_steps = request['basic_info']['n_steps'] n_dofs = robot.GetActiveDOF() i_dofs = robot.GetActiveDOFIndices() # Add trajectory-wide costs and constraints to each timestep. for t in xrange(1, n_steps): for constraint in traj_constraints: self._addFunction(prob, t, i_dofs, n_dofs, constraint) for cost in traj_costs: self._addFunction(prob, t, i_dofs, n_dofs, cost) # Add goal costs and constraints. for constraint in goal_constraints: self._addFunction(prob, n_steps - 1, i_dofs, n_dofs, constraint) for cost in goal_costs: self._addFunction(prob, n_steps - 1, i_dofs, n_dofs, cost) # Perform trajectory optimization. t_start = time.time() result = trajoptpy.OptimizeProblem(prob) t_elapsed = time.time() - t_start logger.debug("Optimization took {:.3f} seconds".format(t_elapsed)) # Check for constraint violations. for name, error in result.GetConstraints(): if error > constraint_threshold: raise PlanningError( "Trajectory violates contraint '{:s}': {:f} > {:f}". format(name, error, constraint_threshold)) # Check for the returned trajectory. waypoints = result.GetTraj() if waypoints is None: raise PlanningError("Trajectory result was empty.") # Set active DOFs to match active manipulator and plan. p = openravepy.KinBody.SaveParameters with robot.CreateRobotStateSaver(p.ActiveDOF): # Set robot DOFs to DOFs in optimization problem prob.SetRobotActiveDOFs() # Check that trajectory is collision free from trajoptpy.check_traj import traj_is_safe if not traj_is_safe(waypoints, robot): raise PlanningError("Result was in collision.") # Convert the waypoints to a trajectory. return self._WaypointsToTraj(robot, waypoints) finally: for body in env.GetBodies(): for key in TRAJOPT_USERDATA_KEYS: body.RemoveUserData(key) trajopt_env_userdata = env.GetKinBody('__trajopt_data__') if trajopt_env_userdata is not None: env.Remove(trajopt_env_userdata)