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())
