def get_finger_pts_traj(robot, lr, full_traj_or_ee_finger_traj): """ ee_traj = sim_util.get_ee_traj(robot, lr, arm_traj) flr2finger_pts_traj1 = get_finger_pts_traj(robot, lr, (ee_traj, finger_traj)) full_traj = sim_util.get_full_traj(robot, {lr:arm_traj}, {lr:finger_traj}) flr2finger_pts_traj2 = get_finger_pts_traj(robot, lr, full_traj) """ flr2finger_pts_traj = {} assert type(full_traj_or_ee_finger_traj) == tuple if full_traj_or_ee_finger_traj[0].ndim == 3: ee_traj, finger_traj = full_traj_or_ee_finger_traj assert len(ee_traj) == len(finger_traj) for finger_lr in 'lr': gripper_full_traj = get_full_traj(robot, {}, {lr:finger_traj}) rel_ee_traj = get_ee_traj(robot, lr, gripper_full_traj) rel_finger_traj = get_ee_traj(robot, lr, gripper_full_traj, ee_link_name_fmt="%s"+"_gripper_%s_finger_tip_link"%finger_lr) flr2finger_pts_traj[finger_lr] = [] for (world_from_ee, world_from_rel_ee, world_from_rel_finger) in zip(ee_traj, rel_ee_traj, rel_finger_traj): ee_from_finger = mu.invertHmat(world_from_rel_ee).dot(world_from_rel_finger) world_from_finger = world_from_ee.dot(ee_from_finger) finger_pts = world_from_finger[:3,3] + get_finger_rel_pts(finger_lr).dot(world_from_finger[:3,:3].T) flr2finger_pts_traj[finger_lr].append(finger_pts) flr2finger_pts_traj[finger_lr] = np.asarray(flr2finger_pts_traj[finger_lr]) else: full_traj = full_traj_or_ee_finger_traj for finger_lr in 'lr': finger_traj = get_ee_traj(robot, lr, full_traj, ee_link_name_fmt="%s"+"_gripper_%s_finger_tip_link"%finger_lr) flr2finger_pts_traj[finger_lr] = [] for world_from_finger in finger_traj: flr2finger_pts_traj[finger_lr].append(world_from_finger[:3,3] + get_finger_rel_pts(finger_lr).dot(world_from_finger[:3,:3].T)) flr2finger_pts_traj[finger_lr] = np.asarray(flr2finger_pts_traj[finger_lr]) return flr2finger_pts_traj
def get_finger_pts_traj(robot, lr, full_traj_or_ee_finger_traj): """ ee_traj = sim_util.get_ee_traj(robot, lr, arm_traj) flr2finger_pts_traj1 = get_finger_pts_traj(robot, lr, (ee_traj, finger_traj)) full_traj = sim_util.get_full_traj(robot, {lr:arm_traj}, {lr:finger_traj}) flr2finger_pts_traj2 = get_finger_pts_traj(robot, lr, full_traj) """ flr2finger_pts_traj = {} assert type(full_traj_or_ee_finger_traj) == tuple if full_traj_or_ee_finger_traj[0].ndim == 3: ee_traj, finger_traj = full_traj_or_ee_finger_traj assert len(ee_traj) == len(finger_traj) for finger_lr in 'lr': gripper_full_traj = get_full_traj(robot, {}, {lr: finger_traj}) rel_ee_traj = get_ee_traj(robot, lr, gripper_full_traj) rel_finger_traj = get_ee_traj( robot, lr, gripper_full_traj, ee_link_name_fmt="%s" + "_gripper_%s_finger_tip_link" % finger_lr) flr2finger_pts_traj[finger_lr] = [] for (world_from_ee, world_from_rel_ee, world_from_rel_finger) in zip(ee_traj, rel_ee_traj, rel_finger_traj): ee_from_finger = mu.invertHmat(world_from_rel_ee).dot( world_from_rel_finger) world_from_finger = world_from_ee.dot(ee_from_finger) finger_pts = world_from_finger[:3, 3] + get_finger_rel_pts( finger_lr).dot(world_from_finger[:3, :3].T) flr2finger_pts_traj[finger_lr].append(finger_pts) flr2finger_pts_traj[finger_lr] = np.asarray( flr2finger_pts_traj[finger_lr]) else: full_traj = full_traj_or_ee_finger_traj for finger_lr in 'lr': finger_traj = get_ee_traj( robot, lr, full_traj, ee_link_name_fmt="%s" + "_gripper_%s_finger_tip_link" % finger_lr) flr2finger_pts_traj[finger_lr] = [] for world_from_finger in finger_traj: flr2finger_pts_traj[finger_lr].append( world_from_finger[:3, 3] + get_finger_rel_pts( finger_lr).dot(world_from_finger[:3, :3].T)) flr2finger_pts_traj[finger_lr] = np.asarray( flr2finger_pts_traj[finger_lr]) return flr2finger_pts_traj
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 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 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())