def traj_collisions(sim_env, full_traj, collision_dist_threshold, n=100):
"""
Returns the set of collisions.
manip = Manipulator or list of indices
"""
traj, dof_inds = full_traj
traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)),
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 animate_traj(traj, robot, pause=True, restore=True):
"""make sure to set active DOFs beforehand"""
if restore: _saver = openravepy.RobotStateSaver(robot)
viewer = trajoptpy.GetViewer(robot.GetEnv())
for (i,dofs) in enumerate(traj):
print "step %i/%i"%(i+1,len(traj))
robot.SetActiveDOFValues(dofs)
if pause: viewer.Idle()
else: viewer.Step()
def check_collision(self, grasp_left, grasp_right):
with openravepy.RobotStateSaver(self.gmodel_left.robot):
with openravepy.RobotStateSaver(self.gmodel_right.robot):
with self.env:
contacts_left, finalconfig_left, mindist_left, volume_left = self.gmodel_left.testGrasp(
grasp=grasp_left, translate=True, forceclosure=True)
contacts_right, finalconfig_right, mindist_right, volume_right = self.gmodel_right.testGrasp(
grasp=grasp_right, translate=True, forceclosure=True)
self.gmodel_left.robot.GetController().Reset(0)
self.gmodel_left.robot.SetDOFValues(finalconfig_left[0])
self.gmodel_left.robot.SetTransform(finalconfig_left[1])
self.gmodel_right.robot.GetController().Reset(0)
self.gmodel_right.robot.SetDOFValues(finalconfig_right[0])
self.gmodel_right.robot.SetTransform(finalconfig_right[1])
self.env.UpdatePublishedBodies()
return self.env.CheckCollision(self.robot_left,
self.robot_right)
def animate_traj(traj, robot, pause=True, step_viewer=1, restore=True, callback=None, execute_step_cond=None):
"""make sure to set active DOFs beforehand"""
if restore: _saver = openravepy.RobotStateSaver(robot)
if step_viewer or pause: viewer = trajoptpy.GetViewer(robot.GetEnv())
for (i,dofs) in enumerate(traj):
sys.stdout.write("step %i/%i\r"%(i+1,len(traj)))
sys.stdout.flush()
if callback is not None: callback(i)
if execute_step_cond is not None and not execute_step_cond(i): continue
robot.SetActiveDOFValues(dofs)
if pause: viewer.Idle()
elif step_viewer!=0 and (i%step_viewer==0 or i==len(traj)-1): viewer.Step()
sys.stdout.write("\n")
def traj_collisions(traj, robot, n=100):
"""
Returns the set of collisions.
manip = Manipulator or list of indices
"""
traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)),
traj)
ss = rave.RobotStateSaver(robot)
env = robot.GetEnv()
col_times = []
for (i, row) in enumerate(traj_up):
robot.SetActiveDOFValues(row)
col_now = env.CheckCollision(robot)
if col_now:
col_times.append(i)
return col_times
def show_grasp(self, grasp, delay=1.5):
with openravepy.RobotStateSaver(self.gmodel.robot):
with self.gmodel.GripperVisibility(self.gmodel.manip):
time.sleep(0.1) # let viewer update?
try:
with self.env:
contacts,finalconfig,mindist,volume = self.gmodel.testGrasp(grasp=grasp,translate=True,forceclosure=True)
#if mindist == 0:
# print 'grasp is not in force closure!'
contactgraph = self.gmodel.drawContacts(contacts) if len(contacts) > 0 else None
self.gmodel.robot.GetController().Reset(0)
self.gmodel.robot.SetDOFValues(finalconfig[0])
self.gmodel.robot.SetTransform(finalconfig[1])
self.env.UpdatePublishedBodies()
time.sleep(delay)
except openravepy.planning_error,e:
print 'bad grasp!',e
def check_traj(traj, manip, n=100):
traj_up = mu.interp2d(np.linspace(0, 1, n), np.linspace(0, 1, len(traj)),
traj)
robot = manip.GetRobot()
ss = rave.RobotStateSaver(robot)
arm_inds = manip.GetArmIndices()
env = robot.GetEnv()
collision = False
col_times = []
for (i, row) in enumerate(traj_up):
robot.SetDOFValues(row, arm_inds)
col_now = env.CheckCollision(robot)
if col_now:
collision = True
col_times.append(i)
if col_times: print "collision at timesteps", col_times
else: print "no collisions"
return collision
def chomp_plan(robot, group_name, active_joint_names, active_affine,
target_dof_values, init_trajs):
datadir = 'chomp_data'
n_points = args.n_steps
assert active_affine == 0 or active_affine == 11
use_base = active_affine == 11
saver = openravepy.RobotStateSaver(robot)
target_base_pose = None
if use_base:
with robot:
robot.SetActiveDOFValues(target_dof_values)
target_base_pose = openravepy.poseFromMatrix(robot.GetTransform())
robot.SetActiveDOFs(
robot.GetActiveDOFIndices(),
0) # turn of affine dofs; chomp takes that separately
target_dof_values = target_dof_values[:-3] # strip off the affine part
openravepy.RaveSetDebugLevel(openravepy.DebugLevel.Warn)
m_chomp = get_chomp_module(robot.GetEnv())
env_hash = hash_env(robot.GetEnv())
if active_affine != 0:
env_hash += "_aa" + str(active_affine)
# load distance field
j1idxs = [m.GetArmIndices()[0] for m in robot.GetManipulators()]
for link in robot.GetLinks():
for j1idx in j1idxs:
if robot.DoesAffect(j1idx, link.GetIndex()):
link.Enable(False)
try:
aabb_padding = 1.0 if not use_base else 3.0 # base problems should need a distance field over a larger volume
m_chomp.computedistancefield(kinbody=robot,
aabb_padding=aabb_padding,
cache_filename='%s/chomp-sdf-%s.dat' %
(datadir, env_hash))
except Exception, e:
print 'Exception in computedistancefield:', repr(e)
def get_ee_traj(robot,
lr,
arm_traj_or_full_traj,
ee_link_name_fmt="%s_gripper_tool_frame"):
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
ee_link_name = ee_link_name_fmt % lr
ee_link = robot.GetLink(ee_link_name)
if type(arm_traj_or_full_traj) == tuple: # it is a full_traj
full_traj = arm_traj_or_full_traj
traj = full_traj[0]
dof_inds = full_traj[1]
else:
arm_traj = arm_traj_or_full_traj
traj = arm_traj
dof_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_traj = []
with openravepy.RobotStateSaver(robot):
for i_step in range(traj.shape[0]):
robot.SetDOFValues(traj[i_step], dof_inds)
ee_traj.append(ee_link.GetTransform())
return np.array(ee_traj)
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 plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
handles = []
def animate_traj(traj, robot):
with robot:
viewer = trajoptpy.GetViewer(robot.GetEnv())
for i, traj_mats in enumerate(zip(traj, new_hmats)):
dofs = traj_mats[0]
mat = traj_mats[1]
print mat
robot.SetDOFValues(
dofs,
robot.GetManipulator(manip_name).GetArmIndices())
colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]
for i in range(3):
point = mat[:3, 3]
axis = mat[:3, i] / 10
handles.append(
Globals.env.drawarrow(p1=point,
p2=point + axis,
linewidth=0.004,
color=colors[i]))
viewer.Idle()
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
#animate_traj(init_traj, robot)
#init_traj[0] = robot.GetDOFValues(arm_inds)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": manip_name,
"start_fixed": False
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
"params": {
"coeffs": [10],
"dist_pen": [0.005]
}
}],
"constraints": [],
"init_info": {
"type": "given_traj",
"data": [x.tolist() for x in init_traj]
}
}
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append({
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [20, 20, 20],
"rot_coeff": [20, 20, 20]
}
})
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(
s, Globals.env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
#animate_traj(traj, robot)
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name, pos_errs.max(), pos_errs)
return traj
def plan_follow_traj(robot, manip_name, ee_link, new_hmats, old_traj):
n_steps = len(new_hmats)
assert old_traj.shape[0] == n_steps
assert old_traj.shape[1] == 7
arm_inds = robot.GetManipulator(manip_name).GetArmIndices()
ee_linkname = ee_link.GetName()
init_traj = old_traj.copy()
#init_traj[0] = robot.GetDOFValues(arm_inds)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": manip_name,
"start_fixed": False
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [n_steps / 5.]
}
}, {
"type": "collision",
"params": {
"coeffs": [10],
"dist_pen": [0.01]
}
}],
"constraints": [],
"init_info": {
"type": "given_traj",
"data": [x.tolist() for x in init_traj]
}
}
poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats]
for (i_step, pose) in enumerate(poses):
request["costs"].append({
"type": "pose",
"params": {
"xyz": pose[4:7].tolist(),
"wxyz": pose[0:4].tolist(),
"link": ee_linkname,
"timestep": i_step,
"pos_coeffs": [10, 10, 10],
"rot_coeffs": [10, 10, 10]
}
})
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(
s, robot.GetEnv()) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
saver = openravepy.RobotStateSaver(robot)
pos_errs = []
for i_step in xrange(1, n_steps):
row = traj[i_step]
robot.SetDOFValues(row, arm_inds)
tf = ee_link.GetTransform()
pos = tf[:3, 3]
pos_err = np.linalg.norm(poses[i_step][4:7] - pos)
pos_errs.append(pos_err)
pos_errs = np.array(pos_errs)
print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % (
manip_name, pos_errs.max(), pos_errs)
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())
