def traj_collisions(sim_env, full_traj, collision_dist_threshold, upsample=0):
"""
Returns the set of collisions.
manip = Manipulator or list of indices
"""
traj, dof_inds = full_traj
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
if upsample > 0:
traj_up = mu.interp2d(np.linspace(0, 1, upsample),
np.linspace(0, 1, len(traj)), traj)
else:
traj_up = traj
cc = trajoptpy.GetCollisionChecker(sim_env.env)
with openravepy.RobotStateSaver(sim_env.robot):
sim_env.robot.SetActiveDOFs(dof_inds)
col_times = []
for (i, row) in enumerate(traj_up):
sim_env.robot.SetActiveDOFValues(row)
col_now = cc.BodyVsAll(sim_env.robot)
#with util.suppress_stdout():
# col_now2 = cc.PlotCollisionGeometry()
col_now = [
cn for cn in col_now
if cn.GetDistance() < collision_dist_threshold
]
if col_now:
#print [cn.GetDistance() for cn in col_now]
col_times.append(i)
#print "trajopt.CollisionChecker: ", len(col_now)
#print col_now2
return col_times
def traj_collisions(sim_env, full_traj, collision_dist_threshold, upsample=0):
"""
Returns the set of collisions.
manip = Manipulator or list of indices
"""
traj, dof_inds = full_traj
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
if upsample > 0:
traj_up = mu.interp2d(np.linspace(0,1,upsample), np.linspace(0,1,len(traj)), traj)
else:
traj_up = traj
cc = trajoptpy.GetCollisionChecker(sim_env.env)
with openravepy.RobotStateSaver(sim_env.robot):
sim_env.robot.SetActiveDOFs(dof_inds)
col_times = []
for (i,row) in enumerate(traj_up):
sim_env.robot.SetActiveDOFValues(row)
col_now = cc.BodyVsAll(sim_env.robot)
#with util.suppress_stdout():
# col_now2 = cc.PlotCollisionGeometry()
col_now = [cn for cn in col_now if cn.GetDistance() < collision_dist_threshold]
if col_now:
#print [cn.GetDistance() for cn in col_now]
col_times.append(i)
#print "trajopt.CollisionChecker: ", len(col_now)
#print col_now2
return col_times
def execute_trajectory(self,
full_traj,
step_viewer=1,
interactive=False,
max_cart_vel_trans_traj=.05,
max_cart_vel=.02,
sim_callback=None):
# TODO: incorporate other parts of sim_full_traj_maybesim
if sim_callback is None:
sim_callback = lambda i: self.step()
traj, dof_inds = full_traj
# # clip finger joint angles to the binary gripper angles if necessary
# for lr in 'lr':
# joint_ind = self.robot.GetJoint("%s_gripper_l_finger_joint"%lr).GetDOFIndex()
# if joint_ind in dof_inds:
# ind = dof_inds.index(joint_ind)
# traj[:,ind] = np.minimum(traj[:,ind], get_binary_gripper_angle(True))
# traj[:,ind] = np.maximum(traj[:,ind], get_binary_gripper_angle(False))
# in simulation mode, we must make sure to gradually move to the new starting position
self.robot.SetActiveDOFs(dof_inds)
curr_vals = self.robot.GetActiveDOFValues()
transition_traj = np.r_[[curr_vals], [traj[0]]]
sim_util.unwrap_in_place(transition_traj, dof_inds=dof_inds)
transition_traj = retiming.retime_traj(
self.robot,
dof_inds,
transition_traj,
max_cart_vel=max_cart_vel_trans_traj)
animate_traj.animate_traj(
transition_traj,
self.robot,
restore=False,
pause=interactive,
callback=sim_callback,
step_viewer=step_viewer if self.viewer else 0)
traj[0] = transition_traj[-1]
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
traj = retiming.retime_traj(
self.robot, dof_inds, traj, max_cart_vel=max_cart_vel
) # make the trajectory slow enough for the simulation
animate_traj.animate_traj(
traj,
self.robot,
restore=False,
pause=interactive,
callback=sim_callback,
step_viewer=step_viewer if self.viewer else 0)
if self.viewer and step_viewer:
self.viewer.Step()
return True
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 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 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 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 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())
