def plan_full_traj(robot, hmat_start, hmat_end, start_fixed, n_steps=10, \ lr='r', beta_rot=1000.0, grasp_cup=False, R=0.02, D=0.035, \ cup_xyz=None): manip_name = {"l": "leftarm", "r": "rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame" % lr ee_link = robot.GetLink(ee_link_name) new_hmats = np.asarray(resampling.interp_hmats(np.arange(n_steps), \ np.r_[0, n_steps-1], [hmat_start, hmat_end])) dof_vals = robot.GetDOFValues( robot.GetManipulator(manip_name).GetArmIndices()) old_traj = np.tile(dof_vals, (n_steps, 1)) pose_weights = np.logspace(0, 1, num=n_steps, base=10) / 10 # pose_costs = (rotation squared err, position squared error) traj, _, pose_costs = \ planning.plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj, start_fixed=start_fixed, beta_rot=beta_rot, beta_pos=100000.0, \ pose_weights=pose_weights, gamma=1000, \ grasp_cup=grasp_cup, R=R, D=D, cup_xyz=cup_xyz) dof_inds = sim_util.dof_inds_from_name(robot, manip_name) return traj, dof_inds, pose_costs
def position_base_request(shared, cost_fn_xsg, starting_config, goal_config, n_steps, init_position=[0, 0]): # init_position - initial position of robot's base # seems like nsteps needs to be defined as 1 when the base is the only active DOF initialized # Initialize robot position T = shared.robot.GetTransform() T[:, 3][:2] = init_position shared.robot.SetTransform(T) gripper_before(shared) armids = list(shared.manip.GetArmIndices()) #get arm indices shared.robot.SetDOFValues(starting_config, armids) links = shared.robot.GetLinks() link_idx = links.index(shared.robot.GetLink('r_gripper_palm_link')) linkstrans = shared.robot.GetLinkTransformations() xyz_target = list(linkstrans[link_idx][:3][:, 3]) #get xyz of specific link quat_target = list( Quaternion(matrix=linkstrans[link_idx])) #get quat of specific link if shared.use_base: shared.robot.SetActiveDOFs( np.r_[shared.manip.GetArmIndices()], openravepy.DOFAffine.X + openravepy.DOFAffine.Y) request = { "basic_info": { "n_steps": n_steps, "start_fixed": True # DOF values at first timestep are fixed based on current robot state }, "costs": [ { "type": "joint_vel", "params": { "coeffs": [1] } }, { "type": "collision", "params": { "coeffs": [ 1 ], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen": [ 0.025 ] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps } }, ], "constraints": [], "init_info": {} } ##Initialize trajectory as stationary##: request["init_info"]["type"] = "stationary" request["basic_info"][ "manip"] = "active" if shared.use_base else "rightarm" for i in range(n_steps): request["constraints"].append({ "type": "pose", "name": "pose" + str(i), "params": { "pos_coeffs": [6, 6, 6], #[6,6,6], "rot_coeffs": [2, 2, 2], #[2,2,2], "xyz": list(xyz_target), "wxyz": list(quat_target), "link": "r_gripper_palm_link", "timestep": i, } }) s = json.dumps(request) prob = trajoptpy.ConstructProblem(s, shared.env) cost_fn = lambda x: cost_fn_xsg(x, starting_config, goal_config) with openravepy.RobotStateSaver(shared.robot): with util.suppress_stdout(): n_dof = 7 if shared.use_base: n_dof = 9 prob.AddCost(cost_fn, [(n_steps - 1, j) for j in range(n_dof)], "express%i" % (n_steps - 1)) result = trajoptpy.OptimizeProblem(prob) traj = result.GetTraj() dof_inds = sim_util.dof_inds_from_name(shared.robot, shared.manip_name) sim_util.unwrap_in_place(traj, dof_inds) return traj, result
def joint_fit_tps_follow_finger_pts_trajs(robot, manip_name, flr2finger_link_names, flr2finger_rel_pts, flr2old_finger_pts_trajs, old_traj, f, closing_pts=None, no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None, alpha=settings.ALPHA, beta_pos=settings.BETA_POS, gamma=settings.GAMMA): orig_dof_inds = robot.GetActiveDOFIndices() orig_dof_vals = robot.GetDOFValues() n_steps = old_traj.shape[0] dof_inds = sim_util.dof_inds_from_name(robot, manip_name) assert old_traj.shape[1] == len(dof_inds) for flr2old_finger_pts_traj in flr2old_finger_pts_trajs: for old_finger_pts_traj in flr2old_finger_pts_traj.values(): assert len(old_finger_pts_traj) == n_steps assert len(flr2finger_link_names) == len(flr2old_finger_pts_trajs) # expand these (n, d) = f.x_na.shape bend_coefs = np.ones(d) * f.bend_coef if np.isscalar( f.bend_coef) else f.bend_coef rot_coefs = np.ones(d) * f.rot_coef if np.isscalar( f.rot_coef) else f.rot_coef if f.wt_n is None: wt_n = np.ones(n) else: wt_n = f.wt_n if wt_n.ndim == 1: wt_n = wt_n[:, None] if wt_n.shape[1] == 1: wt_n = np.tile(wt_n, (1, d)) if no_collision_cost_first: init_traj, _, (N, init_z), _, _ = joint_fit_tps_follow_finger_pts_trajs( robot, manip_name, flr2finger_link_names, flr2finger_rel_pts, flr2old_finger_pts_trajs, old_traj, f, closing_pts=closing_pts, no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits, alpha=alpha, beta_pos=beta_pos, gamma=gamma) else: init_traj = old_traj.copy() N = f.N init_z = f.z if start_fixed: init_traj = np.r_[robot.GetDOFValues(dof_inds)[None, :], init_traj[1:]] sim_util.unwrap_in_place(init_traj, dof_inds) init_traj += robot.GetDOFValues(dof_inds) - init_traj[0, :] request = { "basic_info": { "n_steps": n_steps, "m_ext": n, "n_ext": d, "manip": manip_name, "start_fixed": start_fixed }, "costs": [{ "type": "joint_vel", "params": { "coeffs": [gamma / (n_steps - 1)] } }, { "type": "tps", "name": "tps", "params": { "x_na": [row.tolist() for row in f.x_na], "y_ng": [row.tolist() for row in f.y_ng], "bend_coefs": bend_coefs.tolist(), "rot_coefs": rot_coefs.tolist(), "wt_n": [row.tolist() for row in wt_n], "N": [row.tolist() for row in N], "alpha": alpha, } }], "constraints": [], "init_info": { "type": "given_traj", "data": [x.tolist() for x in init_traj], "data_ext": [row.tolist() for row in init_z] } } if use_collision_cost: request["costs"].append({ "type": "collision", "params": { "continuous": True, "coeffs": [ 1000 ], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen": [ 0.025 ] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps } }) if joint_vel_limits is not None: request["constraints"].append({ "type": "joint_vel_limits", "params": { "vals": joint_vel_limits, "first_step": 0, "last_step": n_steps - 1 } }) if closing_pts is not None: request["costs"].append({ "type": "tps_jac_orth", "params": { "tps_cost_name": "tps", "pts": closing_pts.tolist(), "coeffs": [10.0] * len(closing_pts), } }) for (flr2finger_link_name, flr2old_finger_pts_traj) in zip(flr2finger_link_names, flr2old_finger_pts_trajs): for finger_lr, finger_link_name in flr2finger_link_name.items(): finger_rel_pts = flr2finger_rel_pts[finger_lr] old_finger_pts_traj = flr2old_finger_pts_traj[finger_lr] for (i_step, old_finger_pts) in enumerate(old_finger_pts_traj): if start_fixed and i_step == 0: continue request["costs"].append({ "type": "tps_rel_pts", "params": { "tps_cost_name": "tps", "src_xyzs": old_finger_pts.tolist(), "rel_xyzs": finger_rel_pts.tolist(), "link": finger_link_name, "timestep": i_step, "pos_coeffs": [np.sqrt(beta_pos / n_steps)] * 4, } }) s = json.dumps(request) with openravepy.RobotStateSaver(robot): with util.suppress_stdout(): prob = trajoptpy.ConstructProblem(s, robot.GetEnv( )) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj() f.z = result.GetExt() theta = N.dot(f.z) f.trans_g = theta[0, :] f.lin_ag = theta[1:d + 1, :] f.w_ng = theta[d + 1:] tps_rel_pts_costs = np.sum([ cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "tps_rel_pts" ]) tps_rel_pts_err = [] with openravepy.RobotStateSaver(robot): for (flr2finger_link_name, flr2old_finger_pts_traj) in zip(flr2finger_link_names, flr2old_finger_pts_trajs): for finger_lr, finger_link_name in flr2finger_link_name.items(): finger_link = robot.GetLink(finger_link_name) finger_rel_pts = flr2finger_rel_pts[finger_lr] old_finger_pts_traj = flr2old_finger_pts_traj[finger_lr] for (i_step, old_finger_pts) in enumerate(old_finger_pts_traj): if start_fixed and i_step == 0: continue robot.SetDOFValues(traj[i_step], dof_inds) new_hmat = finger_link.GetTransform() tps_rel_pts_err.append( f.transform_points(old_finger_pts) - (new_hmat[:3, 3][None, :] + finger_rel_pts.dot(new_hmat[:3, :3].T))) tps_rel_pts_err = np.concatenate(tps_rel_pts_err, axis=0) tps_rel_pts_costs2 = (beta_pos / n_steps) * np.square( tps_rel_pts_err).sum() # TODO don't square n_steps tps_cost = np.sum([ cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "tps" ]) tps_cost2 = alpha * f.get_objective().sum() matching_err = f.transform_points(f.x_na) - f.y_ng joint_vel_cost = np.sum([ cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel" ]) joint_vel_err = np.diff(traj, axis=0) joint_vel_cost2 = (gamma / (n_steps - 1)) * np.square(joint_vel_err).sum() sim_util.unwrap_in_place(traj, dof_inds) joint_vel_err = np.diff(traj, axis=0) collision_costs = [ cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type ] if len(collision_costs) > 0: collision_err = np.asarray(collision_costs) collision_costs = np.sum(collision_costs) tps_jac_orth_cost = [ cost_val for (cost_type, cost_val) in result.GetCosts() if "tps_jac_orth" in cost_type ] if len(tps_jac_orth_cost) > 0: tps_jac_orth_cost = np.sum(tps_jac_orth_cost) f_jacs = f.compute_jacobian(closing_pts) tps_jac_orth_err = [] for jac in f_jacs: tps_jac_orth_err.extend((jac.dot(jac.T) - np.eye(3)).flatten()) tps_jac_orth_err = np.asarray(tps_jac_orth_err) tps_jac_orth_cost2 = np.square(10.0 * tps_jac_orth_err).sum() obj_value = np.sum( [cost_val for (cost_type, cost_val) in result.GetCosts()]) print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed") print "{:>15} | {:>10}".format("COSTS", "-" * 23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2) print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps", tps_cost, tps_cost2) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-") print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps_rel_pts(s)", tps_rel_pts_costs, tps_rel_pts_costs2) if np.isscalar(tps_jac_orth_cost): print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps_jac_orth", tps_jac_orth_cost, tps_jac_orth_cost2) print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-") print "" print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max") print "{:>15} | {:>10}".format("ERRORS", "-" * 23) print "{:>15} | {:>10,.4} | {:>10,.4}".format( "joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max())) print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps (matching)", np.abs(matching_err).min(), np.abs(matching_err).max()) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10,.4}".format( "collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format( "tps_rel_pts(s)", np.abs(tps_rel_pts_err).min(), np.abs(tps_rel_pts_err).max()) if np.isscalar(tps_jac_orth_cost): print "{:>15} | {:>10,.4} | {:>10,.4}".format( "tps_jac_orth", np.abs(tps_jac_orth_err).min(), np.abs(tps_jac_orth_err).max()) print "" # make sure this function doesn't change state of the robot assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices()) assert not np.any(orig_dof_vals - robot.GetDOFValues()) return traj, obj_value, tps_rel_pts_costs, tps_cost
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None, beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA): orig_dof_inds = robot.GetActiveDOFIndices() orig_dof_vals = robot.GetDOFValues() n_steps = len(ee_trajs[0]) dof_inds = sim_util.dof_inds_from_name(robot, manip_name) assert old_traj.shape[0] == n_steps assert old_traj.shape[1] == len(dof_inds) assert len(ee_link_names) == len(ee_trajs) if no_collision_cost_first: init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits, beta_pos=beta_pos, beta_rot=beta_rot, gamma=gamma) else: init_traj = old_traj.copy() if start_fixed: init_traj = np.r_[robot.GetDOFValues(dof_inds)[None, :], init_traj[1:]] sim_util.unwrap_in_place(init_traj, dof_inds) init_traj += robot.GetDOFValues(dof_inds) - init_traj[0, :] request = { "basic_info": { "n_steps": n_steps, "manip": manip_name, "start_fixed": start_fixed }, "costs": [ { "type": "joint_vel", "params": { "coeffs": [gamma / (n_steps - 1)] } }, ], "constraints": [], "init_info": { "type": "given_traj", "data": [x.tolist() for x in init_traj] } } if use_collision_cost: request["costs"].append({ "type": "collision", "params": { "continuous": True, "coeffs": [ 1000 ], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen": [ 0.025 ] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps } }) if joint_vel_limits is not None: request["constraints"].append({ "type": "joint_vel_limits", "params": { "vals": joint_vel_limits, "first_step": 0, "last_step": n_steps - 1 } }) for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs): poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj] for (i_step, pose) in enumerate(poses): if start_fixed and i_step == 0: continue request["costs"].append({ "type": "pose", "params": { "xyz": pose[4:7].tolist(), "wxyz": pose[0:4].tolist(), "link": ee_link_name, "timestep": i_step, "pos_coeffs": [np.sqrt(beta_pos / n_steps)] * 3, "rot_coeffs": [np.sqrt(beta_rot / n_steps)] * 3 } }) s = json.dumps(request) with openravepy.RobotStateSaver(robot): orig_dof_vals with util.suppress_stdout(): prob = trajoptpy.ConstructProblem(s, robot.GetEnv( )) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj() pose_costs = np.sum([ cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose" ]) pose_err = [] with openravepy.RobotStateSaver(robot): for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs): ee_link = robot.GetLink(ee_link_name) for (i_step, hmat) in enumerate(ee_traj): if start_fixed and i_step == 0: continue robot.SetDOFValues(traj[i_step], dof_inds) new_hmat = ee_link.GetTransform() pose_err.append( openravepy.poseFromMatrix( mu.invertHmat(hmat).dot(new_hmat))) pose_err = np.asarray(pose_err) pose_costs2 = (beta_rot / n_steps) * np.square(pose_err[:, 1:4]).sum() + ( beta_pos / n_steps) * np.square(pose_err[:, 4:7]).sum() joint_vel_cost = np.sum([ cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel" ]) joint_vel_err = np.diff(traj, axis=0) joint_vel_cost2 = (gamma / (n_steps - 1)) * np.square(joint_vel_err).sum() sim_util.unwrap_in_place(traj, dof_inds) joint_vel_err = np.diff(traj, axis=0) collision_costs = [ cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type ] if len(collision_costs) > 0: collision_err = np.asarray(collision_costs) collision_costs = np.sum(collision_costs) obj_value = np.sum( [cost_val for (cost_type, cost_val) in result.GetCosts()]) print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed") print "{:>15} | {:>10}".format("COSTS", "-" * 23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-") print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2) print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-") print "" print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max") print "{:>15} | {:>10}".format("ERRORS", "-" * 23) print "{:>15} | {:>10,.4} | {:>10,.4}".format( "joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max())) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10,.4}".format( "collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format( "rot pose(s)", np.abs(pose_err[:, 1:4]).min(), np.abs(pose_err[:, 1:4]).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format( "trans pose(s)", np.abs(pose_err[:, 4:7]).min(), np.abs(pose_err[:, 4:7]).max()) print "" # make sure this function doesn't change state of the robot assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices()) assert not np.any(orig_dof_vals - robot.GetDOFValues()) return traj, obj_value, pose_costs
def transfer(self, demo, test_scene_state, callback=None, plotting=False): reg = self.registration_factory.register(demo, test_scene_state, callback=callback) handles = [] if plotting: demo_cloud = demo.scene_state.cloud test_cloud = reg.test_scene_state.cloud demo_color = demo.scene_state.color test_color = reg.test_scene_state.color handles.append(self.sim.env.plot3(demo_cloud[:,:3], 2, demo_color if demo_color is not None else (1,0,0))) handles.append(self.sim.env.plot3(test_cloud[:,:3], 2, test_color if test_color is not None else (0,0,1))) self.sim.viewer.Step() active_lr = "" for lr in 'lr': if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved(demo.aug_traj.lr2arm_traj[lr]): active_lr += lr _, timesteps_rs = sim_util.unif_resample(np.c_[(1./settings.JOINT_LENGTH_PER_STEP) * np.concatenate([demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1), (1./settings.FINGER_CLOSE_RATE) * np.concatenate([demo.aug_traj.lr2finger_traj[lr] for lr in active_lr], axis=1)], 1.) demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs) if self.init_trajectory_transferer: warm_init_traj = self.init_trajectory_transferer.transfer(reg, demo, plotting=plotting) manip_name = "" flr2finger_link_names = [] flr2demo_finger_pts_trajs_rs = [] init_traj = np.zeros((len(timesteps_rs),0)) for lr in active_lr: arm_name = {"l":"leftarm", "r":"rightarm"}[lr] finger_name = "%s_gripper_l_finger_joint"%lr if manip_name: manip_name += "+" manip_name += arm_name + "+" + finger_name if self.init_trajectory_transferer: init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr], warm_init_traj.lr2finger_traj[lr]] else: init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr], demo_aug_traj_rs.lr2finger_traj[lr]] if plotting: handles.append(self.sim.env.drawlinestrip(demo.aug_traj.lr2ee_traj[lr][:,:3,3], 2, (1,0,0))) handles.append(self.sim.env.drawlinestrip(demo_aug_traj_rs.lr2ee_traj[lr][:,:3,3], 2, (1,1,0))) transformed_ee_traj_rs = reg.f.transform_hmats(demo_aug_traj_rs.lr2ee_traj[lr]) handles.append(self.sim.env.drawlinestrip(transformed_ee_traj_rs[:,:3,3], 2, (0,1,0))) self.sim.viewer.Step() flr2demo_finger_pts_traj_rs = sim_util.get_finger_pts_traj(self.sim.robot, lr, (demo_aug_traj_rs.lr2ee_traj[lr], demo_aug_traj_rs.lr2finger_traj[lr])) flr2demo_finger_pts_trajs_rs.append(flr2demo_finger_pts_traj_rs) flr2transformed_finger_pts_traj_rs = {} flr2finger_link_name = {} flr2finger_rel_pts = {} for finger_lr in 'lr': flr2transformed_finger_pts_traj_rs[finger_lr] = reg.f.transform_points(np.concatenate(flr2demo_finger_pts_traj_rs[finger_lr], axis=0)).reshape((-1,4,3)) flr2finger_link_name[finger_lr] = "%s_gripper_%s_finger_tip_link"%(lr,finger_lr) flr2finger_rel_pts[finger_lr] = sim_util.get_finger_rel_pts(finger_lr) flr2finger_link_names.append(flr2finger_link_name) if plotting: handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2demo_finger_pts_traj_rs, (1,1,0))) handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2transformed_finger_pts_traj_rs, (0,1,0))) self.sim.viewer.Step() if not self.init_trajectory_transferer: # modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do) dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name) joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint"%lr) init_traj[:,dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits([joint_ind])[0][0] print "planning joint TPS and finger points trajectory following" test_traj, obj_value, tps_rel_pts_costs, tps_cost = planning.joint_fit_tps_follow_finger_pts_trajs(self.sim.robot, manip_name, flr2finger_link_names, flr2finger_rel_pts, flr2demo_finger_pts_trajs_rs, init_traj, reg.f, use_collision_cost=self.use_collision_cost, start_fixed=False, alpha=self.alpha, beta_pos=self.beta_pos, gamma=self.gamma) full_traj = (test_traj, sim_util.dof_inds_from_name(self.sim.robot, manip_name)) test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj(self.sim.robot, full_traj, lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj, lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj) if plotting: for lr in active_lr: flr2new_transformed_finger_pts_traj_rs = {} for finger_lr in 'lr': flr2new_transformed_finger_pts_traj_rs[finger_lr] = reg.f.transform_points(np.concatenate(flr2demo_finger_pts_traj_rs[finger_lr], axis=0)).reshape((-1,4,3)) handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2new_transformed_finger_pts_traj_rs, (0,1,1))) handles.append(self.sim.env.drawlinestrip(test_aug_traj.lr2ee_traj[lr][:,:3,3], 2, (0,0,1))) flr2test_finger_pts_traj = sim_util.get_finger_pts_traj(self.sim.robot, lr, full_traj) handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2test_finger_pts_traj, (0,0,1))) self.sim.viewer.Step() return test_aug_traj
def transfer(self, demo, test_scene_state, callback=None, plotting=False): reg = self.registration_factory.register(demo, test_scene_state, callback=callback) handles = [] if plotting: demo_cloud = demo.scene_state.cloud test_cloud = reg.test_scene_state.cloud demo_color = demo.scene_state.color test_color = reg.test_scene_state.color handles.append( self.sim.env.plot3( demo_cloud[:, :3], 2, demo_color if demo_color is not None else (1, 0, 0))) handles.append( self.sim.env.plot3( test_cloud[:, :3], 2, test_color if test_color is not None else (0, 0, 1))) self.sim.viewer.Step() active_lr = "" for lr in 'lr': if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved( demo.aug_traj.lr2arm_traj[lr]): active_lr += lr _, timesteps_rs = sim_util.unif_resample( np.c_[(1. / settings.JOINT_LENGTH_PER_STEP) * np.concatenate( [demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1 ), (1. / settings.FINGER_CLOSE_RATE) * np.concatenate( [demo.aug_traj.lr2finger_traj[lr] for lr in active_lr], axis=1)], 1.) demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs) if self.init_trajectory_transferer: warm_init_traj = self.init_trajectory_transferer.transfer( reg, demo, plotting=plotting) manip_name = "" flr2finger_link_names = [] flr2demo_finger_pts_trajs_rs = [] init_traj = np.zeros((len(timesteps_rs), 0)) for lr in active_lr: arm_name = {"l": "leftarm", "r": "rightarm"}[lr] finger_name = "%s_gripper_l_finger_joint" % lr if manip_name: manip_name += "+" manip_name += arm_name + "+" + finger_name if self.init_trajectory_transferer: init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr], warm_init_traj.lr2finger_traj[lr]] else: init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr], demo_aug_traj_rs.lr2finger_traj[lr]] if plotting: handles.append( self.sim.env.drawlinestrip( demo.aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (1, 0, 0))) handles.append( self.sim.env.drawlinestrip( demo_aug_traj_rs.lr2ee_traj[lr][:, :3, 3], 2, (1, 1, 0))) transformed_ee_traj_rs = reg.f.transform_hmats( demo_aug_traj_rs.lr2ee_traj[lr]) handles.append( self.sim.env.drawlinestrip( transformed_ee_traj_rs[:, :3, 3], 2, (0, 1, 0))) self.sim.viewer.Step() flr2demo_finger_pts_traj_rs = sim_util.get_finger_pts_traj( self.sim.robot, lr, (demo_aug_traj_rs.lr2ee_traj[lr], demo_aug_traj_rs.lr2finger_traj[lr])) flr2demo_finger_pts_trajs_rs.append(flr2demo_finger_pts_traj_rs) flr2transformed_finger_pts_traj_rs = {} flr2finger_link_name = {} flr2finger_rel_pts = {} for finger_lr in 'lr': flr2transformed_finger_pts_traj_rs[ finger_lr] = reg.f.transform_points( np.concatenate(flr2demo_finger_pts_traj_rs[finger_lr], axis=0)).reshape((-1, 4, 3)) flr2finger_link_name[ finger_lr] = "%s_gripper_%s_finger_tip_link" % (lr, finger_lr) flr2finger_rel_pts[finger_lr] = sim_util.get_finger_rel_pts( finger_lr) flr2finger_link_names.append(flr2finger_link_name) if plotting: handles.extend( sim_util.draw_finger_pts_traj(self.sim, flr2demo_finger_pts_traj_rs, (1, 1, 0))) handles.extend( sim_util.draw_finger_pts_traj( self.sim, flr2transformed_finger_pts_traj_rs, (0, 1, 0))) self.sim.viewer.Step() if not self.init_trajectory_transferer: # modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do) dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name) joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint" % lr) init_traj[:, dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits( [joint_ind])[0][0] print "planning joint TPS and finger points trajectory following" test_traj, obj_value, tps_rel_pts_costs, tps_cost = planning.joint_fit_tps_follow_finger_pts_trajs( self.sim.robot, manip_name, flr2finger_link_names, flr2finger_rel_pts, flr2demo_finger_pts_trajs_rs, init_traj, reg.f, use_collision_cost=self.use_collision_cost, start_fixed=False, alpha=self.alpha, beta_pos=self.beta_pos, gamma=self.gamma) full_traj = (test_traj, sim_util.dof_inds_from_name(self.sim.robot, manip_name)) test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj( self.sim.robot, full_traj, lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj, lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj) if plotting: for lr in active_lr: flr2new_transformed_finger_pts_traj_rs = {} for finger_lr in 'lr': flr2new_transformed_finger_pts_traj_rs[ finger_lr] = reg.f.transform_points( np.concatenate( flr2demo_finger_pts_traj_rs[finger_lr], axis=0)).reshape((-1, 4, 3)) handles.extend( sim_util.draw_finger_pts_traj( self.sim, flr2new_transformed_finger_pts_traj_rs, (0, 1, 1))) handles.append( self.sim.env.drawlinestrip( test_aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (0, 0, 1))) flr2test_finger_pts_traj = sim_util.get_finger_pts_traj( self.sim.robot, lr, full_traj) handles.extend( sim_util.draw_finger_pts_traj(self.sim, flr2test_finger_pts_traj, (0, 0, 1))) self.sim.viewer.Step() return test_aug_traj
def transfer(self, reg, demo, plotting=False): handles = [] if plotting: demo_cloud = demo.scene_state.cloud test_cloud = reg.test_scene_state.cloud demo_color = demo.scene_state.color test_color = reg.test_scene_state.color handles.append( self.sim.env.plot3( demo_cloud[:, :3], 2, demo_color if demo_color is not None else (1, 0, 0))) handles.append( self.sim.env.plot3( test_cloud[:, :3], 2, test_color if test_color is not None else (0, 0, 1))) self.sim.viewer.Step() active_lr = "" for lr in 'lr': if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved( demo.aug_traj.lr2arm_traj[lr]): active_lr += lr _, timesteps_rs = sim_util.unif_resample( np.c_[(1. / settings.JOINT_LENGTH_PER_STEP) * np.concatenate( [demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1 ), (1. / settings.FINGER_CLOSE_RATE) * np.concatenate( [demo.aug_traj.lr2finger_traj[lr] for lr in active_lr], axis=1)], 1.) demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs) if self.init_trajectory_transferer: warm_init_traj = self.init_trajectory_transferer.transfer( reg, demo, plotting=plotting) manip_name = "" ee_link_names = [] transformed_ee_trajs_rs = [] init_traj = np.zeros((len(timesteps_rs), 0)) for lr in active_lr: arm_name = {"l": "leftarm", "r": "rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame" % lr if manip_name: manip_name += "+" manip_name += arm_name ee_link_names.append(ee_link_name) if self.init_trajectory_transferer: init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr]] else: init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr]] transformed_ee_traj_rs = reg.f.transform_hmats( demo_aug_traj_rs.lr2ee_traj[lr]) transformed_ee_trajs_rs.append(transformed_ee_traj_rs) if plotting: handles.append( self.sim.env.drawlinestrip( demo.aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (1, 0, 0))) handles.append( self.sim.env.drawlinestrip( demo_aug_traj_rs.lr2ee_traj[lr][:, :3, 3], 2, (1, 1, 0))) handles.append( self.sim.env.drawlinestrip( transformed_ee_traj_rs[:, :3, 3], 2, (0, 1, 0))) self.sim.viewer.Step() if not self.init_trajectory_transferer: # modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do) dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name) joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint" % lr) init_traj[:, dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits( [joint_ind])[0][0] print "planning pose trajectory following" test_traj, obj_value, pose_errs = planning.plan_follow_trajs( self.sim.robot, manip_name, ee_link_names, transformed_ee_trajs_rs, init_traj, start_fixed=False, use_collision_cost=self.use_collision_cost, beta_pos=self.beta_pos, beta_rot=self.beta_rot) # the finger trajectory is the same for the demo and the test trajectory for lr in active_lr: finger_name = "%s_gripper_l_finger_joint" % lr manip_name += "+" + finger_name test_traj = np.c_[test_traj, demo_aug_traj_rs.lr2finger_traj[lr]] full_traj = (test_traj, sim_util.dof_inds_from_name(self.sim.robot, manip_name)) test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj( self.sim.robot, full_traj, lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj, lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj) if plotting: for lr in active_lr: handles.append( self.sim.env.drawlinestrip( test_aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (0, 0, 1))) self.sim.viewer.Step() return test_aug_traj
def plan_follow_trajs(robot, manip_name, manip, xyz_target, starting_config): #global n_steps, Final_pose, manip, ideal_config, ideal_config_vanilla n_steps = 100 ideal_config = [-1.0, -1, -1, -1.7, 1.7, 0, 0] #quat_target = [1,0,0,0] # wxyz robot.SetDOFValues(starting_config, manip.GetArmIndices()) # t = manip.GetEndEffectorTransform() # quat_target = [x for x in (list(Quaternion(matrix=t)))] #quat_target = list(transform2quat(manip.GetEndEffectorTransform())) armids = list(manip.GetArmIndices()) #get arm indices links = robot.GetLinks() link_idx = links.index(robot.GetLink('r_gripper_palm_link')) linkstrans = robot.GetLinkTransformations() xyz_target1 = list(linkstrans[link_idx][:3][:, 3]) #get xyz of specific link quat_target1 = list(Quaternion(matrix=linkstrans[link_idx])) #computing initial trajectory #goal_config = iksolver(ideal_config) init_traj = interpolate(starting_config, ideal_config, n_steps) #filling in request dictionary request = { "basic_info": { "n_steps": n_steps, "manip": manip_name, #"start_fixed" : True }, "costs": [ { "type": "joint_vel", #"params": {"coeffs" : [gamma/(n_steps-1)]} "params": { "coeffs": [1] } }, ], "constraints": [ #{ # "type" : "pose", # "params" : { # "xyz" : xyz_target1, # "wxyz" : quat_target1, # "link": "r_gripper_palm_link", # "timestep" : n_steps-1, # "rot_coeffs" : [2,2,2], # "pos_coeffs" : [5,5,5] # } # # "type" : "joint", # joint-space target # # "params" : {"vals" : Final_pose.tolist() } # length of vals = # dofs of manip #}, # { # "type" : "pose", # "params" : { # "xyz" : xyz_wrist_target, # "wxyz" : quat_target, # "link": "r_gripper_r_link", # "timestep" : n_steps-1, # "rot_coeffs" : [0,0,0], # "pos_coeffs" : [1,1,1] # } # } ], "init_info": { "type": "given_traj", "data": init_traj.tolist() #"type": "straight_line", #"enpoint": init_joint_target.tolist() } } for t in range(n_steps): pose_constraint = { "type": "pose", "params": { "xyz": xyz_target1, "wxyz": quat_target1, "link": "r_gripper_palm_link", "timestep": t, "rot_coeffs": [2, 2, 2], "pos_coeffs": [5, 5, 5] } } request["constraints"].append(pose_constraint) #robot.SetDOFValues(starting_config, manip.GetArmIndices()) s = json.dumps(request) # with openravepy.RobotStateSaver(robot): # with util.suppress_stdout(): prob = trajoptpy.ConstructProblem( s, robot.GetEnv()) # create object that stores optimization problem cost_fn = lambda x: cost_distance_bet_deltas(x, coeff=1) for t in range(n_steps): prob.AddCost(cost_fn, [(t, j) for j in range(7)], "table%i" % t) print "optimizing prob to get result." result = trajoptpy.OptimizeProblem(prob) # do optimization print "done optimizing prob to get result." traj = result.GetTraj() dof_inds = sim_util.dof_inds_from_name(robot, manip_name) sim_util.unwrap_in_place(traj, dof_inds) print " " print "Resulting Traj: " print traj return traj
def setup_lfd_environment_sim(args): actions = h5py.File(args.eval.actionfile, 'r') init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment( actions, args.eval.fake_data_segment, args.eval.fake_data_transform) table_height = init_rope_xyz[:, 2].mean() - .02 sim_objs = [] sim_objs.append( XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append(BoxSimulationObject( "table", [1, 0, table_height + (-.1 + .01)], [.85, .85, .1], dynamic=False)) print 'Setting up lfd environment' sim = DynamicRopeSimulationRobotWorld() world = sim sim.add_objects(sim_objs) if args.eval.ground_truth: lfd_env = GroundTruthRopeLfdEnvironment( sim, world, upsample=args.eval.upsample, upsample_rad=args.eval.upsample_rad, downsample_size=args.eval.downsample_size) else: lfd_env = LfdEnvironment( sim, world, downsample_size=args.eval.downsample_size) dof_inds = sim_util.dof_inds_from_name( sim.robot, '+'.join(init_joint_names)) values, dof_inds = zip( *[(value, dof_ind) for value, dof_ind in zip(init_joint_values, dof_inds) if dof_ind != -1]) # this also sets the torso (torso_lift_joint) to the height in the data sim.robot.SetDOFValues(values, dof_inds) sim_util.reset_arms_to_side(sim) if args.animation: viewer = trajoptpy.GetViewer(sim.env) if os.path.isfile(args.window_prop_file) and os.path.isfile( args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: viewer.SetWindowProp(*window_prop) viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" viewer.Idle() print "saving window and camera properties" try: window_prop = viewer.GetWindowProp() camera_matrix = viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." viewer.Step() if args.eval.dof_limits_factor != 1.0: assert 0 < args.eval.dof_limits_factor and args.eval.dof_limits_factor <= 1.0 active_dof_indices = sim.robot.GetActiveDOFIndices() active_dof_limits = sim.robot.GetActiveDOFLimits() for lr in 'lr': manip_name = {"l": "leftarm", "r": "rightarm"}[lr] dof_inds = sim.robot.GetManipulator(manip_name).GetArmIndices() limits = np.asarray(sim.robot.GetDOFLimits(dof_inds)) limits_mean = limits.mean(axis=0) limits_width = np.diff(limits, axis=0) new_limits = limits_mean + args.eval.dof_limits_factor * \ np.r_[-limits_width / 2.0, limits_width / 2.0] for i, ind in enumerate(dof_inds): active_dof_limits[0][ active_dof_indices.tolist().index(ind)] = new_limits[0, i] active_dof_limits[1][ active_dof_indices.tolist().index(ind)] = new_limits[1, i] sim.robot.SetDOFLimits(active_dof_limits[0], active_dof_limits[1]) return lfd_env, sim
def pre_motion_traj(premotion_starting_config, attempt_starting_config, \ joint_vel_coeff=20, collision_coeff=20, init_traj_input=None): #global n_steps, Final_pose, manip, ideal_config, ideal_config_vanilla n_steps = 30 robot.SetDOFValues(attempt_starting_config, manip.GetArmIndices()) armids = list(manip.GetArmIndices()) #get arm indices links = robot.GetLinks() link_idx = links.index(robot.GetLink('r_gripper_palm_link')) linkstrans = robot.GetLinkTransformations() xyz_target1 = list(linkstrans[link_idx][:3][:,3]) #get xyz of specific link quat_target1 = list(Quaternion(matrix=linkstrans[link_idx])) robot.SetDOFValues(premotion_starting_config, manip.GetArmIndices()) init_traj = interpolate(premotion_starting_config, attempt_starting_config, n_steps) #filling in request dictionary request = { "basic_info" : { "n_steps" : n_steps, "manip" : "rightarm", "start_fixed" : True }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : [joint_vel_coeff]} }, { "type" : "collision", "params" : { "coeffs" : [collision_coeff], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps } }, ], "constraints" : [{"type": "pose", "params" : { "xyz" : xyz_target1, "wxyz" : quat_target1, "link": "r_gripper_palm_link", "timestep" : n_steps-1, "rot_coeffs" : [2,2,2], "pos_coeffs" : [5,5,5] }}, { "type" : "joint", # joint-space target "params" : {"vals" : attempt_starting_config.tolist() } # length of vals = # dofs of manip }], "init_info" : { #"type": "given_traj", #"data": init_traj.tolist() #"type": "straight_line", #"endpoint": attempt_starting_config.tolist() #"type": "stationary", } } if init_traj_input is None: request["init_info"]["type"] = "straight_line" request["init_info"]["endpoint"] = attempt_starting_config.tolist() else: request["init_info"]["type"] = "given_traj" request["init_info"]["data"] = init_traj_input.tolist() s = json.dumps(request) with openravepy.RobotStateSaver(robot): with util.suppress_stdout(): prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj() dof_inds = sim_util.dof_inds_from_name(robot, manip_name) sim_util.unwrap_in_place(traj, dof_inds) return traj, result
def setup_lfd_environment_sim(args): actions = h5py.File(args.eval.actionfile, 'r') init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment( actions, args.eval.fake_data_segment, args.eval.fake_data_transform) table_height = init_rope_xyz[:, 2].mean() - .02 sim_objs = [] sim_objs.append( XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append( BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)], [.85, .85, .1], dynamic=False)) print 'Setting up lfd environment' sim = DynamicRopeSimulationRobotWorld() world = sim sim.add_objects(sim_objs) if args.eval.ground_truth: lfd_env = GroundTruthRopeLfdEnvironment( sim, world, upsample=args.eval.upsample, upsample_rad=args.eval.upsample_rad, downsample_size=args.eval.downsample_size) else: lfd_env = LfdEnvironment(sim, world, downsample_size=args.eval.downsample_size) dof_inds = sim_util.dof_inds_from_name(sim.robot, '+'.join(init_joint_names)) values, dof_inds = zip( *[(value, dof_ind) for value, dof_ind in zip(init_joint_values, dof_inds) if dof_ind != -1]) # this also sets the torso (torso_lift_joint) to the height in the data sim.robot.SetDOFValues(values, dof_inds) sim_util.reset_arms_to_side(sim) if args.animation: viewer = trajoptpy.GetViewer(sim.env) if os.path.isfile(args.window_prop_file) and os.path.isfile( args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: viewer.SetWindowProp(*window_prop) viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" viewer.Idle() print "saving window and camera properties" try: window_prop = viewer.GetWindowProp() camera_matrix = viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." viewer.Step() if args.eval.dof_limits_factor != 1.0: assert 0 < args.eval.dof_limits_factor and args.eval.dof_limits_factor <= 1.0 active_dof_indices = sim.robot.GetActiveDOFIndices() active_dof_limits = sim.robot.GetActiveDOFLimits() for lr in 'lr': manip_name = {"l": "leftarm", "r": "rightarm"}[lr] dof_inds = sim.robot.GetManipulator(manip_name).GetArmIndices() limits = np.asarray(sim.robot.GetDOFLimits(dof_inds)) limits_mean = limits.mean(axis=0) limits_width = np.diff(limits, axis=0) new_limits = limits_mean + args.eval.dof_limits_factor * \ np.r_[-limits_width / 2.0, limits_width / 2.0] for i, ind in enumerate(dof_inds): active_dof_limits[0][active_dof_indices.tolist().index( ind)] = new_limits[0, i] active_dof_limits[1][active_dof_indices.tolist().index( ind)] = new_limits[1, i] sim.robot.SetDOFLimits(active_dof_limits[0], active_dof_limits[1]) return lfd_env, sim
if TRAJ_FILE is not None and osp.dirname( osp.realpath(__file__)) not in TRAJ_FILE: TRAJ_FILE = osp.join(CACHE_DIR, TRAJ_FILE) init() if TRAJ_FILE is None: traj = attempt_traj() traj_fname = TRAJ_FILE_PREFIX + "_" + get_time_stamp() + ".pkl" pickle.dump(traj, open(traj_fname, 'wb')) else: traj = pickle.load(open(TRAJ_FILE, 'rb')) traj = traj[:16] traj_back = np.copy(traj[::-1]) robot = globalvars.or_sim.env.GetRobots()[0] manip_name = "rightarm" dof_inds = sim_util.dof_inds_from_name(robot, manip_name) #sim_util.unwrap_in_place(traj, dof_inds) print "visualizing attempt trajectory.." lr = 'r' # Execute task with just right gripper robot = globalvars.or_sim.env.GetRobots()[0] manip = robot.GetActiveManipulator() fulltraj = traj, manip.GetArmIndices().tolist() fulltraj_back = traj_back, manip.GetArmIndices().tolist() wait_until_done_moving(globalvars.pr2) if globalvars.pr2 is not None: pr2_follow_traj(globalvars.pr2, fulltraj, speed_factor=4.0) pr2_follow_traj(globalvars.pr2, fulltraj_back, speed_factor=2.0) time.sleep(4) pr2_follow_traj(globalvars.pr2, fulltraj, speed_factor=4.0)
def joint_fit_tps_follow_finger_pts_trajs(robot, manip_name, flr2finger_link_names, flr2finger_rel_pts, flr2old_finger_pts_trajs, old_traj, f, closing_pts=None, no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None, alpha=settings.ALPHA, beta_pos=settings.BETA_POS, gamma=settings.GAMMA): orig_dof_inds = robot.GetActiveDOFIndices() orig_dof_vals = robot.GetDOFValues() n_steps = old_traj.shape[0] dof_inds = sim_util.dof_inds_from_name(robot, manip_name) assert old_traj.shape[1] == len(dof_inds) for flr2old_finger_pts_traj in flr2old_finger_pts_trajs: for old_finger_pts_traj in flr2old_finger_pts_traj.values(): assert len(old_finger_pts_traj)== n_steps assert len(flr2finger_link_names) == len(flr2old_finger_pts_trajs) # expand these (n,d) = f.x_na.shape bend_coefs = np.ones(d) * f.bend_coef if np.isscalar(f.bend_coef) else f.bend_coef rot_coefs = np.ones(d) * f.rot_coef if np.isscalar(f.rot_coef) else f.rot_coef if f.wt_n is None: wt_n = np.ones(n) else: wt_n = f.wt_n if wt_n.ndim == 1: wt_n = wt_n[:,None] if wt_n.shape[1] == 1: wt_n = np.tile(wt_n, (1,d)) if no_collision_cost_first: init_traj, _, (N, init_z) , _, _ = joint_fit_tps_follow_finger_pts_trajs(robot, manip_name, flr2finger_link_names, flr2finger_rel_pts, flr2old_finger_pts_trajs, old_traj, f, closing_pts=closing_pts, no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits, alpha=alpha, beta_pos=beta_pos, gamma=gamma) else: init_traj = old_traj.copy() N = f.N init_z = f.z if start_fixed: init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]] sim_util.unwrap_in_place(init_traj, dof_inds) init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:] request = { "basic_info" : { "n_steps" : n_steps, "m_ext" : n, "n_ext" : d, "manip" : manip_name, "start_fixed" : start_fixed }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : [gamma/(n_steps-1)]} }, { "type" : "tps", "name" : "tps", "params" : {"x_na" : [row.tolist() for row in f.x_na], "y_ng" : [row.tolist() for row in f.y_ng], "bend_coefs" : bend_coefs.tolist(), "rot_coefs" : rot_coefs.tolist(), "wt_n" : [row.tolist() for row in wt_n], "N" : [row.tolist() for row in N], "alpha" : alpha, } } ], "constraints" : [ ], "init_info" : { "type":"given_traj", "data":[x.tolist() for x in init_traj], "data_ext":[row.tolist() for row in init_z] } } if use_collision_cost: request["costs"].append( { "type" : "collision", "params" : { "continuous" : True, "coeffs" : [1000], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps } }) if joint_vel_limits is not None: request["constraints"].append( { "type" : "joint_vel_limits", "params": {"vals" : joint_vel_limits, "first_step" : 0, "last_step" : n_steps-1 } }) if closing_pts is not None: request["costs"].append( { "type":"tps_jac_orth", "params": { "tps_cost_name":"tps", "pts":closing_pts.tolist(), "coeffs":[10.0]*len(closing_pts), } }) for (flr2finger_link_name, flr2old_finger_pts_traj) in zip(flr2finger_link_names, flr2old_finger_pts_trajs): for finger_lr, finger_link_name in flr2finger_link_name.items(): finger_rel_pts = flr2finger_rel_pts[finger_lr] old_finger_pts_traj = flr2old_finger_pts_traj[finger_lr] for (i_step, old_finger_pts) in enumerate(old_finger_pts_traj): if start_fixed and i_step == 0: continue request["costs"].append( {"type":"tps_rel_pts", "params":{ "tps_cost_name":"tps", "src_xyzs":old_finger_pts.tolist(), "rel_xyzs":finger_rel_pts.tolist(), "link":finger_link_name, "timestep":i_step, "pos_coeffs":[np.sqrt(beta_pos/n_steps)]*4, } }) s = json.dumps(request) with openravepy.RobotStateSaver(robot): with util.suppress_stdout(): prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj() f.z = result.GetExt() theta = N.dot(f.z) f.trans_g = theta[0,:] f.lin_ag = theta[1:d+1,:] f.w_ng = theta[d+1:] tps_rel_pts_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "tps_rel_pts"]) tps_rel_pts_err = [] with openravepy.RobotStateSaver(robot): for (flr2finger_link_name, flr2old_finger_pts_traj) in zip(flr2finger_link_names, flr2old_finger_pts_trajs): for finger_lr, finger_link_name in flr2finger_link_name.items(): finger_link = robot.GetLink(finger_link_name) finger_rel_pts = flr2finger_rel_pts[finger_lr] old_finger_pts_traj = flr2old_finger_pts_traj[finger_lr] for (i_step, old_finger_pts) in enumerate(old_finger_pts_traj): if start_fixed and i_step == 0: continue robot.SetDOFValues(traj[i_step], dof_inds) new_hmat = finger_link.GetTransform() tps_rel_pts_err.append(f.transform_points(old_finger_pts) - (new_hmat[:3,3][None,:] + finger_rel_pts.dot(new_hmat[:3,:3].T))) tps_rel_pts_err = np.concatenate(tps_rel_pts_err, axis=0) tps_rel_pts_costs2 = (beta_pos/n_steps) * np.square(tps_rel_pts_err).sum() # TODO don't square n_steps tps_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "tps"]) tps_cost2 = alpha * f.get_objective().sum() matching_err = f.transform_points(f.x_na) - f.y_ng joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"]) joint_vel_err = np.diff(traj, axis=0) joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum() sim_util.unwrap_in_place(traj, dof_inds) joint_vel_err = np.diff(traj, axis=0) collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type] if len(collision_costs) > 0: collision_err = np.asarray(collision_costs) collision_costs = np.sum(collision_costs) tps_jac_orth_cost = [cost_val for (cost_type, cost_val) in result.GetCosts() if "tps_jac_orth" in cost_type] if len(tps_jac_orth_cost) > 0: tps_jac_orth_cost = np.sum(tps_jac_orth_cost) f_jacs = f.compute_jacobian(closing_pts) tps_jac_orth_err = [] for jac in f_jacs: tps_jac_orth_err.extend((jac.dot(jac.T) - np.eye(3)).flatten()) tps_jac_orth_err = np.asarray(tps_jac_orth_err) tps_jac_orth_cost2 = np.square( 10.0 * tps_jac_orth_err ).sum() obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()]) print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed") print "{:>15} | {:>10}".format("COSTS", "-"*23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2) print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps", tps_cost, tps_cost2) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-") print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps_rel_pts(s)", tps_rel_pts_costs, tps_rel_pts_costs2) if np.isscalar(tps_jac_orth_cost): print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps_jac_orth", tps_jac_orth_cost, tps_jac_orth_cost2) print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-") print "" print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max") print "{:>15} | {:>10}".format("ERRORS", "-"*23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max())) print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps (matching)", np.abs(matching_err).min(), np.abs(matching_err).max()) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps_rel_pts(s)", np.abs(tps_rel_pts_err).min(), np.abs(tps_rel_pts_err).max()) if np.isscalar(tps_jac_orth_cost): print "{:>15} | {:>10,.4} | {:>10,.4}".format("tps_jac_orth", np.abs(tps_jac_orth_err).min(), np.abs(tps_jac_orth_err).max()) print "" # make sure this function doesn't change state of the robot assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices()) assert not np.any(orig_dof_vals - robot.GetDOFValues()) return traj, obj_value, tps_rel_pts_costs, tps_cost
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None, beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA): orig_dof_inds = robot.GetActiveDOFIndices() orig_dof_vals = robot.GetDOFValues() n_steps = len(ee_trajs[0]) dof_inds = sim_util.dof_inds_from_name(robot, manip_name) assert old_traj.shape[0] == n_steps assert old_traj.shape[1] == len(dof_inds) assert len(ee_link_names) == len(ee_trajs) if no_collision_cost_first: init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits, beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma) else: init_traj = old_traj.copy() if start_fixed: init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]] sim_util.unwrap_in_place(init_traj, dof_inds) init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:] request = { "basic_info" : { "n_steps" : n_steps, "manip" : manip_name, "start_fixed" : start_fixed }, "costs" : [ { "type" : "joint_vel", "params": {"coeffs" : [gamma/(n_steps-1)]} }, ], "constraints" : [ ], "init_info" : { "type":"given_traj", "data":[x.tolist() for x in init_traj] } } if use_collision_cost: request["costs"].append( { "type" : "collision", "params" : { "continuous" : True, "coeffs" : [1000], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps "dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps } }) if joint_vel_limits is not None: request["constraints"].append( { "type" : "joint_vel_limits", "params": {"vals" : joint_vel_limits, "first_step" : 0, "last_step" : n_steps-1 } }) for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs): poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj] for (i_step,pose) in enumerate(poses): if start_fixed and i_step == 0: continue request["costs"].append( {"type":"pose", "params":{ "xyz":pose[4:7].tolist(), "wxyz":pose[0:4].tolist(), "link":ee_link_name, "timestep":i_step, "pos_coeffs":[np.sqrt(beta_pos/n_steps)]*3, "rot_coeffs":[np.sqrt(beta_rot/n_steps)]*3 } }) s = json.dumps(request) with openravepy.RobotStateSaver(robot): orig_dof_vals with util.suppress_stdout(): prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj() pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"]) pose_err = [] with openravepy.RobotStateSaver(robot): for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs): ee_link = robot.GetLink(ee_link_name) for (i_step, hmat) in enumerate(ee_traj): if start_fixed and i_step == 0: continue robot.SetDOFValues(traj[i_step], dof_inds) new_hmat = ee_link.GetTransform() pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat))) pose_err = np.asarray(pose_err) pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum() joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"]) joint_vel_err = np.diff(traj, axis=0) joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum() sim_util.unwrap_in_place(traj, dof_inds) joint_vel_err = np.diff(traj, axis=0) collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type] if len(collision_costs) > 0: collision_err = np.asarray(collision_costs) collision_costs = np.sum(collision_costs) obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()]) print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed") print "{:>15} | {:>10}".format("COSTS", "-"*23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-") print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2) print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-") print "" print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max") print "{:>15} | {:>10}".format("ERRORS", "-"*23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max())) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max()) print "" # make sure this function doesn't change state of the robot assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices()) assert not np.any(orig_dof_vals - robot.GetDOFValues()) return traj, obj_value, pose_costs
def transfer(self, reg, demo, plotting=False): handles = [] if plotting: demo_cloud = demo.scene_state.cloud test_cloud = reg.test_scene_state.cloud demo_color = demo.scene_state.color test_color = reg.test_scene_state.color handles.append(self.sim.env.plot3(demo_cloud[:,:3], 2, demo_color if demo_color is not None else (1,0,0))) handles.append(self.sim.env.plot3(test_cloud[:,:3], 2, test_color if test_color is not None else (0,0,1))) self.sim.viewer.Step() active_lr = "" for lr in 'lr': if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved(demo.aug_traj.lr2arm_traj[lr]): active_lr += lr _, timesteps_rs = sim_util.unif_resample(np.c_[(1./settings.JOINT_LENGTH_PER_STEP) * np.concatenate([demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1), (1./settings.FINGER_CLOSE_RATE) * np.concatenate([demo.aug_traj.lr2finger_traj[lr] for lr in active_lr], axis=1)], 1.) demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs) if self.init_trajectory_transferer: warm_init_traj = self.init_trajectory_transferer.transfer(reg, demo, plotting=plotting) manip_name = "" ee_link_names = [] transformed_ee_trajs_rs = [] init_traj = np.zeros((len(timesteps_rs),0)) for lr in active_lr: arm_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr if manip_name: manip_name += "+" manip_name += arm_name ee_link_names.append(ee_link_name) if self.init_trajectory_transferer: init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr]] else: init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr]] transformed_ee_traj_rs = reg.f.transform_hmats(demo_aug_traj_rs.lr2ee_traj[lr]) transformed_ee_trajs_rs.append(transformed_ee_traj_rs) if plotting: handles.append(self.sim.env.drawlinestrip(demo.aug_traj.lr2ee_traj[lr][:,:3,3], 2, (1,0,0))) handles.append(self.sim.env.drawlinestrip(demo_aug_traj_rs.lr2ee_traj[lr][:,:3,3], 2, (1,1,0))) handles.append(self.sim.env.drawlinestrip(transformed_ee_traj_rs[:,:3,3], 2, (0,1,0))) self.sim.viewer.Step() if not self.init_trajectory_transferer: # modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do) dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name) joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint"%lr) init_traj[:,dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits([joint_ind])[0][0] print "planning pose trajectory following" test_traj, obj_value, pose_errs = planning.plan_follow_trajs(self.sim.robot, manip_name, ee_link_names, transformed_ee_trajs_rs, init_traj, start_fixed=False, use_collision_cost=self.use_collision_cost, beta_pos=self.beta_pos, beta_rot=self.beta_rot) # the finger trajectory is the same for the demo and the test trajectory for lr in active_lr: finger_name = "%s_gripper_l_finger_joint"%lr manip_name += "+" + finger_name test_traj = np.c_[test_traj, demo_aug_traj_rs.lr2finger_traj[lr]] full_traj = (test_traj, sim_util.dof_inds_from_name(self.sim.robot, manip_name)) test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj(self.sim.robot, full_traj, lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj, lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj) if plotting: for lr in active_lr: handles.append(self.sim.env.drawlinestrip(test_aug_traj.lr2ee_traj[lr][:,:3,3], 2, (0,0,1))) self.sim.viewer.Step() return test_aug_traj
def get_move_traj(t_start, t_end, R_start, R_end, start_fixed): hmat_start = np.r_[np.c_[R_start, t_start], np.c_[0,0,0,1]] hmat_end = np.r_[np.c_[R_end, t_end], np.c_[0,0,0,1]] new_hmats = np.asarray(resampling.interp_hmats(np.arange(n_steps), np.r_[0, n_steps-1], [hmat_start, hmat_end])) dof_vals = sim.robot.GetManipulator(manip_name).GetArmDOFValues() old_traj = np.tile(dof_vals, (n_steps,1)) traj, _, _ = planning.plan_follow_traj(sim.robot, manip_name, ee_link, new_hmats, old_traj, start_fixed=start_fixed, beta_rot=10000.0) return traj pick_pos = rope_pos0 + .1 * (rope_pos1 - rope_pos0) drop_pos = rope_pos3 + .1 * (rope_pos2 - rope_pos3) + np.r_[0, .2, 0] pick_R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) drop_R = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]) move_height = .2 dof_inds = sim_util.dof_inds_from_name(sim.robot, manip_name) traj = get_move_traj(pick_pos + np.r_[0,0,move_height], pick_pos, pick_R, pick_R, False) sim.execute_trajectory((traj, dof_inds)) sim.close_gripper(lr) traj = get_move_traj(pick_pos, pick_pos + np.r_[0,0,move_height], pick_R, pick_R, True) sim.execute_trajectory((traj, dof_inds)) traj = get_move_traj(pick_pos + np.r_[0,0,move_height], drop_pos + np.r_[0,0,move_height], pick_R, drop_R, True) sim.execute_trajectory((traj, dof_inds)) traj = get_move_traj(drop_pos + np.r_[0,0,move_height], drop_pos, drop_R, drop_R, True) sim.execute_trajectory((traj, dof_inds)) sim.open_gripper(lr)
def plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, no_collision_cost_first=False, use_collision_cost=True, start_fixed=False, joint_vel_limits=None, beta_pos=settings.BETA_POS, beta_rot=settings.BETA_ROT, gamma=settings.GAMMA, pose_weights=1, grasp_cup=False, R=0.02, D=0.035, cup_xyz=None): orig_dof_inds = robot.GetActiveDOFIndices() orig_dof_vals = robot.GetDOFValues() n_steps = len(ee_trajs[0]) dof_inds = sim_util.dof_inds_from_name(robot, manip_name) assert old_traj.shape[0] == n_steps assert old_traj.shape[1] == len(dof_inds) assert len(ee_link_names) == len(ee_trajs) if no_collision_cost_first: init_traj, _, _ = plan_follow_trajs(robot, manip_name, ee_link_names, ee_trajs, old_traj, no_collision_cost_first=False, use_collision_cost=False, start_fixed=start_fixed, joint_vel_limits=joint_vel_limits, beta_pos = beta_pos, beta_rot = beta_rot, gamma = gamma) else: init_traj = old_traj.copy() if start_fixed: init_traj = np.r_[robot.GetDOFValues(dof_inds)[None,:], init_traj[1:]] sim_util.unwrap_in_place(init_traj, dof_inds) init_traj += robot.GetDOFValues(dof_inds) - init_traj[0,:] request = { "basic_info" : { "n_steps" : n_steps, "manip" : manip_name, "start_fixed" : start_fixed }, "costs" : [ { "type" : "joint_vel", #"params": {"coeffs" : [gamma/(n_steps-1)]} "params": {"coeffs" : [1]} }, ], "constraints" : [ { "type" : "cart_vel", "name" : "cart_vel", "params" : { "max_displacement" : .02, "first_step" : 0, "last_step" : n_steps-1, #inclusive "link" : "r_gripper_tool_frame" } }, #{ # "type" : "collision", # "params" : { # "continuous" : True, # "coeffs" : [100]*(n_steps-3) + [0]*3, # "dist_pen" : [0.01] # } #} ], "init_info" : { "type":"given_traj", "data":[x.tolist() for x in init_traj] } } if use_collision_cost: request["costs"].append( { "type" : "collision", "params" : { "continuous" : True, "coeffs" : [20000], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps #"coeffs" : [0] * (len(ee_trajs[0]) - 3) + [100]*3, "dist_pen" : [0.025] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps } }) if joint_vel_limits is not None: request["constraints"].append( { "type" : "joint_vel_limits", "params": {"vals" : joint_vel_limits, "first_step" : 0, "last_step" : n_steps-1 } }) #coeff_mult = [0.0] * (len(poses) - 1) + [100] #coeff_mult = np.linspace(1, 100, num=len(poses)) #coeff_mult_trans = np.logspace(-3, 1, num=len(ee_trajs[0])) #coeff_mult_rot = np.logspace(-3, 1, num=len(ee_trajs[0])) coeff_mult_trans = np.array([0] * (len(ee_trajs[0])-1) + [100]) coeff_mult_rot = np.array([0] * (len(ee_trajs[0])-1) + [100]) if not grasp_cup: #coeff_mult_trans = np.array([0.1] * (len(ee_trajs[0])-1) + [100]) #coeff_mult_rot = np.array([1] * len(ee_trajs[0])) for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs): poses = [openravepy.poseFromMatrix(hmat) for hmat in ee_traj] for (i_step,pose) in enumerate(poses): if start_fixed and i_step == 0: continue request["costs"].append( {"type":"pose", "params":{ "xyz":pose[4:7].tolist(), "wxyz":pose[0:4].tolist(), "link":ee_link_name, "timestep":i_step, #"pos_coeffs":list(pose_weights * [np.sqrt(beta_pos/n_steps)]*3), "pos_coeffs":[np.sqrt(beta_pos/n_steps) * coeff_mult_trans[i_step]]*3, #"rot_coeffs":list(pose_weights * [np.sqrt(beta_rot/n_steps)]*3) "rot_coeffs":[np.sqrt(beta_rot/n_steps) * coeff_mult_rot[i_step]]*3 } }) print "Cup XYZ:", cup_xyz s = json.dumps(request) with openravepy.RobotStateSaver(robot): with util.suppress_stdout(): prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem # for t in range(1,n_steps): # prob.AddCost(table_cost, [(t,j) for j in range(7)], "table%i"%t) if grasp_cup: tool_link = robot.GetLink("r_gripper_tool_frame") local_dir = np.array([0.,0.,1.]) manip = robot.GetManipulator(manip_name) arm_inds = manip.GetArmIndices() arm_joints = [robot.GetJointFromDOFIndex(ind) for ind in arm_inds] f = lambda x: gripper_tilt(x, robot, arm_inds, tool_link, local_dir) f2 = lambda x: gripper_tilt2(x, robot, arm_inds, tool_link, local_dir) dfdx = lambda x: gripper_tilt_dx(x, robot, arm_inds, tool_link, local_dir, arm_joints) #for t in xrange(1,n_steps): for t in xrange(n_steps-2,n_steps): prob.AddConstraint(f, dfdx, [(t,j) for j in xrange(7)], "EQ", "up%i"%t) #for t in xrange(1,n_steps-1): # prob.AddConstraint(f2, [(t,j) for j in xrange(7)], "EQ", "up%i"%t) #prob.AddConstraint(f2, [(n_steps-1,j) for j in xrange(7)], "EQ", "up%i"%(n_steps-1)) g = lambda x: gripper_around_cup(x, robot, arm_inds, tool_link, cup_xyz, R, D) prob.AddConstraint(g, [(n_steps-1,j) for j in xrange(7)], "EQ", "cup%i"%(n_steps-1)) result = trajoptpy.OptimizeProblem(prob) # do optimization print [(_name, viol) for (_name, viol) in result.GetConstraints() if viol > 0.0001] #assert [viol <= 1e-4 for (_name,viol) in result.GetConstraints()] traj = result.GetTraj() pose_costs = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "pose"]) pose_err = [] with openravepy.RobotStateSaver(robot): for (ee_link_name, ee_traj) in zip(ee_link_names, ee_trajs): ee_link = robot.GetLink(ee_link_name) for (i_step, hmat) in enumerate(ee_traj): if start_fixed and i_step == 0: continue robot.SetDOFValues(traj[i_step], dof_inds) new_hmat = ee_link.GetTransform() pose_err.append(openravepy.poseFromMatrix(mu.invertHmat(hmat).dot(new_hmat))) pose_err = np.asarray(pose_err) pose_costs2 = (beta_rot/n_steps) * np.square(pose_err[:,1:4]).sum() + (beta_pos/n_steps) * np.square(pose_err[:,4:7]).sum() pose_costs3 = (beta_rot/n_steps) * np.dot(coeff_mult_rot[np.newaxis,1:], np.square(pose_err[:,1:4])).sum() + (beta_pos/n_steps) * np.dot(coeff_mult_trans[np.newaxis,1:], np.square(pose_err[:,4:7])).sum() joint_vel_cost = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts() if cost_type == "joint_vel"]) joint_vel_err = np.diff(traj, axis=0) joint_vel_cost2 = (gamma/(n_steps-1)) * np.square(joint_vel_err).sum() sim_util.unwrap_in_place(traj, dof_inds) joint_vel_err = np.diff(traj, axis=0) collision_costs = [cost_val for (cost_type, cost_val) in result.GetCosts() if "collision" in cost_type] #collision_costs = [cost_val for (cost_type, cost_val) in result.GetConstraints() if "collision" in cost_type] if len(collision_costs) > 0: collision_err = np.asarray(collision_costs) collision_costs = np.sum(collision_costs) obj_value = np.sum([cost_val for (cost_type, cost_val) in result.GetCosts()]) print "{:>15} | {:>10} | {:>10}".format("", "trajopt", "computed") print "{:>15} | {:>10}".format("COSTS", "-"*23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel", joint_vel_cost, joint_vel_cost2) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10}".format("collision(s)", collision_costs, "-") print "{:>15} | {:>10,.4} | {:>10,.4}".format("pose(s)", pose_costs, pose_costs2) print "{:>15} | {:>10,.4} | {:>10}".format("total_obj", obj_value, "-") print "" print "{:>15} | {:>10} | {:>10}".format("", "abs min", "abs max") print "{:>15} | {:>10}".format("ERRORS", "-"*23) print "{:>15} | {:>10,.4} | {:>10,.4}".format("joint_vel (deg)", np.rad2deg(np.abs(joint_vel_err).min()), np.rad2deg(np.abs(joint_vel_err).max())) if np.isscalar(collision_costs): print "{:>15} | {:>10,.4} | {:>10,.4}".format("collision(s)", np.abs(-collision_err).min(), np.abs(-collision_err).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format("rot pose(s)", np.abs(pose_err[:,1:4]).min(), np.abs(pose_err[:,1:4]).max()) print "{:>15} | {:>10,.4} | {:>10,.4}".format("trans pose(s)", np.abs(pose_err[:,4:7]).min(), np.abs(pose_err[:,4:7]).max()) print "" # make sure this function doesn't change state of the robot assert not np.any(orig_dof_inds - robot.GetActiveDOFIndices()) assert not np.any(orig_dof_vals - robot.GetDOFValues()) rot_err = np.abs(pose_err[:,1:4]).sum() pos_err = np.abs(pose_err[:,4:7]).sum() print collision_err return traj, obj_value, (pose_costs3, np.abs(collision_err[:-1]).sum())