def __init__(self, arm_name, sim=None, interact=False):
"""
:param arm_name: "left" or "right"
:param sim: OpenRave simulator (or create if None)
:param interact: enable trajopt viewer
"""
assert arm_name == 'left' or arm_name == 'right'
self.sim = sim
if self.sim is None:
self.sim = simulator.Simulator()
self.robot = self.sim.robot
self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm
wrist_flex_index = self.robot.GetJointIndex(arm_name[0] +
'_wrist_flex_joint')
lower, upper = self.robot.GetDOFLimits()
lower[wrist_flex_index], upper[
wrist_flex_index] = -np.pi / 2., np.pi / 2.
self.robot.SetDOFLimits(lower, upper)
if interact:
trajoptpy.SetInteractive(True)
self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0])
self.joint_indices = self.manip.GetArmIndices()
def move_arm_straight(env,
manip,
n_steps,
link_name,
joints_start,
joints_end,
xyz_start=None,
quat_start=None,
xyz_end=None,
quat_end=None,
interactive=False):
'''
Finds a trajectory (of length n_steps) such that the specified link travels
as close as possible to a straight line in cartesian space, constrained to
the specified starting and ending joint values.
xyz_start, quat_start, xyz_end, quat_end are the starting and ending cartesian
poses that correspond to joints_start and joints_end. Setting them to None
will compute them here.
WARNING: start/end joints, xyz, and quats must all agree, otherwise the optimization problem will be infeasible!
Returns: trajectory (numpy.ndarray, n_steps x dof)
'''
if xyz_start is None or quat_start is None:
xyz_start, quat_start = joints2xyzquat(manip, link_name, joints_start)
if xyz_end is None or quat_end is None:
xyz_end, quat_end = joints2xyzquat(manip, link_name, joints_end)
request = move_arm_straight_request(manip, n_steps, link_name,
joints_start, joints_end, xyz_start,
quat_start, xyz_end, quat_end)
trajoptpy.SetInteractive(interactive)
prob = trajoptpy.ConstructProblem(json.dumps(request), env)
result = trajoptpy.OptimizeProblem(prob)
return result.GetTraj()
def move_arm_cart(env,
manip,
link_name,
xyzs,
quats,
init_soln,
interactive=False):
assert len(xyzs) == len(quats) == len(init_soln)
request = move_arm_cart_request(manip, link_name, xyzs, quats, init_soln)
trajoptpy.SetInteractive(interactive)
prob = trajoptpy.ConstructProblem(json.dumps(request), env)
result = trajoptpy.OptimizeProblem(prob)
return result.GetTraj()
def main():
args = parse_input_args()
setup_log_file(args)
set_global_vars(args)
print 'Setting Global Vars'
trajoptpy.SetInteractive(args.interactive)
lfd_env, sim = setup_lfd_environment_sim(args)
reg_and_traj_transferer = setup_registration_and_trajectory_transferer(
args, sim)
if args.subparser_name == "eval":
label_demos_parallel(args, reg_and_traj_transferer, lfd_env, sim)
else:
raise RuntimeError("Invalid subparser name")
def main():
args = parse_input_args()
setup_log_file(args)
sim_env = sim_util.SimulationEnv()
set_global_vars(args, sim_env)
trajoptpy.SetInteractive(args.interactive)
load_simulation(args, sim_env)
if args.subparser_name == "eval":
eval_on_holdout(args, sim_env)
elif args.subparser_name == "replay":
replay_on_holdout(args, sim_env)
else:
raise RuntimeError("Invalid subparser name")
def init_env(problemset):
env = openravepy.Environment()
env.StopSimulation()
robot2file = {"pr2": "robots/pr2-beta-static.zae"}
if args.planner == "trajopt":
if args.interactive: trajoptpy.SetInteractive(True)
plan_func = trajopt_plan
elif args.planner == "ompl":
setup_ompl(env)
plan_func = ompl_plan
elif args.planner in ["chomp", "chomp2"]:
setup_chomp(env)
plan_func = chomp_plan
# chomp needs a robot with spheres
chomp_pr2_file = "pr2_with_spheres.robot.xml" if problemset[
"active_affine"] == 0 else "pr2_with_spheres_fullbody.robot.xml"
robot2file["pr2"] = osp.join(pbc.envfile_dir, chomp_pr2_file)
env.Load(osp.join(pbc.envfile_dir, problemset["env_file"]))
env.Load(robot2file[problemset["robot_name"]])
robot = env.GetRobots()[0]
if args.planner == "trajopt":
try:
postsetup_trajopt(env)
except Exception:
print "can't find ros config file. skipping self collision ignore"
robot.SetTransform(
openravepy.matrixFromPose(problemset["default_base_pose"]))
rave_joint_names = [joint.GetName() for joint in robot.GetJoints()]
rave_inds, rave_values = [], []
for (name, val) in zip(problemset["joint_names"],
problemset["default_joint_values"]):
if name in rave_joint_names:
i = rave_joint_names.index(name)
rave_inds.append(i)
rave_values.append(val)
robot.SetDOFValues(rave_values, rave_inds)
active_joint_inds = [
rave_joint_names.index(name) for name in problemset["active_joints"]
]
robot.SetActiveDOFs(active_joint_inds, problemset["active_affine"])
return env, robot, plan_func
def do_traj_ik_graph_search_opt(pr2, lr, gripper_poses):
gripper_poses = [pose for i, pose in enumerate(gripper_poses) if i % 20 == 0]
graph_search_res = do_traj_ik_graph_search(pr2, lr, gripper_poses)
hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses]
poses = rave.poseFromMatrices(hmats)
quats = poses[:,:4]
xyzs = poses[:,4:7]
manip_name = {"r":"rightarm", "l":"leftarm"}[lr]
request = get_ik_request(manip_name, "%s_gripper_tool_frame"%lr,
xyzs[-1], quats[-1], graph_search_res, xyzs, quats)
s = json.dumps(request)
print "REQUEST:", s
pr2.robot.SetDOFValues(graph_search_res[0], pr2.robot.GetManipulator(manip_name).GetArmIndices())
trajoptpy.SetInteractive(True);
prob = trajoptpy.ConstructProblem(s, pr2.env)
result = trajoptpy.OptimizeProblem(prob)
def main():
demofile = h5py.File(args.demos_h5file, 'r')
trajoptpy.SetInteractive(False)
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
Globals.robot.SetDOFValues(PR2_L_POSTURES["side"], Globals.robot.GetManipulator("leftarm").GetArmIndices())
Globals.robot.SetDOFValues(mirror_arm_joints(PR2_L_POSTURES["side"]), Globals.robot.GetManipulator("rightarm").GetArmIndices())
init_rope_xyz, _ = load_random_start_segment(demofile)
init_rope_xyz_robot = apply_tfm(init_rope_xyz[()], args.inittfmfile)
table_height = init_rope_xyz_robot[:,2].mean() - .03
table_xml = make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01])
Globals.env.LoadData(table_xml)
Globals.sim = ropesim.Simulation(Globals.env, Globals.robot)
Globals.viewer = trajoptpy.GetViewer(Globals.env)
Globals.viewer.Idle()
if not args.same_as_training:
for i in range(0, args.dataset_size):
print "State ", i, " of ", args.dataset_size
rope_nodes, demo_id = sample_rope_state(demofile)
save_example_action(rope_nodes, demo_id)
else:
start_keys = [k for k in demofile.keys()
if k.startswith('demo') and k.endswith('00')]
for demo_id in start_keys:
xyz_camera = demofile[demo_id]['cloud_xyz'][()]
xyz_robot = apply_tfm(xyz_camera, args.inittfmfile)
rope_nodes = rope_initialization.find_path_through_point_cloud(
xyz_robot, perturb_peak_dist = None,
num_perturb_points = 0)
replace_rope(rope_nodes)
Globals.sim.settle()
Globals.viewer.Step()
new_xyz_robot = Globals.sim.observe_cloud()
save_example_action(new_xyz_robot, demo_id)
time.sleep(1)
def __init__(self, arm_name, sim=None, interact=False):
"""
:param arm_name: "left" or "right"
:param sim: OpenRave simulator (or create if None)
:param interact: enable trajopt viewer
"""
assert arm_name == 'left' or arm_name == 'right'
self.sim = sim
if self.sim is None:
self.sim = simulator.Simulator()
self.robot = self.sim.robot
self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm
if interact:
trajoptpy.SetInteractive(True)
self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0])
self.joint_indices = self.manip.GetArmIndices()
def main():
args = parse_input_args()
if args.subparser_name == "eval":
eval_util.save_results_args(args.resultfile, args)
elif args.subparser_name == "replay":
loaded_args = eval_util.load_results_args(args.replay.loadresultfile)
assert 'eval' not in vars(args)
args.eval = loaded_args.eval
else:
raise RuntimeError("Invalid subparser name")
setup_log_file(args)
set_global_vars(args)
trajoptpy.SetInteractive(args.interactive)
lfd_env, sim = setup_lfd_environment_sim(args)
reg_and_traj_transferer = setup_registration_and_trajectory_transferer(args, sim)
if args.eval.action_selection == 'feature':
get_features(args)
if args.eval.action_selection == 'greedy':
action_selection = GreedyActionSelection(reg_and_traj_transferer.registration_factory)
else:
action_selection = FeatureActionSelection(reg_and_traj_transferer.registration_factory, GlobalVars.features, GlobalVars.actions, GlobalVars.demos, simulator=reg_and_traj_transferer, lfd_env=lfd_env, width=args.eval.width, depth=args.eval.depth)
if args.subparser_name == "eval":
start = time.time()
if args.eval.parallel:
eval_on_holdout_parallel(args, action_selection, reg_and_traj_transferer, lfd_env, sim)
else:
eval_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim)
print "eval time is:\t{}".format(time.time() - start)
elif args.subparser_name == "replay":
replay_on_holdout(args, action_selection, reg_and_traj_transferer, lfd_env, sim)
else:
raise RuntimeError("Invalid subparser name")
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--lr", default="r")
parser.add_argument("--use_cost", action="store_true")
parser.add_argument("--interactive", action="store_true")
parser.add_argument("--no_prompt", action="store_true")
args = parser.parse_args()
trajoptpy.SetInteractive(
args.interactive
) # pause every iteration, until you press 'p'. Press escape to disable further plotting
rospy.init_node("pr2_arm")
with open('config/environment/pr2.yaml') as yaml_string:
pr2_env = utils.from_yaml(yaml_string)
if args.lr == 'r':
min_target_pos = np.array([.5, -.5, .8])
max_target_pos = np.array([.7, .2, 1])
else:
min_target_pos = np.array([.5, -.2, .8])
max_target_pos = np.array([.7, .5, 1])
while not rospy.is_shutdown():
if args.no_prompt or yes_or_no.yes_or_no("execute?"):
target_pos = utils.sample_interval(min_target_pos,
max_target_pos).tolist()
pr2_env.pr2.update_rave()
traj = planning.plan_up_trajectory(pr2_env.pr2.robot,
args.lr,
target_pos,
use_cost=args.use_cost)
bodypart2traj = {"%sarm" % args.lr: traj}
pr2_trajectories.follow_body_traj(pr2_env.pr2,
bodypart2traj,
speed_factor=0.1)
robot = env.GetRobots()[0]
init_transform = np.eye(4)
init_transform[:3,3] = [-.35, 1, .92712]
init_transform[:3,3] = [.1, 1, .92712]
#init_transform[:3,3] = [2.6, 1, .92712]
robot.SetTransform(init_transform)
robot.SetDOFValues(np.zeros(robot.GetDOF()))
robot.SetActiveDOFs(np.arange(robot.GetDOF()), rave.DOFAffine.Transform)
# move arms to side
robot.SetDOFValues([-1.3],[robot.GetJoint("l_arm_shx").GetDOFIndex()])
robot.SetDOFValues([1.3],[robot.GetJoint("r_arm_shx").GetDOFIndex()])
standing_posture = robot.GetActiveDOFValues()
##################
trajoptpy.SetInteractive(True)
cc = trajoptpy.GetCollisionChecker(env)
cc.ExcludeCollisionPair(robot.GetLink("l_foot"), env.GetKinBody("ProjectRoom").GetLink("Floor"))
cc.ExcludeCollisionPair(robot.GetLink("r_foot"), env.GetKinBody("ProjectRoom").GetLink("Floor"))
n_steps = 5
x_button_press = 2.55 # if robot is at this x-coordinate, he can reach the button
xyz_button = env.GetKinBody("bigredbutton").GetTransform()[:3,3]
xyz_button[2] += .15;
totaltraj = []
i=0
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("--interactive", action="store_true")
args = parser.parse_args()
rospack = rospkg.RosPack()
work_path = os.path.join(
os.path.join(rospack.get_path('irl_constraints_learning')),
'src/irl_constraints_learning/path_planning')
env = openravepy.Environment()
env.StopSimulation()
env.Load(os.path.join(work_path, "robots/baxter_structure.xml"))
trajoptpy.SetInteractive(
args.interactive
) # pause every iteration, until you press 'p'. Press escape to disable further plotting
#env.SetViewer('qtcoin') # start the viewer (conflicts with matplotlib)
robot = env.GetRobots()[0] # get the first robot
## manip = robot.GetManipulators()[0] # left_arm
print robot.GetManipulators('left_arm')[0].GetArmIndices()
joint_start = [0, 0, 0, 0, 0, 0, 0]
robot.SetDOFValues(joint_start,
robot.GetManipulators('left_arm')[0].GetArmIndices())
joint_target = [1.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
request = {
"basic_info": {
def plan(env_xml,
manip_name,
start,
goal,
traj=None,
c_fn=None,
eq_fn=None,
ineq_fn=None,
n_steps=100,
lower_limit=None,
upper_limit=None):
env = openravepy.Environment()
env.StopSimulation()
env.Load(env_xml)
trajoptpy.SetInteractive(False)
robot = env.GetRobots()[0] # get the first robot
robot.SetDOFValues(start,
robot.GetManipulators(manip_name)[0].GetArmIndices())
## if lower_limit is not None and upper_limit is not None:
## jnts = robot.GetJoints(robot.GetManipulators(limb+'_arm')[0].GetArmIndices())
## for i, jnt in enumerate(jnts):
## ## from IPython import embed; embed(); sys.exit()
## jnt.SetLimits(lower_limit[i:i+1], upper_limit[i:i+1])
if type(goal) is not list: goal = list(goal)
if traj is None:
init_type = 'straight_line'
traj = goal
else:
init_type = 'given_traj'
n_steps = len(traj)
if type(traj) is not list: traj = traj.tolist()
# cost and constraints
request = {
"basic_info": {
"n_steps": n_steps,
"manip": manip_name,
"start_fixed":
True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs": [
{
"type": "joint_vel", # joint-space velocity cost
"params": {
"coeffs": [1.]
} # a list of length one is automatically expanded to a list of length n_dofs
},
],
"constraints": [
{
"type": "joint", # joint-space target
"params": {
"vals": goal
} # length of vals = # dofs of manip
},
],
"init_info": {
"type": init_type,
"data": traj,
"endpoint": goal
}
}
# set qp
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(
s, env) # create object that stores optimization problem
n_dof = len(start)
# Set cost and constraints
if c_fn is not None:
for t in xrange(1, n_steps):
prob.AddErrorCost(c_fn, [(t, j) for j in xrange(n_dof)], "ABS",
"up%i" % t)
## # EQ: an equality constraint. `f(x) == 0` in a valid solution.
## if eq_fn is not None:
## for t in xrange(1,n_steps):
## prob.AddConstraint(eq_fn, [(t,j) for j in xrange(n_dof)], "EQ", "up%i"%t)
# INEQ: an inequality constraint. `f(x) <= `0` in a valid solution.
if ineq_fn is not None:
for t in xrange(3, n_steps):
if type(ineq_fn) is list:
for i in range(len(ineq_fn)):
prob.AddConstraint(ineq_fn[i],
[(t, j) for j in xrange(n_dof)], "INEQ",
"up%i" % t)
else:
prob.AddConstraint(ineq_fn, [(t, j) for j in xrange(n_dof)],
"INEQ", "up%i" % t)
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print "optimization took %.3f seconds" % t_elapsed
## from trajoptpy.check_traj import traj_is_safe
## prob.SetRobotActiveDOFs() # set robot DOFs to DOFs in optimization problem
## assert traj_is_safe(result.GetTraj(), robot) # Check that trajectory is collision free
## if result.GetTraj() is None or len(result.GetTraj())==0:
## from IPython import embed; embed() #; sys.exit()
## for cost in result.GetCosts():
## if cost[0].find('joint_vel')<0 and cost[1]>1.0:
## print "Optimization failed with high cost value {}".format(cost)
## return []
## for cstr in result.GetConstraints():
## if cstr[1]>0.001:
## print "Optimization failed with high constraint value {}".format(cstr)
## return []
return result.GetTraj()
def _Plan(self, robot, robot_checker, request,
traj_constraints=(), goal_constraints=(),
traj_costs=(), goal_costs=(),
interactive=False, constraint_threshold=1e-4,
sampling_func=VanDerCorputSampleGenerator, norm_order=2,
**kwargs):
"""
Plan to a desired configuration with Trajopt.
This function invokes the Trajopt planner directly on the specified
JSON request. This can be used to implement custom path optimization
algorithms.
Constraints and costs are specified as dicts of:
```
{
'f': [float] -> [float],
'dfdx': [float] -> [float],
'type': ConstraintType or CostType
'dofs': [int]
}
```
The input to f(x) and dfdx(x) is a vector of active DOF values used in
the planning problem. The output is a vector of costs, where the
value *increases* as a constraint or a cost function is violated or
unsatisfied.
See ConstraintType and CostType for descriptions of the various
function specifications and their expected behavior.
The `dofs` parameter can be used to specify a subset of the robot's
DOF indices that should be used. A ValueError is thrown if these
indices are not entirely contained in the current active DOFs of the
robot.
@param robot: the robot whose active DOFs will be used
@param request: a JSON planning request for Trajopt
@param traj_constraints: list of dicts of constraints that should be
applied over the whole trajectory
@param goal_constraints: list of dicts of constraints that should be
applied only at the last waypoint
@param traj_costs: list of dicts of costs that should be applied over
the whole trajectory
@param goal_costs: list of dicts of costs that should be applied only
at the last waypoint
@param interactive: pause every iteration, until you press 'p' or press
escape to disable further plotting
@param constraint_threshold: acceptable per-constraint violation error
@param sampling_func: sample generator to compute validity checks
@param norm_order: order of norm to use for collision checking
@returns traj: trajectory from current configuration to specified goal
"""
import json
import trajoptpy
from prpy import util
# Set up environment.
env = robot.GetEnv()
trajoptpy.SetInteractive(interactive)
# Trajopt's UserData gets confused if the same environment
# is cloned into multiple times, so create a scope to later
# remove all TrajOpt UserData keys.
try:
# Validate request and fill in request fields that must use
# specific values to work.
assert(request['basic_info']['n_steps'] is not None)
request['basic_info']['manip'] = 'active'
request['basic_info']['robot'] = robot.GetName()
request['basic_info']['start_fixed'] = True
n_steps = request['basic_info']['n_steps']
n_dofs = robot.GetActiveDOF()
i_dofs = robot.GetActiveDOFIndices()
# Convert dictionary into json-formatted string and create object
# that stores optimization problem.
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
# Add trajectory-wide costs and constraints to each timestep.
for t in xrange(1, n_steps):
for constraint in traj_constraints:
self._addFunction(prob, t, i_dofs, n_dofs, constraint)
for cost in traj_costs:
self._addFunction(prob, t, i_dofs, n_dofs, cost)
# Add goal costs and constraints.
for constraint in goal_constraints:
self._addFunction(prob, n_steps-1, i_dofs, n_dofs, constraint)
for cost in goal_costs:
self._addFunction(prob, n_steps-1, i_dofs, n_dofs, cost)
# Perform trajectory optimization.
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
logger.debug("Optimization took {:.3f} seconds".format(t_elapsed))
# Check for constraint violations.
for name, error in result.GetConstraints():
if error > constraint_threshold:
raise ConstraintViolationPlanningError(
name,
threshold=constraint_threshold,
violation_by=error)
# Check for the returned trajectory.
waypoints = result.GetTraj()
if waypoints is None:
raise PlanningError("Trajectory result was empty.")
# Convert the trajectory to OpenRAVE format.
traj = self._WaypointsToTraj(robot, waypoints)
# Check that trajectory is collision free.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
# Set robot DOFs to DOFs in optimization problem.
prob.SetRobotActiveDOFs()
checkpoints = util.GetLinearCollisionCheckPts(robot, traj,
norm_order=norm_order,
sampling_func=sampling_func)
for _, q_check in checkpoints:
self._checkCollisionForIKSolutions(
robot, robot_checker, [q_check])
# Convert the waypoints to a trajectory.
prpy.util.SetTrajectoryTags(traj, {
Tags.SMOOTH: True
}, append=True)
return traj
finally:
for body in env.GetBodies():
for key in TRAJOPT_USERDATA_KEYS:
body.RemoveUserData(key)
trajopt_env_userdata = env.GetKinBody('__trajopt_data__')
if trajopt_env_userdata is not None:
env.Remove(trajopt_env_userdata)
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)
trajoptpy.SetInteractive(args.interactive)
env = openravepy.Environment()
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
loadsuccess = env.Load(
osp.join(trajoptpy.data_dir, args.scenefile + ".env.xml"))
assert loadsuccess
robot = env.GetRobots()[0]
robot.SetDOFValues(dof_vals)
# import xml.etree.ElementTree as ET
# cc = trajoptpy.GetCollisionChecker(env)
# root = ET.parse("/opt/ros/groovy/share/pr2_moveit_config/config/pr2.srdf").getroot()
# disabled_elems=root.findall("disable_collisions")
# for elem in disabled_elems:
# linki = robot.GetLink(elem.get("link1"))
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.execution:
rospy.init_node("exec_task", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
subprocess.call("killall XnSensorServer", shell=True)
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
new_xyz = np.squeeze(demofile[args.fake_data_segment]["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(
args.fake_data_transform[3:6])
hmat[:3, 3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :]
else:
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
Globals.pr2.join_all()
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
################################
redprint("Finding closest demonstration")
if args.fake_data_segment:
seg_name = args.fake_data_segment
elif args.choice == "costs":
f, seg_name = choose_segment(demofile, new_xyz)
else:
seg_name = choose_segment(demofile, new_xyz)
seg_info = demofile[seg_name]
# redprint("using demo %s, description: %s"%(seg_name, seg_info["description"]))
################################
redprint("Generating end-effector trajectory")
if not args.choice == "costs":
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0, 1)))
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1, 1)))
f = registration.tps_rpm(old_xyz,
new_xyz,
plot_cb=tpsrpm_plot_cb,
plotting=1)
handles.extend(
plotting_openrave.draw_grid(Globals.env,
f.transform_points,
old_xyz.min(axis=0),
old_xyz.max(axis=0),
xres=.1,
yres=.1,
zres=.04))
link2eetraj = {}
for lr in 'lr':
link_name = "%s_gripper_tool_frame" % lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
if not args.choice == "costs":
handles.append(
Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2,
(1, 0, 0, 1)))
handles.append(
Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2,
(0, 1, 0, 1)))
# TODO plot
# plot_warping_and_trajectories(f, old_xyz, new_xyz, old_ee_traj, new_ee_traj)
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
print colorize.colorize("mini segments:",
"red"), miniseg_starts, miniseg_ends
for (i_miniseg,
(i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if args.execution == "real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i" %
(seg_name, i_miniseg))
bodypart2traj = {}
arms_used = ""
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj = asarray(seg_info[manip_name][i_start:i_end +
1])
if arm_moved(old_joint_traj):
ee_link_name = "%s_gripper_tool_frame" % lr
new_ee_traj = link2eetraj[ee_link_name]
if args.execution: Globals.pr2.update_rave()
new_joint_traj = plan_follow_traj(
Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name),
new_ee_traj[i_start:i_end + 1], old_joint_traj)
# (robot, manip_name, ee_link, new_hmats, old_traj):
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
arms_used += lr
################################
redprint(
"Executing joint trajectory for segment %s, part %i using arms '%s'"
% (seg_name, i_miniseg, arms_used))
for lr in 'lr':
set_gripper_maybesim(
lr,
binarize_gripper(seg_info["%s_gripper_joint" %
lr][i_start]))
#trajoptpy.GetViewer(Globals.env).Idle()
if len(bodypart2traj) > 0:
exec_traj_maybesim(bodypart2traj)
# TODO measure failure condtions
if not success:
break
redprint("Segment %s result: %s" % (seg_name, success))
if args.fake_data_segment: break
def PlanToTSR(self, robot, tsrchains, is_interactive=False, **kw_args):
"""
Plan using the given list of TSR chains with TrajOpt.
@param robot the robot whose active manipulator will be used
@param tsrchains a list of TSRChain objects to respect during planning
@param is_interactive pause every iteration, until you press 'p'
or press escape to disable further plotting
@return traj
"""
import json
import time
import trajoptpy
manipulator = robot.GetActiveManipulator()
n_steps = 20
n_dofs = robot.GetActiveDOF()
# Create separate lists for the goal and trajectory-wide constraints.
goal_tsrs = [t for t in tsrchains if t.sample_goal]
traj_tsrs = [t for t in tsrchains if t.constrain]
# Find an initial collision-free IK solution by sampling goal TSRs.
from openravepy import (IkFilterOptions, IkParameterization,
IkParameterizationType)
for tsr in self._TsrSampler(goal_tsrs):
ik_param = IkParameterization(tsr.sample(),
IkParameterizationType.Transform6D)
init_joint_config = manipulator.FindIKSolutions(
ik_param, IkFilterOptions.CheckEnvCollisions)
if init_joint_config:
break
if not init_joint_config:
raise PlanningError('No collision-free IK solutions.')
# Construct a planning request with these constraints.
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "active",
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
}, {
"type": "collision",
"params": {
"coeffs": [20],
"dist_pen": [0.025]
},
}],
"constraints": [],
"init_info": {
"type": "straight_line",
"endpoint": init_joint_config.tolist()
}
}
# Set up environment.
env = robot.GetEnv()
trajoptpy.SetInteractive(is_interactive)
# Convert dictionary into json-formatted string and create object that
# stores optimization problem.
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
for t in xrange(1, n_steps):
prob.AddConstraint(constraints._TsrCostFn(robot, traj_tsrs),
[(t, j) for j in xrange(n_dofs)], "EQ",
"up{:d}".format(t))
prob.AddConstraint(constraints._TsrCostFn(robot, goal_tsrs),
[(n_steps - 1, j) for j in xrange(n_dofs)], "EQ",
"up{:d}".format(t))
# Perform trajectory optimization.
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
logger.info("Optimization took {:.3f} seconds".format(t_elapsed))
# Check for constraint violations.
if result.GetConstraints():
raise PlanningError(
"Trajectory did not satisfy constraints: {:s}".format(
str(result.GetConstraints())))
# Check for the returned trajectory.
waypoints = result.GetTraj()
if waypoints is None:
raise PlanningError("Trajectory result was empty.")
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
# Set robot DOFs to DOFs in optimization problem
prob.SetRobotActiveDOFs()
# Check that trajectory is collision free
from trajoptpy.check_traj import traj_is_safe
if not traj_is_safe(waypoints, robot):
return PlanningError("Result was in collision.")
# Convert the waypoints to a trajectory.
return self._WaypointsToTraj(robot, waypoints)
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.log:
LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"),
datetime.datetime.now().strftime("%Y%m%d-%H%M%S"))
os.mkdir(LOG_DIR)
LOG_COUNT = 0
if args.execution:
rospy.init_node("exec_task", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
fake_seg = demofile[args.fake_data_segment]
new_xyz = np.squeeze(fake_seg["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(
args.fake_data_transform[3:6])
hmat[:3, 3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :]
r2r = ros2rave.RosToRave(Globals.robot,
asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot,
asarray(fake_seg["joint_states"]["position"][0]))
else:
Globals.pr2.head.set_pan_tilt(0, 1.2)
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
Globals.pr2.join_all()
time.sleep(.5)
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
#grab_end(new_xyz)
if args.log:
LOG_COUNT += 1
import cv2
cv2.imwrite(osp.join(LOG_DIR, "rgb%i.png" % LOG_COUNT), rgb)
cv2.imwrite(osp.join(LOG_DIR, "depth%i.png" % LOG_COUNT), depth)
np.save(osp.join(LOG_DIR, "xyz%i.npy" % LOG_COUNT), new_xyz)
################################
redprint("Finding closest demonstration")
if args.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s" % (seg_name))
if "done" in seg_name:
redprint("DONE!")
break
if args.log:
with open(osp.join(LOG_DIR, "neighbor%i.txt" % LOG_COUNT),
"w") as fh:
fh.write(seg_name)
################################
redprint("Generating end-effector trajectory")
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0)))
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f, _ = registration.tps_rpm_bij(scaled_old_xyz,
scaled_new_xyz,
plot_cb=tpsrpm_plot_cb,
plotting=5 if args.animation else 0,
rot_reg=np.r_[1e-4, 1e-4, 1e-1],
n_iter=50,
reg_init=10,
reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
handles.extend(
plotting_openrave.draw_grid(Globals.env,
f.transform_points,
old_xyz.min(axis=0) - np.r_[0, 0, .1],
old_xyz.max(axis=0) + np.r_[0, 0, .1],
xres=.1,
yres=.1,
zres=.04))
link2eetraj = {}
for lr in 'lr':
link_name = "%s_gripper_tool_frame" % lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
handles.append(
Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2,
(1, 0, 0, 1)))
handles.append(
Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2,
(0, 1, 0, 1)))
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
print colorize.colorize("mini segments:",
"red"), miniseg_starts, miniseg_ends
for (i_miniseg,
(i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if args.execution == "real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i" %
(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
lr2oldtraj = {}
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj = asarray(seg_info[manip_name][i_start:i_end +
1])
#print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end
if arm_moved(old_joint_traj):
lr2oldtraj[lr] = old_joint_traj
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
_, timesteps_rs = unif_resample(old_total_traj,
JOINT_LENGTH_PER_STEP)
####
### Generate fullbody traj
bodypart2traj = {}
for (lr, old_joint_traj) in lr2oldtraj.items():
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj_rs = mu.interp2d(timesteps_rs,
np.arange(len(old_joint_traj)),
old_joint_traj)
ee_link_name = "%s_gripper_tool_frame" % lr
new_ee_traj = link2eetraj[ee_link_name][i_start:i_end + 1]
new_ee_traj_rs = resampling.interp_hmats(
timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
if args.execution: Globals.pr2.update_rave()
new_joint_traj = planning.plan_follow_traj(
Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,
old_joint_traj_rs)
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
################################
redprint(
"Executing joint trajectory for segment %s, part %i using arms '%s'"
% (seg_name, i_miniseg, bodypart2traj.keys()))
for lr in 'lr':
success &= set_gripper_maybesim(
lr,
binarize_gripper(seg_info["%s_gripper_joint" %
lr][i_start]))
# Doesn't actually check if grab occurred, unfortunately
if not success: break
if len(bodypart2traj) > 0:
success &= exec_traj_maybesim(bodypart2traj)
if not success: break
redprint("Segment %s result: %s" % (seg_name, success))
if args.fake_data_segment: break
def smoothTrajectory(self, configStart, configsPair, constraints, maxCSpaceJump, mover, \
viewEffector, visualize):
'''Runs trajectory optimization with viewing constraints.
- Input configStart: Initial configuration in trajectory/current configuration of the arm.
- Input configsPair: A pair of lists of configurations from which to choose the viewing points.
- Input constraints: A pair of ViewConstraint objects for each viewing point.
- Input maxCSpaceJump: Estimated c-space distance between waypoints in the trajectory (radians).
- Input mover: ArmMover object for computing IK and checking collisions.
- Input viewEffector: Name of the sensor link in OpenRAVE.
- Input cloud: Point cloud (nx3 array) to add as obstacle. Removes before function returns. Ignores if None.
- Input visualize: True to run the OpenRAVE visualization for the trajopt run.
- Returns trajectory: List of 7-DOF configurations according to OpenRAVE ordering.
- Returns essentialIndices: Indices in the trajectory that correspond to beginning, viewing (x2), and ending points.
- Returns cost: The L2 length of the trajectory (radians).
- Returns inCollision: True if the resulting trajectory has a collision, according to controller.
- Returns meetsPosition: True if the resulting trajectory goes through the viewing points.
- Returns meetsKeepout: True if the trajectory between viewing points is outside of the keepout.
- Returns meetsOrient: True if the trajectory between viewing points looks at the target point.
'''
trajoptpy.SetInteractive(visualize)
if mover.isInCollision(configStart):
raise Exception(
"The current position of the arm is believed to be in collision."
)
# 1. Deduce waypoint indices, trajectory length, and initialization.
pointsPerRadian = int(round(1.0 / maxCSpaceJump))
initConfigs = [configStart]
essentialIndices = [0]
pair = motion_planner.ClosestPair(configsPair[0], configsPair[1])
selectedConfigs = [configStart, pair[0], pair[1]]
for i in xrange(1, len(selectedConfigs)):
nextConfig = selectedConfigs[i]
prevConfig = initConfigs[-1]
delta = nextConfig - prevConfig
nSteps = int(round(norm(delta) * pointsPerRadian))
essentialIndices.append(essentialIndices[-1] + nSteps + 1)
for j in xrange(nSteps):
initConfigs.append(
(prevConfig +
(float(j + 1) / float(nSteps + 1)) * delta).tolist())
initConfigs.append(nextConfig.tolist())
# 2. Specify optimization problem and constraints.
trajLen = essentialIndices[-1] + 1
request = {
"basic_info": {
"n_steps": trajLen,
"manip": mover.name + "_arm",
"start_fixed": True
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [4]
}
}, {
"type": "collision",
"name": "cont_coll",
"params": {
"continuous": True,
"coeffs": [4],
"dist_pen": [0.02]
}
}],
"constraints": [],
"init_info": {
"type": "given_traj",
"data": initConfigs
}
}
# add waypoint constraints
for i in xrange(2):
timeStep = essentialIndices[i + 1]
request["constraints"].append({
"type": "joint",
"params": {
"vals": initConfigs[timeStep],
"timestep": timeStep
}
})
# construct problem
mover.robot.SetDOFValues(configStart, mover.manip.GetArmIndices())
initConfigs[0] = mover.robot.GetDOFValues().tolist(
) # wtf? Errors sometimes without this.
problem = trajoptpy.ConstructProblem(json.dumps(request), mover.env)
# joint limit constraints
for i in xrange(1, essentialIndices[2] + 1):
problem.AddConstraint(
constraints[0].GetDistanceFromLowerJointLimits,
[(i, j) for j in xrange(7)], "INEQ", "jointLo_{}".format(i))
problem.AddConstraint(
constraints[0].GetDistanceFromUpperJointLimits,
[(i, j) for j in xrange(7)], "INEQ", "jointHi_{}".format(i))
# viewing constraints
for i in xrange(essentialIndices[1] + 1, essentialIndices[2]):
constraint = constraints[
0] if i < essentialIndices[2] else constraints[1]
problem.AddConstraint(constraint.GetKeepoutPenalty,
[(i, j) for j in xrange(7)], "INEQ",
"viewKeep_{}".format(i))
problem.AddConstraint(constraint.GetOrientationPenalty,
[(i, j) for j in xrange(7)], "EQ",
"viewOrient_{}".format(i))
# 3. Solve optimization problem and return result.
result = trajoptpy.OptimizeProblem(problem)
trajectory = result.GetTraj()
# check result
cost = motion_planner.ComputePathCost(trajectory)
collision = motion_planner.IsPathInCollision(trajectory,
mover,
ignoreSoftCollision=True)
meetsPosition = norm(initConfigs[essentialIndices[1]]-trajectory[essentialIndices[1]]) < 0.005 \
and norm(initConfigs[essentialIndices[2]]-trajectory[essentialIndices[2]]) < 0.005
meetsKeepout = constraints[0].PathSatisfiesKeepout(\
trajectory[essentialIndices[1]+1:essentialIndices[2]])
meetsOrient = constraints[0].PathSatisfiesOrientation(\
trajectory[essentialIndices[1]+1:essentialIndices[2]])
if collision:
print("Detected a collision in the trajectory")
return trajectory, essentialIndices, cost, collision, meetsPosition, meetsKeepout, meetsOrient
handles.append(env.plot3(points=ppe_trans_arr[tb.state_change_index][0:3,3],
pointsize=5.0,
colors=array(((0,1,0)))))
print ('STATE CHANGE INDEX IS',tb.state_change_index)
if __name__ == "__main__":
env = Environment()
env.SetViewer('qtcoin')
env.Reset()
env.Load("/home/anto/ebolabot_ws/src/task_breakdown_openrave/src/task_breakdown_openrave/empty.env.xml")
time.sleep(0.1)
InitOpenRAVELogging()
trajoptpy.SetInteractive(False)
robot = env.GetRobots()[0]
human = env.GetRobots()[1]
goggles = env.GetRobots()[2]
forehead = env.GetKinBody('forehead')
tb = TaskBreakdown()
gr = GogglesRobot()
gr.Init(env, goggles)
handles = []
tb.Init(robot, env, human, goggles, gr, forehead, handles)
tuckarms(env, robot)
ppe_loc = []
def __init__(self, interactive):
env = openravepy.Environment()
env.StopSimulation()
print "Loading robot model"
env.Load("model/model_test.xml")
print "loaded robot model"
print "Loading environment"
env.Load("environment/env.xml")
print "Environment loaded"
trajoptpy.SetInteractive(
True
) # pause every iteration, until you press 'p'. Press escape to disable further plotting
robot = env.GetRobots()[0]
print robot.GetManipulator("arm")
print robot.GetManipulator('arm').GetArmIndices()
joint_start = [0.0, 0.0]
robot.SetDOFValues(joint_start,
robot.GetManipulator('arm').GetArmIndices())
joint_target = [-np.pi / 2.0, 0.0]
n_steps = 100
control_rate = 30.0
delta_t = 1.0 / control_rate
max_velocity = 2.0
request = {
"basic_info": {
"n_steps": n_steps,
"manip": "arm", # see below for valid values
"start_fixed":
True # i.e., DOF values at first timestep are fixed based on current robot state
},
"costs": [
{
"type": "joint_vel", # joint-space velocity cost
"params": {
"coeffs": [1]
} # a list of length one is automatically expanded to a list of length n_dofs
# also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1]
},
{
"type": "collision",
"params": {
"coeffs": [
20
], # penalty coefficients. list of length one is automatically expanded to a list of length n_timesteps
"dist_pen": [
0.25
] # robot-obstacle distance that penalty kicks in. expands to length n_timesteps
},
}
],
"constraints": [
{
"type": "joint", # joint-space target
"params": {
"vals": joint_target
} # length of vals = # dofs of manip
},
{
"type": "joint_vel_limits",
"name": "joint_vel_limits",
"params": {
"vals":
[delta_t * max_velocity, delta_t * max_velocity],
"first_step": 0,
"last_step": n_steps - 1, #inclusive
}
}
],
"init_info": {
"type": "straight_line", # straight line in joint space.
"endpoint": joint_target
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(
s, env) # create object that stores optimization problem
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
print result
print "optimization took %.3f seconds" % t_elapsed
prob.SetRobotActiveDOFs(
) # set robot DOFs to DOFs in optimization problem
print traj_is_safe(result.GetTraj(),
robot) # Check that trajectory is collision free
def _Plan(self,
robot,
request,
traj_constraints=(),
goal_constraints=(),
traj_costs=(),
goal_costs=(),
interactive=False,
constraint_threshold=1e-4,
**kwargs):
"""
Plan to a desired configuration with Trajopt.
This function invokes the Trajopt planner directly on the specified
JSON request. This can be used to implement custom path optimization
algorithms.
Constraints and costs are specified as dicts of:
```
{
'f': [float] -> [float],
'dfdx': [float] -> [float],
'type': ConstraintType or CostType
'dofs': [int]
}
```
The input to f(x) and dfdx(x) is a vector of active DOF values used in
the planning problem. The output is a vector of costs, where the
value *increases* as a constraint or a cost function is violated or
unsatisfied.
See ConstraintType and CostType for descriptions of the various
function specifications and their expected behavior.
The `dofs` parameter can be used to specify a subset of the robot's
DOF indices that should be used. A ValueError is thrown if these
indices are not entirely contained in the current active DOFs of the
robot.
@param robot: the robot whose active DOFs will be used
@param request: a JSON planning request for Trajopt
@param traj_constraints: list of dicts of constraints that should be
applied over the whole trajectory
@param goal_constraints: list of dicts of constraints that should be
applied only at the last waypoint
@param traj_costs: list of dicts of costs that should be applied over
the whole trajectory
@param goal_costs: list of dicts of costs that should be applied only
at the last waypoint
@param interactive: pause every iteration, until you press 'p' or press
escape to disable further plotting
@param constraint_threshold: acceptable per-constraint violation error
@returns traj: trajectory from current configuration to specified goal
"""
import json
import trajoptpy
# Set up environment.
env = robot.GetEnv()
trajoptpy.SetInteractive(interactive)
# Trajopt's UserData gets confused if the same environment
# is cloned into multiple times, so create a scope to later
# remove all TrajOpt UserData keys.
try:
# Convert dictionary into json-formatted string and create object
# that stores optimization problem.
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
assert (request['basic_info']['manip'] == 'active')
assert (request['basic_info']['n_steps'] is not None)
n_steps = request['basic_info']['n_steps']
n_dofs = robot.GetActiveDOF()
i_dofs = robot.GetActiveDOFIndices()
# Add trajectory-wide costs and constraints to each timestep.
for t in xrange(1, n_steps):
for constraint in traj_constraints:
self._addFunction(prob, t, i_dofs, n_dofs, constraint)
for cost in traj_costs:
self._addFunction(prob, t, i_dofs, n_dofs, cost)
# Add goal costs and constraints.
for constraint in goal_constraints:
self._addFunction(prob, n_steps - 1, i_dofs, n_dofs,
constraint)
for cost in goal_costs:
self._addFunction(prob, n_steps - 1, i_dofs, n_dofs, cost)
# Perform trajectory optimization.
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
logger.debug("Optimization took {:.3f} seconds".format(t_elapsed))
# Check for constraint violations.
for name, error in result.GetConstraints():
if error > constraint_threshold:
raise PlanningError(
"Trajectory violates contraint '{:s}': {:f} > {:f}".
format(name, error, constraint_threshold))
# Check for the returned trajectory.
waypoints = result.GetTraj()
if waypoints is None:
raise PlanningError("Trajectory result was empty.")
# Set active DOFs to match active manipulator and plan.
p = openravepy.KinBody.SaveParameters
with robot.CreateRobotStateSaver(p.ActiveDOF):
# Set robot DOFs to DOFs in optimization problem
prob.SetRobotActiveDOFs()
# Check that trajectory is collision free
from trajoptpy.check_traj import traj_is_safe
if not traj_is_safe(waypoints, robot):
raise PlanningError("Result was in collision.")
# Convert the waypoints to a trajectory.
return self._WaypointsToTraj(robot, waypoints)
finally:
for body in env.GetBodies():
for key in TRAJOPT_USERDATA_KEYS:
body.RemoveUserData(key)
trajopt_env_userdata = env.GetKinBody('__trajopt_data__')
if trajopt_env_userdata is not None:
env.Remove(trajopt_env_userdata)
