def gen_init_trajs(problemset, robot, n_steps, start_joints, end_joints):
waypoint_step = (n_steps - 1) // 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints)) / 2]
if args.multi_init:
if args.use_random_inits:
print 'using random initializations'
joint_waypoints.extend(
sample_base_positions(robot, num=5, tucked=True))
# if the problem file has waypoints, just use those
else:
if problemset["group_name"] in [
"right_arm", "left_arm", "whole_body"
]:
joint_waypoints.extend(get_postures(problemset["group_name"]))
trajs = []
for i, waypoint in enumerate(joint_waypoints):
if i == 0:
inittraj = mu.linspace2d(start_joints, end_joints, n_steps)
else:
inittraj = np.empty((n_steps, robot.GetActiveDOF()))
inittraj[:waypoint_step + 1] = mu.linspace2d(
start_joints, waypoint, waypoint_step + 1)
inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints,
n_steps - waypoint_step)
trajs.append(inittraj)
return trajs
def interpolate_waypoint(start, waypoint, end, num_steps):
waypoint_step = (num_steps - 1) // 2
traj = np.empty((num_steps, 7))
traj[:waypoint_step + 1] = mu.linspace2d(start, waypoint,
waypoint_step + 1)
traj[waypoint_step:] = mu.linspace2d(waypoint, end,
num_steps - waypoint_step)
return traj
def predict_with_waypoint(self,
init_joint,
target_joint,
waypoint=None,
waypoint_step=0):
inittraj = np.empty((self.n_steps, self.dof))
inittraj[:waypoint_step + 1] = mu.linspace2d(init_joint, waypoint,
waypoint_step + 1)
inittraj[waypoint_step:] = mu.linspace2d(waypoint, target_joint,
self.n_steps - waypoint_step)
return inittraj, np.array([[0]])
def move_arm_straight_request(manip, n_steps, link_name, joints_start, joints_end, xyz_start, quat_start, xyz_end, quat_end):
'''
WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible!
'''
init_joints = mu.linspace2d(joints_start, joints_end, n_steps)
request = {
'basic_info': {
'n_steps': n_steps,
'manip': manip.GetName(),
'start_fixed': False, # take care of this with the init_joints constraint instead
},
'costs': [
{ 'type': 'joint_vel', 'params': { 'coeffs': [1] } },
{ 'type': 'collision', 'params': { 'coeffs': [1], 'dist_pen': [0.025] } },
],
'constraints': [
{
'type' : 'joint',
'name' : 'init_joints',
'params' : { 'vals': list(joints_start), 'timestep': 0 },
},
{
'type' : 'joint',
'name' : 'final_joints',
'params' : { 'vals': list(joints_end) },
},
],
'init_info': {
'type': 'given_traj',
'data': [j.tolist() for j in init_joints],
},
}
# add costs for deviation from a straight line in cartesian space
lin_xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps)
lin_quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
for i in range(1, n_steps-1):
request['costs'].append({
'type': 'pose',
'params': {
'xyz': list(lin_xyzs[i]),
'wxyz': list(lin_quats[i]),
'pos_coeffs': [1, 1, 1],
'rot_coeffs': [1, 1, 1],
'link': link_name,
'timestep': i,
}
})
return request
def trajopt_multi_plan(env,
robot,
goal_config,
num_inits=10,
num_steps=10,
warn_if_unchanged=False,
inits=None,
**args):
start_joints = robot.GetActiveDOFValues()
linear_init = mu.linspace2d(start_joints, goal_config, num_steps)
results = []
default_res = trajopt_simple_plan(env,
robot,
goal_config,
num_steps=num_steps,
**args)
results.append(default_res)
for i in range(num_inits - 1):
if inits is not None:
modified_init = inits[i]
else:
modified_init = np.mod(linear_init + smooth_perturb(), 2 * np.pi)
res = trajopt_simple_plan(env,
robot,
goal_config,
init=modified_init,
num_steps=num_steps,
**args)
if warn_if_unchanged and np.allclose(modified_init, res.GetTraj()):
print('Warning: unchanged init')
if not np.allclose(default_res.GetTraj(), res.GetTraj()):
results.append(res)
return results
def trajopt_simple_plan(env,
robot,
goal_config,
num_steps=10,
custom_costs={},
init=None,
custom_traj_costs={},
request_callbacks=[],
joint_vel_coeff=1.):
start_joints = robot.GetActiveDOFValues()
if init is None:
init = mu.linspace2d(start_joints, goal_config, num_steps)
request = {
'basic_info': {
'n_steps': num_steps,
'manip': robot.arm.GetName(),
'start_fixed': True
},
'costs': [{
'type': 'collision',
'params': {
'coeffs': [20],
'dist_pen': [0.025]
}
}, {
'type': 'joint_vel',
'params': {
'coeffs': [joint_vel_coeff]
}
}],
'constraints': [{
'type': 'joint',
'params': {
'vals': goal_config.tolist()
}
}],
'init_info': {
'type': 'given_traj',
'data': init.tolist()
}
}
[callback(request) for callback in request_callbacks]
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
for cost_name in custom_costs:
cost_fn = custom_costs[cost_name]
prob.AddCost(cost_fn,
[(t, j) for t in range(num_steps) for j in range(7)],
'{:s}{:d}'.format(cost_name, t))
for cost_name in custom_traj_costs:
cost_fn = custom_traj_costs[cost_name]
if isinstance(cost_fn, tuple):
prob.AddErrorCost(cost_fn[0], cost_fn[1],
[(t, j) for t in range(num_steps)
for j in range(7)], 'SQUARED', cost_name)
else:
prob.AddCost(cost_fn, [(t, j) for t in range(num_steps)
for j in range(7)], cost_name)
result = trajoptpy.OptimizeProblem(prob)
return result
def random_init_maker(given_init, one_wp=False):
start = given_init[0]
end = given_init[-1]
if one_wp:
changed_idx = np.random.choice(range(3, 7))
original = given_init[changed_idx]
new_pt = np.random.multivariate_normal(original, .05 * np.eye(7))
modified_init = np.concatenate([
mu.linspace2d(given_init[0], new_pt, changed_idx + 1),
mu.linspace2d(new_pt, given_init[-1], 10 - changed_idx)[1:]
],
axis=0)
else:
modified_init = given_init.copy()
modified_init[1:-1] = np.random.multivariate_normal(
given_init[1:-1].reshape(-1),
np.linalg.norm(start - end) * 0.0025 * np.eye(56)).reshape(8, -1)
return modified_init
def main():
### Parameters ###
ENV_FILE = "../../trajopt/data/pr2_table.env.xml"
MANIP_NAME = "rightarm"
N_STEPS = 10
LINK_NAME = "r_gripper_tool_frame"
INTERACTIVE = True
#joints_start_end = np.array([
# [-0.95, -0.38918253, -2.43888696, -1.23400121, -0.87433154, -0.97616443, -2.10997203],
# [-0.4, -0.4087081, -3.77121706, -1.2273375, 0.69885101, -0.8992004, 3.13313843]
#])
#joints_start_end = np.array([[0.34066373, -0.49439586, -3.3 , -1.31059503 , -1.28229698, -0.15682819, -116.19626995],
# [ 0.5162424 , -0.42037121 , -3.7 , -1.30277208 , 1.31120586, -0.16411924 ,-118.57637204]])
#joints_start_end = np.array([[ -1.83204054 , -0.33201855 , -1.01105089 , -1.43693186 , -1.099908, -2.00040616, -116.17133393],
#[ -0.38176851 , 0.17886005 , -1.4 , -1.89752658 , -1.93285873, -1.60546868, -114.70809047]])
joints_start_end = np.array([[0.33487707, -0.50480484 , -3.3 , -1.33546928 , -1.37194549 , -0.14645853 ,-116.11672039], [ 4.71340480e-01 , -4.56593341e-01 , -3.60000000e+00 , -1.33176173e+00,
1.21859723e+00 , -9.98780266e-02, -1.18561732e+02]])
##################
joints_start_end[:,2] = np.unwrap(joints_start_end[:,2])
joints_start_end[:,4] = np.unwrap(joints_start_end[:,4])
joints_start_end[:,6] = np.unwrap(joints_start_end[:,6])
joints_start = joints_start_end[0,:]
joints_end = joints_start_end[1,:]
### Env setup ####
env = rave.RaveGetEnvironment(1)
if env is None:
env = rave.Environment()
env.StopSimulation()
atexit.register(rave.RaveDestroy)
env.Load(ENV_FILE)
robot = env.GetRobots()[0]
manip = robot.GetManipulator(MANIP_NAME)
robot.SetDOFValues(joints_start, manip.GetArmIndices())
##################
result_traj = move_arm_straight(env, manip, N_STEPS, LINK_NAME, joints_start, joints_end, interactive=INTERACTIVE)
print 'Showing original straight-line trajectory'
env.SetViewer('qtcoin')
env.UpdatePublishedBodies()
import time
time.sleep(2)
play_traj(mu.linspace2d(joints_start, joints_end, N_STEPS), env, manip)
raw_input('press enter to continue')
print 'Showing optimized trajectory'
play_traj(result_traj, env, manip)
raw_input('press enter to continue')
def gen_init_trajs(problemset, robot, n_steps, start_joints, end_joints):
waypoint_step = (n_steps - 1)// 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2]
if args.multi_init:
if args.use_random_inits:
print 'using random initializations'
joint_waypoints.extend(sample_base_positions(robot, num=5, tucked=True))
# if the problem file has waypoints, just use those
else:
if problemset["group_name"] in ["right_arm", "left_arm", "whole_body"]:
joint_waypoints.extend(get_postures(problemset["group_name"]))
trajs = []
for i, waypoint in enumerate(joint_waypoints):
if i == 0:
inittraj = mu.linspace2d(start_joints, end_joints, n_steps)
else:
inittraj = np.empty((n_steps, robot.GetActiveDOF()))
inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1)
inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step)
trajs.append(inittraj)
return trajs
def generate_trajs():
use_planning = (request.form['generate_option'] == 'planning')
temp_to_label = []
with env:
sg_pair_idcs = np.random.choice(range(len(c.start_goal_pairs)),
size=4,
replace=False)
for idx in sg_pair_idcs:
start, goal = c.start_goal_pairs[idx]
robot.SetActiveDOFValues(start)
if not use_planning:
given = mu.linspace2d(start, goal, 10)
else:
given = planners.trajopt_simple_plan(
env,
robot,
goal,
custom_traj_costs=custom_cost,
joint_vel_coeff=session_vars['joint_vel_coeff']).GetTraj()
wps = [given]
for i in range(3):
delta = utils.smooth_perturb(.1)
wps.append(given + delta)
for xA, xB in itertools.combinations(wps, 2):
if not np.allclose(xA[:, :7], xB[:, :7]):
trajA = utils.waypoints_to_traj(
display_env,
display_robot1,
xA,
session_vars['speed'],
None,
use_ee_dist=session_vars['use_ee_dist'])
trajB = utils.waypoints_to_traj(
display_env,
display_robot2,
xB,
session_vars['speed'],
None,
use_ee_dist=session_vars['use_ee_dist'])
temp_to_label.append((xA, trajA, xB, trajB))
random.shuffle(temp_to_label)
to_label.extend(temp_to_label)
starting_dofs = to_label[0][1].GetWaypoint(0)[:7]
display_robot1.SetActiveDOFValues(starting_dofs)
display_robot2.SetActiveDOFValues(starting_dofs)
return redirect(url_for('index'))
def plan(robot, manip, end_joints, end_pose=None):
arm_inds = robot.GetManipulator(manip).GetArmIndices()
start_joints = robot.GetDOFValues(arm_inds)
n_steps = 11
coll_coeff = 10
dist_pen = .05
waypoint_step = (n_steps - 1) // 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints)) / 2]
joint_waypoints.extend(get_postures(manip))
success = False
t_start = time()
t_verify = 0
t_opt = 0
for (i_init, waypoint) in enumerate(joint_waypoints):
d = {
"basic_info": {
"n_steps": n_steps,
"manip": manip,
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
"params": {
"coeffs": [coll_coeff],
"dist_pen": [dist_pen]
}
}, {
"type": "collision",
"params": {
"continuous": False,
"coeffs": [coll_coeff],
"dist_pen": [dist_pen]
}
}],
"constraints": [{
"type": "joint",
"params": {
"vals": end_joints.tolist()
}
} if end_pose is None else {
"type": "pose",
"params": {
"xyz": end_pose[4:7].tolist(),
"wxyz": end_pose[0:4].tolist(),
"link": "%s_gripper_tool_frame" % manip[0]
}
}],
"init_info": {
"type": "given_traj"
}
}
if args.multi_init:
# print "trying with midpoint", waypoint
inittraj = np.empty((n_steps, 7))
inittraj[:waypoint_step + 1] = mu.linspace2d(
start_joints, waypoint, waypoint_step + 1)
inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints,
n_steps - waypoint_step)
else:
inittraj = mu.linspace2d(start_joints, end_joints, n_steps)
d["init_info"]["data"] = [row.tolist() for row in inittraj]
s = json.dumps(d)
prob = trajoptpy.ConstructProblem(s, env)
t_start_opt = time()
result = trajoptpy.OptimizeProblem(prob)
t_opt += time() - t_start_opt
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
t_start_verify = time()
is_safe = traj_is_safe(traj, robot)
t_verify += time() - t_start_verify
if not is_safe:
print "optimal trajectory has a collision. trying a new initialization"
else:
print "planning successful after %s initialization" % (i_init + 1)
success = True
break
t_total = time() - t_start
return PlanResult(success, t_total, t_opt, t_verify)
def predict(self, init_joint, target_joint):
inittraj = np.empty((self.n_steps, self.dof))
inittraj = mu.linspace2d(init_joint, target_joint, self.n_steps)
return inittraj, np.array([[0]])
def main():
### Parameters ###
ENV_FILE = "../../trajopt/data/pr2_table.env.xml"
MANIP_NAME = "rightarm"
N_STEPS = 10
LINK_NAME = "r_gripper_tool_frame"
INTERACTIVE = True
#joints_start_end = np.array([
# [-0.95, -0.38918253, -2.43888696, -1.23400121, -0.87433154, -0.97616443, -2.10997203],
# [-0.4, -0.4087081, -3.77121706, -1.2273375, 0.69885101, -0.8992004, 3.13313843]
#])
#joints_start_end = np.array([[0.34066373, -0.49439586, -3.3 , -1.31059503 , -1.28229698, -0.15682819, -116.19626995],
# [ 0.5162424 , -0.42037121 , -3.7 , -1.30277208 , 1.31120586, -0.16411924 ,-118.57637204]])
#joints_start_end = np.array([[ -1.83204054 , -0.33201855 , -1.01105089 , -1.43693186 , -1.099908, -2.00040616, -116.17133393],
#[ -0.38176851 , 0.17886005 , -1.4 , -1.89752658 , -1.93285873, -1.60546868, -114.70809047]])
joints_start_end = np.array([[
0.33487707, -0.50480484, -3.3, -1.33546928, -1.37194549, -0.14645853,
-116.11672039
],
[
4.71340480e-01, -4.56593341e-01,
-3.60000000e+00, -1.33176173e+00,
1.21859723e+00, -9.98780266e-02,
-1.18561732e+02
]])
##################
joints_start_end[:, 2] = np.unwrap(joints_start_end[:, 2])
joints_start_end[:, 4] = np.unwrap(joints_start_end[:, 4])
joints_start_end[:, 6] = np.unwrap(joints_start_end[:, 6])
joints_start = joints_start_end[0, :]
joints_end = joints_start_end[1, :]
### Env setup ####
env = rave.RaveGetEnvironment(1)
if env is None:
env = rave.Environment()
env.StopSimulation()
atexit.register(rave.RaveDestroy)
env.Load(ENV_FILE)
robot = env.GetRobots()[0]
manip = robot.GetManipulator(MANIP_NAME)
robot.SetDOFValues(joints_start, manip.GetArmIndices())
##################
result_traj = move_arm_straight(env,
manip,
N_STEPS,
LINK_NAME,
joints_start,
joints_end,
interactive=INTERACTIVE)
print 'Showing original straight-line trajectory'
env.SetViewer('qtcoin')
env.UpdatePublishedBodies()
import time
time.sleep(2)
play_traj(mu.linspace2d(joints_start, joints_end, N_STEPS), env, manip)
raw_input('press enter to continue')
print 'Showing optimized trajectory'
play_traj(result_traj, env, manip)
raw_input('press enter to continue')
def move_arm_straight_request(manip, n_steps, link_name, joints_start,
joints_end, xyz_start, quat_start, xyz_end,
quat_end):
'''
WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible!
'''
init_joints = mu.linspace2d(joints_start, joints_end, n_steps)
request = {
'basic_info': {
'n_steps': n_steps,
'manip': manip.GetName(),
'start_fixed':
False, # take care of this with the init_joints constraint instead
},
'costs': [
{
'type': 'joint_vel',
'params': {
'coeffs': [1]
}
},
{
'type': 'collision',
'params': {
'coeffs': [1],
'dist_pen': [0.025]
}
},
],
'constraints': [
{
'type': 'joint',
'name': 'init_joints',
'params': {
'vals': list(joints_start),
'timestep': 0
},
},
{
'type': 'joint',
'name': 'final_joints',
'params': {
'vals': list(joints_end)
},
},
],
'init_info': {
'type': 'given_traj',
'data': [j.tolist() for j in init_joints],
},
}
# add costs for deviation from a straight line in cartesian space
lin_xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps)
lin_quats = [
rave.quatSlerp(quat_start, quat_end, t)
for t in np.linspace(0, 1, n_steps)
]
for i in range(1, n_steps - 1):
request['costs'].append({
'type': 'pose',
'params': {
'xyz': list(lin_xyzs[i]),
'wxyz': list(lin_quats[i]),
'pos_coeffs': [1, 1, 1],
'rot_coeffs': [1, 1, 1],
'link': link_name,
'timestep': i,
}
})
return request
def plan(robot, manip, end_joints, end_pose = None):
arm_inds = robot.GetManipulator(manip).GetArmIndices()
start_joints = robot.GetDOFValues(arm_inds)
n_steps = 11
coll_coeff = 10
dist_pen = .05
waypoint_step = (n_steps - 1)// 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2]
joint_waypoints.extend(get_postures(manip))
success = False
t_start = time()
t_verify = 0
t_opt = 0
for (i_init,waypoint) in enumerate(joint_waypoints):
d = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : manip,
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "collision",
"params" : {"coeffs" : [coll_coeff],"dist_pen" : [dist_pen]}
},
{
"type" : "collision",
"params" : {"continuous": False, "coeffs" : [coll_coeff],"dist_pen" : [dist_pen]}
}
],
"constraints" : [
{"type" : "joint", "params" : {"vals" : end_joints.tolist()}} if end_pose is None else
{"type" : "pose", "params" : {"xyz" : end_pose[4:7].tolist(), "wxyz" : end_pose[0:4].tolist(), "link":"%s_gripper_tool_frame"%manip[0]}}
],
"init_info" : {
"type" : "given_traj"
}
}
if args.multi_init:
# print "trying with midpoint", waypoint
inittraj = np.empty((n_steps, 7))
inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1)
inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step)
else:
inittraj = mu.linspace2d(start_joints, end_joints, n_steps)
d["init_info"]["data"] = [row.tolist() for row in inittraj]
s = json.dumps(d)
prob = trajoptpy.ConstructProblem(s, env)
t_start_opt = time()
result = trajoptpy.OptimizeProblem(prob)
t_opt += time() - t_start_opt
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
t_start_verify = time()
is_safe = traj_is_safe(traj, robot)
t_verify += time() - t_start_verify
if not is_safe:
print "optimal trajectory has a collision. trying a new initialization"
else:
print "planning successful after %s initialization"%(i_init+1)
success = True
break
t_total = time() - t_start
return PlanResult(success, t_total, t_opt, t_verify)
def callback(getreq):
assert isinstance(getreq, ms.GetMotionPlanRequest)
req = getreq.motion_plan_request
getresp = ms.GetMotionPlanResponse()
resp = getresp.motion_plan_response
manip = GROUPMAP[req.group_name]
update_rave_from_ros(robot, req.start_state.joint_state.position, req.start_state.joint_state.name)
base_pose = req.start_state.multi_dof_joint_state.poses[0]
base_q = base_pose.orientation
base_p = base_pose.position
t = rave.matrixFromPose([base_q.w, base_q.x, base_q.y, base_q.z, base_p.x, base_p.y, base_p.z])
robot.SetTransform(t)
start_joints = robot.GetDOFValues(robot.GetManipulator(manip).GetArmIndices())
n_steps = 9
coll_coeff = 10
dist_pen = .04
inds = robot.GetManipulator(manip).GetArmJoints()
alljoints = robot.GetJoints()
names = arm_joint_names = [alljoints[i].GetName() for i in inds]
for goal_cnt in req.goal_constraints:
if len(goal_cnt.joint_constraints) > 0:
assert len(goal_cnt.joint_constraints)==7
end_joints = np.zeros(7)
for i in xrange(7):
jc = goal_cnt.joint_constraints[i]
dof_idx = arm_joint_names.index(jc.joint_name)
end_joints[dof_idx] = jc.position
waypoint_step = (n_steps - 1)// 2
joint_waypoints = [(np.asarray(start_joints) + np.asarray(end_joints))/2]
if args.multi_init:
leftright = req.group_name.split("_")[0]
joint_waypoints.extend(get_postures(leftright))
success = False
for waypoint in joint_waypoints:
robot.SetDOFValues(start_joints, inds)
d = {
"basic_info" : {
"n_steps" : n_steps,
"manip" : manip,
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "continuous_collision",
"params" : {"coeffs" : [coll_coeff],"dist_pen" : [dist_pen]}
}
],
"constraints" : [],
"init_info" : {
"type" : "given_traj"
}
}
d["constraints"].append({
"type" : "joint",
"name" : "joint",
"params" : {
"vals" : end_joints.tolist()
}
})
if args.multi_init:
inittraj = np.empty((n_steps, 7))
inittraj[:waypoint_step+1] = mu.linspace2d(start_joints, waypoint, waypoint_step+1)
inittraj[waypoint_step:] = mu.linspace2d(waypoint, end_joints, n_steps - waypoint_step)
else:
inittraj = mu.linspace2d(start_joints, end_joints, n_steps)
print "this",inittraj
print "other",mu.linspace2d(start_joints, end_joints, n_steps)
d["init_info"]["data"] = [row.tolist() for row in inittraj]
if len(goal_cnt.position_constraints) > 0:
raise NotImplementedError
s = json.dumps(d)
t_start = time()
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
planning_duration_seconds = time() - t_start
traj = result.GetTraj()
if check_traj(traj, robot.GetManipulator(manip)):
print "aw, there's a collision"
else:
print "no collisions"
success = True
break
if not success:
resp.error_code.val = FAILURE
return resp
resp.trajectory.joint_trajectory.joint_names = names
for row in traj:
jtp = tm.JointTrajectoryPoint()
jtp.positions = row.tolist()
jtp.time_from_start = rospy.Duration(0)
resp.trajectory.joint_trajectory.points.append(jtp)
mdjt = resp.trajectory.multi_dof_joint_trajectory
mdjt.joint_names = ['world_joint']
mdjt.frame_ids = ['odom_combined']
mdjt.child_frame_ids = ['base_footprint']
mdjt.header = req.start_state.joint_state.header
pose = gm.Pose()
pose.orientation.w = 1
for _ in xrange(len(resp.trajectory.joint_trajectory.points)):
jtp = mm.MultiDOFJointTrajectoryPoint()
jtp.poses.append(pose)
#for i in xrange(len(mdjs.joint_names)):
#if mdjs.joint_names[i] == "world_joint":
#world_joint_pose = mdjs.poses[i]
#world_joint_frame_id = mdjs.frame_ids[i]
#world_joint_child_frame_id = mdjs.child_frame_ids[i]
#print "world_joint_frame_id", world_joint_frame_id
#print "world_joint_child_frame_id", world_joint_child_frame_id
#mdjt = resp.trajectory.multi_dof_joint_trajectory
#mdjt.header.frame_id = req.start_state.joint_state.header.frame_id
#mdjt.joint_names.append("world_joint")
resp.error_code.val = SUCCESS
resp.planning_time = rospy.Duration(planning_duration_seconds)
resp.trajectory.joint_trajectory.header = req.start_state.joint_state.header
resp.group_name = req.group_name
resp.trajectory_start.joint_state = req.start_state.joint_state
#resp.trajectory.multi_dof_joint_trajectory
#resp.trajectory.multi_dof_joint_trajectory.header = req.start_state.joint_state.header
getresp.motion_plan_response = resp
return getresp
def main(args):
exp_name = args.exp_name
traj_counter = 0
# Directory setup
vid_dir = os.path.join('web', 'vids', exp_name)
if os.path.exists(vid_dir):
print('Video directory already exists!')
traj_counter = len(os.listdir(vid_dir))
else:
os.makedirs(vid_dir)
# Queue to send child process commands.
task_queue = multiprocessing.Queue()
# Queue for receiving trajs generated by child process.
traj_queue = multiprocessing.Queue()
p = multiprocessing.Process(target=comms_proc,
args=[exp_name, task_queue, traj_queue])
p.start()
client = connect('style_experiment')
planned_wps = {idcs: None for idcs in constants.sg_train_idcs}
# Training queue of *labeled* comparisons
labeled_comparison_queue = ComparisonQueue(exp_name)
env, robot = utils.setup(render=True)
raw_input('Press enter to continue...')
monitor = record_video.get_geometry()
# cf = CostFunction(
# robot,
# use_all_links=True,
# quadratic=False)
cf = LinearCostFunction(robot, num_dofs=50)
cf_save_path = os.path.join('saves', 'style_experiment', exp_name, 'model')
custom_cost = {'NN': planners.get_trajopt_cost(cf)}
summary_writer = tf.summary.FileWriter(
os.path.join('tb_logs', 'style_experiment', exp_name))
# Actual main loop
global_step = 0
prev_num_labeled = 0
num_perturbs = 0
while True:
num_labeled = len(labeled_comparison_queue)
sufficient_labeled_data = num_labeled >= label_batchsize
# Training step
if num_labeled - prev_num_labeled > label_batchsize and num_perturbs > 5:
prev_num_labeled = num_labeled
num_perturbs = 0
for _ in range(num_labeled):
comp = labeled_comparison_queue.sample()
wpsA = binary_to_array(comp.wpsA)
wpsB = binary_to_array(comp.wpsB)
label = comp.label
wpsA_batch = np.repeat(wpsA[None], 8, axis=0)
wpsB_batch = np.repeat(wpsB[None], 8, axis=0)
label_batch = np.repeat(label, 8)
for batch_idx in range(8):
offset = np.random.uniform(0, 2 * np.pi)
wpsA_batch[batch_idx, :, 0] = offset
wpsB_batch[batch_idx, :, 0] = offset
cf.train_pref(wpsA_batch, wpsB_batch, label_batch)
wpsA_list, wpsB_list, labels = [], [], []
for comp in labeled_comparison_queue.queue:
wpsA_list.append(binary_to_array(comp.wpsA))
wpsB_list.append(binary_to_array(comp.wpsB))
labels.append(comp.label)
wpsA_list, wpsB_list, labels = np.stack(wpsA_list), np.stack(
wpsB_list), np.array(labels)
pred = cf.get_traj_cost(wpsB_list) < cf.get_traj_cost(wpsA_list)
t_acc = np.mean(labels == pred)
add_simple_summary(summary_writer, 'training.accuracy', t_acc,
global_step)
summary_writer.flush()
cf.save_model(cf_save_path, step=global_step)
global_step += 1
for idcs in planned_wps:
planned_wps[idcs] = None
# Generation step
for idcs in constants.sg_train_idcs:
s_idx, g_idx = idcs
wps = planned_wps[idcs]
if wps is None:
q_s = constants.configs[s_idx]
q_g = constants.configs[g_idx]
with env:
robot.SetActiveDOFValues(q_s)
if sufficient_labeled_data:
wps = planners.trajopt_simple_plan(
env,
robot,
q_g,
custom_costs=custom_cost,
joint_vel_coeff=1).GetTraj()
else:
# Generate pretraining data
wps = mu.linspace2d(q_s, q_g, 10)
planned_wps[idcs] = wps
else:
wps = make_perturbs(wps, 1, .1)[0]
num_perturbs += 1
vid_name = 's{:d}-g{:d}_traj{:d}.mp4'.format(
s_idx, g_idx, traj_counter)
out_path = os.path.join(vid_dir, vid_name)
with env:
traj = utils.waypoints_to_traj(env, robot, wps, 1, None)
# TODO: move the video export to a different process
if not args.no_record:
record_video.record(robot, traj, out_path, monitor=monitor)
traj_queue.put((wps, out_path, idcs, traj_counter))
traj_counter += 1
time.sleep(.1)
def move_arm_straight_request(manip, n_steps, link_name, xyz_start, xyz_end, quat_start, quat_end, start_joints):
manip_name = manip.GetName()
request = {
"basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": True},
"costs": [
{"type": "joint_vel", "params": {"coeffs": [1]}},
{"type": "collision", "params": {"coeffs": [10], "dist_pen": [0.025]}},
],
"constraints": [
{
"type": "pose",
"name": "final_pose",
"params": {
"pos_coeffs": [10, 10, 10],
"rot_coeffs": [10, 10, 10],
"xyz": list(xyz_end),
"wxyz": list(quat_end),
"link": link_name,
},
}
],
"init_info": {},
}
xyzs = mu.linspace2d(xyz_start, xyz_end, n_steps)
quats = [rave.quatSlerp(quat_start, quat_end, t) for t in np.linspace(0, 1, n_steps)]
hmats = [rave.matrixFromPose(np.r_[quat, xyz]) for (xyz, quat) in zip(xyzs, quats)]
def ikfunc(hmat):
return ku.ik_for_link(hmat, manip, link_name, return_all_solns=True)
def nodecost(joints):
robot = manip.GetRobot()
saver = rave.Robot.RobotStateSaver(robot)
robot.SetDOFValues(joints, manip.GetArmJoints())
return 100 * robot.GetEnv().CheckCollision(robot)
paths, costs, timesteps = ku.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost)
i_best = np.argmin(costs)
print "lowest cost of initial trajs:", costs[i_best]
if len(timesteps) < n_steps:
print "timesteps with soln: ", timesteps
print "linearly interpolating the rest"
path_init = mu.interp2d(np.arange(n_steps), timesteps, paths[i_best])
else:
print "all timesteps have an ik solution"
path_init = paths[i_best]
for i in xrange(1, n_steps - 1):
print "waypoint xyz", xyzs[i]
waypoint_cnt = {
"type": "pose",
"name": "waypoint_pose",
"params": {
"xyz": list(xyzs[i]),
"wxyz": list(quats[i]),
"link": link_name,
# "pos_coeffs" : [10,10,10],
# "rot_coeffs" : [10,10,10],
"timestep": i,
},
}
request["constraints"].append(waypoint_cnt)
request["init_info"]["type"] = "given_traj"
request["init_info"]["data"] = [x.tolist() for x in path_init]
return request
def main(iterations, activation, normalize, dropout, lr, batchnorm, q_length,
use_quadratic_features, perturb_amount, provide_vel, include_configs,
use_all_links, h_size, random_inits, _seed, _run):
tf.set_random_seed(_seed) # Set tf's seed to exp seed
env, robot = utils.setup(render=False)
cf = CostFunction(robot,
num_dofs=7,
normalize=normalize,
include_configs=include_configs,
provide_vel=provide_vel,
use_all_links=use_all_links,
activation=activation,
dropout=dropout,
lr=lr,
batchnorm=batchnorm,
quadratic=use_quadratic_features,
h_size=h_size)
ex.info['num_params'] = cf.num_params
ex.info['mlp_input_shape'] = cf.mlp.model.input_shape
summary_writer = tf.summary.FileWriter(
os.path.join('tb_logs', ex_name, str(_run._id)))
if use_quadratic_features:
custom_cost = {'NN': planners.get_trajopt_error_cost(cf)}
else:
custom_cost = {'NN': planners.get_trajopt_cost(cf)}
tqs = {}
for idcs in constants.sg_train_idcs:
tqs[idcs] = utils.TrainingQueue(maxsize=q_length)
# The avg cost of train trajs generated using the true cost fxn.
BEST_COST_SYNTH = 0.13875292480745718
for idcs in constants.sg_train_idcs:
with env:
linear = mu.linspace2d(constants.configs[idcs[0]],
constants.configs[idcs[1]], 10)
for wps_perturbed in make_perturbs(linear, 50, perturb_amount):
true_cost_perturbed = ee_traj_cost(wps_perturbed, robot)
tqs[idcs].add((wps_perturbed, true_cost_perturbed))
for i in range(iterations):
print('[*] Iteration {:d}/{:d}'.format(i, iterations))
# Training
for j in range(500):
for idcs in constants.sg_train_idcs:
tq = tqs[idcs]
(wpsA, cA), (wpsB, cB) = tq.sample(num=2)
if np.allclose(cA, cB):
continue
offset = np.random.uniform(0, 2 * np.pi)
wpsA[:, 0] = offset
wpsB[:, 0] = offset
cf.train_pref(wpsA[None], wpsB[None], [int(cB < cA)])
random_inputs = np.random.uniform(0, 2 * np.pi, size=(100, 10, 7))
with env:
labels = [ee_traj_cost(x, robot) for x in random_inputs]
corr = cf.get_corrcoef(random_inputs, labels)
add_simple_summary(summary_writer, 'test.correlation', corr, i)
summary_writer.flush()
# Generating training data
true_cost_list = [
] # want to keep track of how good our true costs are
true_cost_variances = [] # Only applies if random_init = True
for idcs in constants.sg_train_idcs:
with env:
robot.SetActiveDOFValues(constants.configs[idcs[0]])
if random_inits:
results_list = planners.trajopt_multi_plan(
env,
robot,
constants.configs[idcs[1]],
num_inits=10,
custom_traj_costs=(custom_cost
if use_quadratic_features else {}),
custom_costs=({} if use_quadratic_features else
custom_cost),
joint_vel_coeff=0)
true_costs = []
for res in results_list:
wps = res.GetTraj()
true_cost = ee_traj_cost(wps, robot)
tqs[idcs].add((wps, true_cost))
true_costs.append(true_cost)
true_cost_list.append(np.min(true_costs))
true_cost_variances.append(np.var(true_costs))
else:
wps = planners.trajopt_simple_plan(
env,
robot,
constants.configs[idcs[1]],
custom_traj_costs=(custom_cost
if use_quadratic_features else {}),
custom_costs=({} if use_quadratic_features else
custom_cost),
joint_vel_coeff=0).GetTraj()
true_cost = ee_traj_cost(wps, robot)
tqs[idcs].add((wps, true_cost))
true_cost_list.append(true_cost)
for wps_perturbed in make_perturbs(x, 10, perturb_amount):
true_cost_perturbed = ee_traj_cost(
wps_perturbed, robot)
tqs[idcs].add((wps_perturbed, true_cost_perturbed))
add_simple_summary(summary_writer, 'training.true_cost',
np.mean(true_cost_list), i)
if random_inits:
add_simple_summary(summary_writer, 'training.true_cost_var',
np.var(true_cost_variances, i))
summary_writer.flush() # flush summary writer at end of iteration
save_folder = './saves/learned_ssee/'
cf.save_model(os.path.join(save_folder, 'model'))
for fname in os.listdir(save_folder):
ex.add_artifact(os.path.join(save_folder, fname))