def gen_init_trajs(problemset, robot, n_steps, start_joints, end_joints): waypoint_step = (n_steps - 1) // 2 joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints)) / 2] if args.multi_init: if args.use_random_inits: print 'using random initializations' joint_waypoints.extend( sample_base_positions(robot, num=5, tucked=True)) # if the problem file has waypoints, just use those else: if problemset["group_name"] in [ "right_arm", "left_arm", "whole_body" ]: joint_waypoints.extend(get_postures(problemset["group_name"])) trajs = [] for i, waypoint in enumerate(joint_waypoints): if i == 0: inittraj = mu.linspace2d(start_joints, end_joints, n_steps) else: inittraj = np.empty((n_steps, robot.GetActiveDOF())) inittraj[:waypoint_step + 1] = mu.linspace2d( start_joints, waypoint, waypoint_step + 1) inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step) trajs.append(inittraj) return trajs
def interpolate_waypoint(start, waypoint, end, num_steps): waypoint_step = (num_steps - 1) // 2 traj = np.empty((num_steps, 7)) traj[:waypoint_step + 1] = mu.linspace2d(start, waypoint, waypoint_step + 1) traj[waypoint_step:] = mu.linspace2d(waypoint, end, num_steps - waypoint_step) return traj
def predict_with_waypoint(self, init_joint, target_joint, waypoint=None, waypoint_step=0): inittraj = np.empty((self.n_steps, self.dof)) inittraj[:waypoint_step + 1] = mu.linspace2d(init_joint, waypoint, waypoint_step + 1) inittraj[waypoint_step:] = mu.linspace2d(waypoint, target_joint, self.n_steps - waypoint_step) return inittraj, np.array([[0]])
def move_arm_straight_request(manip, n_steps, link_name, joints_start, joints_end, xyz_start, quat_start, xyz_end, quat_end): ''' WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible! ''' init_joints = mu.linspace2d(joints_start, joints_end, n_steps) request = { 'basic_info': { 'n_steps': n_steps, 'manip': manip.GetName(), 'start_fixed': False, # take care of this with the init_joints constraint instead }, 'costs': [ { 'type': 'joint_vel', 'params': { 'coeffs': [1] } }, { 'type': 'collision', 'params': { 'coeffs': [1], 'dist_pen': [0.025] } }, ], 'constraints': [ { 'type' : 'joint', 'name' : 'init_joints', 'params' : { 'vals': list(joints_start), 'timestep': 0 }, }, { 'type' : 'joint', 'name' : 'final_joints', 'params' : { 'vals': list(joints_end) }, }, ], 'init_info': { 'type': 'given_traj', 'data': [j.tolist() for j in init_joints], }, } # add costs for deviation from a straight line in cartesian space lin_xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps) lin_quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)] for i in range(1, n_steps-1): request['costs'].append({ 'type': 'pose', 'params': { 'xyz': list(lin_xyzs[i]), 'wxyz': list(lin_quats[i]), 'pos_coeffs': [1, 1, 1], 'rot_coeffs': [1, 1, 1], 'link': link_name, 'timestep': i, } }) return request
def trajopt_multi_plan(env, robot, goal_config, num_inits=10, num_steps=10, warn_if_unchanged=False, inits=None, **args): start_joints = robot.GetActiveDOFValues() linear_init = mu.linspace2d(start_joints, goal_config, num_steps) results = [] default_res = trajopt_simple_plan(env, robot, goal_config, num_steps=num_steps, **args) results.append(default_res) for i in range(num_inits - 1): if inits is not None: modified_init = inits[i] else: modified_init = np.mod(linear_init + smooth_perturb(), 2 * np.pi) res = trajopt_simple_plan(env, robot, goal_config, init=modified_init, num_steps=num_steps, **args) if warn_if_unchanged and np.allclose(modified_init, res.GetTraj()): print('Warning: unchanged init') if not np.allclose(default_res.GetTraj(), res.GetTraj()): results.append(res) return results
def trajopt_simple_plan(env, robot, goal_config, num_steps=10, custom_costs={}, init=None, custom_traj_costs={}, request_callbacks=[], joint_vel_coeff=1.): start_joints = robot.GetActiveDOFValues() if init is None: init = mu.linspace2d(start_joints, goal_config, num_steps) request = { 'basic_info': { 'n_steps': num_steps, 'manip': robot.arm.GetName(), 'start_fixed': True }, 'costs': [{ 'type': 'collision', 'params': { 'coeffs': [20], 'dist_pen': [0.025] } }, { 'type': 'joint_vel', 'params': { 'coeffs': [joint_vel_coeff] } }], 'constraints': [{ 'type': 'joint', 'params': { 'vals': goal_config.tolist() } }], 'init_info': { 'type': 'given_traj', 'data': init.tolist() } } [callback(request) for callback in request_callbacks] s = json.dumps(request) prob = trajoptpy.ConstructProblem(s, env) for cost_name in custom_costs: cost_fn = custom_costs[cost_name] prob.AddCost(cost_fn, [(t, j) for t in range(num_steps) for j in range(7)], '{:s}{:d}'.format(cost_name, t)) for cost_name in custom_traj_costs: cost_fn = custom_traj_costs[cost_name] if isinstance(cost_fn, tuple): prob.AddErrorCost(cost_fn[0], cost_fn[1], [(t, j) for t in range(num_steps) for j in range(7)], 'SQUARED', cost_name) else: prob.AddCost(cost_fn, [(t, j) for t in range(num_steps) for j in range(7)], cost_name) result = trajoptpy.OptimizeProblem(prob) return result
def random_init_maker(given_init, one_wp=False): start = given_init[0] end = given_init[-1] if one_wp: changed_idx = np.random.choice(range(3, 7)) original = given_init[changed_idx] new_pt = np.random.multivariate_normal(original, .05 * np.eye(7)) modified_init = np.concatenate([ mu.linspace2d(given_init[0], new_pt, changed_idx + 1), mu.linspace2d(new_pt, given_init[-1], 10 - changed_idx)[1:] ], axis=0) else: modified_init = given_init.copy() modified_init[1:-1] = np.random.multivariate_normal( given_init[1:-1].reshape(-1), np.linalg.norm(start - end) * 0.0025 * np.eye(56)).reshape(8, -1) return modified_init
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')
def gen_init_trajs(problemset, robot, n_steps, start_joints, end_joints): waypoint_step = (n_steps - 1)// 2 joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2] if args.multi_init: if args.use_random_inits: print 'using random initializations' joint_waypoints.extend(sample_base_positions(robot, num=5, tucked=True)) # if the problem file has waypoints, just use those else: if problemset["group_name"] in ["right_arm", "left_arm", "whole_body"]: joint_waypoints.extend(get_postures(problemset["group_name"])) trajs = [] for i, waypoint in enumerate(joint_waypoints): if i == 0: inittraj = mu.linspace2d(start_joints, end_joints, n_steps) else: inittraj = np.empty((n_steps, robot.GetActiveDOF())) inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1) inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step) trajs.append(inittraj) return trajs
def generate_trajs(): use_planning = (request.form['generate_option'] == 'planning') temp_to_label = [] with env: sg_pair_idcs = np.random.choice(range(len(c.start_goal_pairs)), size=4, replace=False) for idx in sg_pair_idcs: start, goal = c.start_goal_pairs[idx] robot.SetActiveDOFValues(start) if not use_planning: given = mu.linspace2d(start, goal, 10) else: given = planners.trajopt_simple_plan( env, robot, goal, custom_traj_costs=custom_cost, joint_vel_coeff=session_vars['joint_vel_coeff']).GetTraj() wps = [given] for i in range(3): delta = utils.smooth_perturb(.1) wps.append(given + delta) for xA, xB in itertools.combinations(wps, 2): if not np.allclose(xA[:, :7], xB[:, :7]): trajA = utils.waypoints_to_traj( display_env, display_robot1, xA, session_vars['speed'], None, use_ee_dist=session_vars['use_ee_dist']) trajB = utils.waypoints_to_traj( display_env, display_robot2, xB, session_vars['speed'], None, use_ee_dist=session_vars['use_ee_dist']) temp_to_label.append((xA, trajA, xB, trajB)) random.shuffle(temp_to_label) to_label.extend(temp_to_label) starting_dofs = to_label[0][1].GetWaypoint(0)[:7] display_robot1.SetActiveDOFValues(starting_dofs) display_robot2.SetActiveDOFValues(starting_dofs) return redirect(url_for('index'))
def plan(robot, manip, end_joints, end_pose=None): arm_inds = robot.GetManipulator(manip).GetArmIndices() start_joints = robot.GetDOFValues(arm_inds) n_steps = 11 coll_coeff = 10 dist_pen = .05 waypoint_step = (n_steps - 1) // 2 joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints)) / 2] joint_waypoints.extend(get_postures(manip)) success = False t_start = time() t_verify = 0 t_opt = 0 for (i_init, waypoint) in enumerate(joint_waypoints): d = { "basic_info": { "n_steps": n_steps, "manip": manip, "start_fixed": True }, "costs": [{ "type": "joint_vel", "params": { "coeffs": [1] } }, { "type": "collision", "params": { "coeffs": [coll_coeff], "dist_pen": [dist_pen] } }, { "type": "collision", "params": { "continuous": False, "coeffs": [coll_coeff], "dist_pen": [dist_pen] } }], "constraints": [{ "type": "joint", "params": { "vals": end_joints.tolist() } } if end_pose is None else { "type": "pose", "params": { "xyz": end_pose[4:7].tolist(), "wxyz": end_pose[0:4].tolist(), "link": "%s_gripper_tool_frame" % manip[0] } }], "init_info": { "type": "given_traj" } } if args.multi_init: # print "trying with midpoint", waypoint inittraj = np.empty((n_steps, 7)) inittraj[:waypoint_step + 1] = mu.linspace2d( start_joints, waypoint, waypoint_step + 1) inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step) else: inittraj = mu.linspace2d(start_joints, end_joints, n_steps) d["init_info"]["data"] = [row.tolist() for row in inittraj] s = json.dumps(d) prob = trajoptpy.ConstructProblem(s, env) t_start_opt = time() result = trajoptpy.OptimizeProblem(prob) t_opt += time() - t_start_opt traj = result.GetTraj() 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)
def predict(self, init_joint, target_joint): inittraj = np.empty((self.n_steps, self.dof)) inittraj = mu.linspace2d(init_joint, target_joint, self.n_steps) return inittraj, np.array([[0]])
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')
def move_arm_straight_request(manip, n_steps, link_name, joints_start, joints_end, xyz_start, quat_start, xyz_end, quat_end): ''' WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible! ''' init_joints = mu.linspace2d(joints_start, joints_end, n_steps) request = { 'basic_info': { 'n_steps': n_steps, 'manip': manip.GetName(), 'start_fixed': False, # take care of this with the init_joints constraint instead }, 'costs': [ { 'type': 'joint_vel', 'params': { 'coeffs': [1] } }, { 'type': 'collision', 'params': { 'coeffs': [1], 'dist_pen': [0.025] } }, ], 'constraints': [ { 'type': 'joint', 'name': 'init_joints', 'params': { 'vals': list(joints_start), 'timestep': 0 }, }, { 'type': 'joint', 'name': 'final_joints', 'params': { 'vals': list(joints_end) }, }, ], 'init_info': { 'type': 'given_traj', 'data': [j.tolist() for j in init_joints], }, } # add costs for deviation from a straight line in cartesian space lin_xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps) lin_quats = [ rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps) ] for i in range(1, n_steps - 1): request['costs'].append({ 'type': 'pose', 'params': { 'xyz': list(lin_xyzs[i]), 'wxyz': list(lin_quats[i]), 'pos_coeffs': [1, 1, 1], 'rot_coeffs': [1, 1, 1], 'link': link_name, 'timestep': i, } }) return request
def plan(robot, manip, end_joints, end_pose = None): arm_inds = robot.GetManipulator(manip).GetArmIndices() start_joints = robot.GetDOFValues(arm_inds) n_steps = 11 coll_coeff = 10 dist_pen = .05 waypoint_step = (n_steps - 1)// 2 joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2] joint_waypoints.extend(get_postures(manip)) success = False t_start = time() t_verify = 0 t_opt = 0 for (i_init,waypoint) in enumerate(joint_waypoints): d = { "basic_info" : { "n_steps" : n_steps, "manip" : manip, "start_fixed" : True }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : [1]} }, { "type" : "collision", "params" : {"coeffs" : [coll_coeff],"dist_pen" : [dist_pen]} }, { "type" : "collision", "params" : {"continuous": False, "coeffs" : [coll_coeff],"dist_pen" : [dist_pen]} } ], "constraints" : [ {"type" : "joint", "params" : {"vals" : end_joints.tolist()}} if end_pose is None else {"type" : "pose", "params" : {"xyz" : end_pose[4:7].tolist(), "wxyz" : end_pose[0:4].tolist(), "link":"%s_gripper_tool_frame"%manip[0]}} ], "init_info" : { "type" : "given_traj" } } if args.multi_init: # print "trying with midpoint", waypoint inittraj = np.empty((n_steps, 7)) inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1) inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step) else: inittraj = mu.linspace2d(start_joints, end_joints, n_steps) d["init_info"]["data"] = [row.tolist() for row in inittraj] s = json.dumps(d) prob = trajoptpy.ConstructProblem(s, env) t_start_opt = time() result = trajoptpy.OptimizeProblem(prob) t_opt += time() - t_start_opt traj = result.GetTraj() 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)
def callback(getreq): assert isinstance(getreq, ms.GetMotionPlanRequest) req = getreq.motion_plan_request getresp = ms.GetMotionPlanResponse() resp = getresp.motion_plan_response manip = GROUPMAP[req.group_name] update_rave_from_ros(robot, req.start_state.joint_state.position, req.start_state.joint_state.name) base_pose = req.start_state.multi_dof_joint_state.poses[0] base_q = base_pose.orientation base_p = base_pose.position t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z]) robot.SetTransform(t) start_joints = robot.GetDOFValues(robot.GetManipulator(manip).GetArmIndices()) n_steps = 9 coll_coeff = 10 dist_pen = .04 inds = robot.GetManipulator(manip).GetArmJoints() alljoints = robot.GetJoints() names = arm_joint_names = [alljoints[i].GetName() for i in inds] for goal_cnt in req.goal_constraints: if len(goal_cnt.joint_constraints) > 0: assert len(goal_cnt.joint_constraints)==7 end_joints = np.zeros(7) for i in xrange(7): jc = goal_cnt.joint_constraints[i] dof_idx = arm_joint_names.index(jc.joint_name) end_joints[dof_idx] = jc.position waypoint_step = (n_steps - 1)// 2 joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2] if args.multi_init: leftright = req.group_name.split("_")[0] joint_waypoints.extend(get_postures(leftright)) success = False for waypoint in joint_waypoints: robot.SetDOFValues(start_joints, inds) d = { "basic_info" : { "n_steps" : n_steps, "manip" : manip, "start_fixed" : True }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : [1]} }, { "type" : "continuous_collision", "params" : {"coeffs" : [coll_coeff],"dist_pen" : [dist_pen]} } ], "constraints" : [], "init_info" : { "type" : "given_traj" } } d["constraints"].append({ "type" : "joint", "name" : "joint", "params" : { "vals" : end_joints.tolist() } }) if args.multi_init: inittraj = np.empty((n_steps, 7)) inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1) inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step) else: inittraj = mu.linspace2d(start_joints, end_joints, n_steps) print "this",inittraj print "other",mu.linspace2d(start_joints, end_joints, n_steps) d["init_info"]["data"] = [row.tolist() for row in inittraj] if len(goal_cnt.position_constraints) > 0: raise NotImplementedError s = json.dumps(d) t_start = time() prob = trajoptpy.ConstructProblem(s, env) result = trajoptpy.OptimizeProblem(prob) planning_duration_seconds = time() - t_start traj = result.GetTraj() if check_traj(traj, robot.GetManipulator(manip)): print "aw, there's a collision" else: print "no collisions" success = True break if not success: resp.error_code.val = FAILURE return resp resp.trajectory.joint_trajectory.joint_names = names for row in traj: jtp = tm.JointTrajectoryPoint() jtp.positions = row.tolist() jtp.time_from_start = rospy.Duration(0) resp.trajectory.joint_trajectory.points.append(jtp) mdjt = resp.trajectory.multi_dof_joint_trajectory mdjt.joint_names = ['world_joint'] mdjt.frame_ids = ['odom_combined'] mdjt.child_frame_ids = ['base_footprint'] mdjt.header = req.start_state.joint_state.header pose = gm.Pose() pose.orientation.w = 1 for _ in xrange(len(resp.trajectory.joint_trajectory.points)): jtp = mm.MultiDOFJointTrajectoryPoint() jtp.poses.append(pose) #for i in xrange(len(mdjs.joint_names)): #if mdjs.joint_names[i] == "world_joint": #world_joint_pose = mdjs.poses[i] #world_joint_frame_id = mdjs.frame_ids[i] #world_joint_child_frame_id = mdjs.child_frame_ids[i] #print "world_joint_frame_id", world_joint_frame_id #print "world_joint_child_frame_id", world_joint_child_frame_id #mdjt = resp.trajectory.multi_dof_joint_trajectory #mdjt.header.frame_id = req.start_state.joint_state.header.frame_id #mdjt.joint_names.append("world_joint") resp.error_code.val = SUCCESS resp.planning_time = rospy.Duration(planning_duration_seconds) resp.trajectory.joint_trajectory.header = req.start_state.joint_state.header resp.group_name = req.group_name resp.trajectory_start.joint_state = req.start_state.joint_state #resp.trajectory.multi_dof_joint_trajectory #resp.trajectory.multi_dof_joint_trajectory.header = req.start_state.joint_state.header getresp.motion_plan_response = resp return getresp
def main(args): exp_name = args.exp_name traj_counter = 0 # Directory setup vid_dir = os.path.join('web', 'vids', exp_name) if os.path.exists(vid_dir): print('Video directory already exists!') traj_counter = len(os.listdir(vid_dir)) else: os.makedirs(vid_dir) # Queue to send child process commands. task_queue = multiprocessing.Queue() # Queue for receiving trajs generated by child process. traj_queue = multiprocessing.Queue() p = multiprocessing.Process(target=comms_proc, args=[exp_name, task_queue, traj_queue]) p.start() client = connect('style_experiment') planned_wps = {idcs: None for idcs in constants.sg_train_idcs} # Training queue of *labeled* comparisons labeled_comparison_queue = ComparisonQueue(exp_name) env, robot = utils.setup(render=True) raw_input('Press enter to continue...') monitor = record_video.get_geometry() # cf = CostFunction( # robot, # use_all_links=True, # quadratic=False) cf = LinearCostFunction(robot, num_dofs=50) cf_save_path = os.path.join('saves', 'style_experiment', exp_name, 'model') custom_cost = {'NN': planners.get_trajopt_cost(cf)} summary_writer = tf.summary.FileWriter( os.path.join('tb_logs', 'style_experiment', exp_name)) # Actual main loop global_step = 0 prev_num_labeled = 0 num_perturbs = 0 while True: num_labeled = len(labeled_comparison_queue) sufficient_labeled_data = num_labeled >= label_batchsize # Training step if num_labeled - prev_num_labeled > label_batchsize and num_perturbs > 5: prev_num_labeled = num_labeled num_perturbs = 0 for _ in range(num_labeled): comp = labeled_comparison_queue.sample() wpsA = binary_to_array(comp.wpsA) wpsB = binary_to_array(comp.wpsB) label = comp.label wpsA_batch = np.repeat(wpsA[None], 8, axis=0) wpsB_batch = np.repeat(wpsB[None], 8, axis=0) label_batch = np.repeat(label, 8) for batch_idx in range(8): offset = np.random.uniform(0, 2 * np.pi) wpsA_batch[batch_idx, :, 0] = offset wpsB_batch[batch_idx, :, 0] = offset cf.train_pref(wpsA_batch, wpsB_batch, label_batch) wpsA_list, wpsB_list, labels = [], [], [] for comp in labeled_comparison_queue.queue: wpsA_list.append(binary_to_array(comp.wpsA)) wpsB_list.append(binary_to_array(comp.wpsB)) labels.append(comp.label) wpsA_list, wpsB_list, labels = np.stack(wpsA_list), np.stack( wpsB_list), np.array(labels) pred = cf.get_traj_cost(wpsB_list) < cf.get_traj_cost(wpsA_list) t_acc = np.mean(labels == pred) add_simple_summary(summary_writer, 'training.accuracy', t_acc, global_step) summary_writer.flush() cf.save_model(cf_save_path, step=global_step) global_step += 1 for idcs in planned_wps: planned_wps[idcs] = None # Generation step for idcs in constants.sg_train_idcs: s_idx, g_idx = idcs wps = planned_wps[idcs] if wps is None: q_s = constants.configs[s_idx] q_g = constants.configs[g_idx] with env: robot.SetActiveDOFValues(q_s) if sufficient_labeled_data: wps = planners.trajopt_simple_plan( env, robot, q_g, custom_costs=custom_cost, joint_vel_coeff=1).GetTraj() else: # Generate pretraining data wps = mu.linspace2d(q_s, q_g, 10) planned_wps[idcs] = wps else: wps = make_perturbs(wps, 1, .1)[0] num_perturbs += 1 vid_name = 's{:d}-g{:d}_traj{:d}.mp4'.format( s_idx, g_idx, traj_counter) out_path = os.path.join(vid_dir, vid_name) with env: traj = utils.waypoints_to_traj(env, robot, wps, 1, None) # TODO: move the video export to a different process if not args.no_record: record_video.record(robot, traj, out_path, monitor=monitor) traj_queue.put((wps, out_path, idcs, traj_counter)) traj_counter += 1 time.sleep(.1)
def move_arm_straight_request(manip, n_steps, link_name, xyz_start, xyz_end, quat_start, quat_end, start_joints): manip_name = manip.GetName() request = { "basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": True}, "costs": [ {"type": "joint_vel", "params": {"coeffs": [1]}}, {"type": "collision", "params": {"coeffs": [10], "dist_pen": [0.025]}}, ], "constraints": [ { "type": "pose", "name": "final_pose", "params": { "pos_coeffs": [10, 10, 10], "rot_coeffs": [10, 10, 10], "xyz": list(xyz_end), "wxyz": list(quat_end), "link": link_name, }, } ], "init_info": {}, } xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps) quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)] hmats = [rave.matrixFromPose(np.r_[quat, xyz]) for (xyz, quat) in zip(xyzs, quats)] def ikfunc(hmat): return ku.ik_for_link(hmat, manip, link_name, return_all_solns=True) def nodecost(joints): robot = manip.GetRobot() saver = rave.Robot.RobotStateSaver(robot) robot.SetDOFValues(joints, manip.GetArmJoints()) return 100 * robot.GetEnv().CheckCollision(robot) paths, costs, timesteps = ku.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost) i_best = np.argmin(costs) print "lowest cost of initial trajs:", costs[i_best] if len(timesteps) < n_steps: print "timesteps with soln: ", timesteps print "linearly interpolating the rest" path_init = mu.interp2d(np.arange(n_steps), timesteps, paths[i_best]) else: print "all timesteps have an ik solution" path_init = paths[i_best] for i in xrange(1, n_steps - 1): print "waypoint xyz", xyzs[i] waypoint_cnt = { "type": "pose", "name": "waypoint_pose", "params": { "xyz": list(xyzs[i]), "wxyz": list(quats[i]), "link": link_name, # "pos_coeffs" : [10,10,10], # "rot_coeffs" : [10,10,10], "timestep": i, }, } request["constraints"].append(waypoint_cnt) request["init_info"]["type"] = "given_traj" request["init_info"]["data"] = [x.tolist() for x in path_init] return request
def main(iterations, activation, normalize, dropout, lr, batchnorm, q_length, use_quadratic_features, perturb_amount, provide_vel, include_configs, use_all_links, h_size, random_inits, _seed, _run): tf.set_random_seed(_seed) # Set tf's seed to exp seed env, robot = utils.setup(render=False) cf = CostFunction(robot, num_dofs=7, normalize=normalize, include_configs=include_configs, provide_vel=provide_vel, use_all_links=use_all_links, activation=activation, dropout=dropout, lr=lr, batchnorm=batchnorm, quadratic=use_quadratic_features, h_size=h_size) ex.info['num_params'] = cf.num_params ex.info['mlp_input_shape'] = cf.mlp.model.input_shape summary_writer = tf.summary.FileWriter( os.path.join('tb_logs', ex_name, str(_run._id))) if use_quadratic_features: custom_cost = {'NN': planners.get_trajopt_error_cost(cf)} else: custom_cost = {'NN': planners.get_trajopt_cost(cf)} tqs = {} for idcs in constants.sg_train_idcs: tqs[idcs] = utils.TrainingQueue(maxsize=q_length) # The avg cost of train trajs generated using the true cost fxn. BEST_COST_SYNTH = 0.13875292480745718 for idcs in constants.sg_train_idcs: with env: linear = mu.linspace2d(constants.configs[idcs[0]], constants.configs[idcs[1]], 10) for wps_perturbed in make_perturbs(linear, 50, perturb_amount): true_cost_perturbed = ee_traj_cost(wps_perturbed, robot) tqs[idcs].add((wps_perturbed, true_cost_perturbed)) for i in range(iterations): print('[*] Iteration {:d}/{:d}'.format(i, iterations)) # Training for j in range(500): for idcs in constants.sg_train_idcs: tq = tqs[idcs] (wpsA, cA), (wpsB, cB) = tq.sample(num=2) if np.allclose(cA, cB): continue offset = np.random.uniform(0, 2 * np.pi) wpsA[:, 0] = offset wpsB[:, 0] = offset cf.train_pref(wpsA[None], wpsB[None], [int(cB < cA)]) random_inputs = np.random.uniform(0, 2 * np.pi, size=(100, 10, 7)) with env: labels = [ee_traj_cost(x, robot) for x in random_inputs] corr = cf.get_corrcoef(random_inputs, labels) add_simple_summary(summary_writer, 'test.correlation', corr, i) summary_writer.flush() # Generating training data true_cost_list = [ ] # want to keep track of how good our true costs are true_cost_variances = [] # Only applies if random_init = True for idcs in constants.sg_train_idcs: with env: robot.SetActiveDOFValues(constants.configs[idcs[0]]) if random_inits: results_list = planners.trajopt_multi_plan( env, robot, constants.configs[idcs[1]], num_inits=10, custom_traj_costs=(custom_cost if use_quadratic_features else {}), custom_costs=({} if use_quadratic_features else custom_cost), joint_vel_coeff=0) true_costs = [] for res in results_list: wps = res.GetTraj() true_cost = ee_traj_cost(wps, robot) tqs[idcs].add((wps, true_cost)) true_costs.append(true_cost) true_cost_list.append(np.min(true_costs)) true_cost_variances.append(np.var(true_costs)) else: wps = planners.trajopt_simple_plan( env, robot, constants.configs[idcs[1]], custom_traj_costs=(custom_cost if use_quadratic_features else {}), custom_costs=({} if use_quadratic_features else custom_cost), joint_vel_coeff=0).GetTraj() true_cost = ee_traj_cost(wps, robot) tqs[idcs].add((wps, true_cost)) true_cost_list.append(true_cost) for wps_perturbed in make_perturbs(x, 10, perturb_amount): true_cost_perturbed = ee_traj_cost( wps_perturbed, robot) tqs[idcs].add((wps_perturbed, true_cost_perturbed)) add_simple_summary(summary_writer, 'training.true_cost', np.mean(true_cost_list), i) if random_inits: add_simple_summary(summary_writer, 'training.true_cost_var', np.var(true_cost_variances, i)) summary_writer.flush() # flush summary writer at end of iteration save_folder = './saves/learned_ssee/' cf.save_model(os.path.join(save_folder, 'model')) for fname in os.listdir(save_folder): ex.add_artifact(os.path.join(save_folder, fname))