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 optimize_ompl_trajopt(self, joint_target, algorithm):
"""
Optimization of the robot via initialization of motion through different way points
We start by perform intermediate way point calculations using the OMPL planner with given algorithm
Optimization of the trajectory is done through a call to TrajOpt
Params:
joint_target <list><list><float> : A series of joint goals combined to form a list
algorithm <string> : OMPL_*** algorithm. If 'RRTConnect', then we are using the 'OML_RRTConnect' algorithm
"""
# self.__update_DOF() # Call to refresh DOF of the robot
trajectory = self.__init_traj(joint_target=joint_target, algorithm=algorithm) # Performs initial OMPL_*** planning
self.traj = []
trajectory_interp = self.__interpolate(trajectory=trajectory, n=10) # Interpolation between waypoints generated by OMPL for finer resolution
self.__set_request(joint_goal=trajectory[-1], n_steps=len(trajectory_interp))
self.request.update({"init_info" : {"type" : "given_traj", "data" : trajectory_interp}}) # Way point initialization after path planning
jd = json.dumps(self.request)
prob = trajoptpy.ConstructProblem(jd, self.env)
result = trajoptpy.OptimizeProblem(prob)
# if self.__check_safe(result.GetTraj()):
# pass
# else:
# raise Exception('No path is safe')
#final_trajectory = planner.optimize_ompl_trajopt(joint_target=joint_target, algorithm="RRTstar")
return result.GetTraj().tolist()
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 TrajoptPath(self, end_t): # Planner assumes current state of robot in OpenRAVE is the initial state request = self.make_fullbody_request(end_t, 23) print request s = json.dumps(request) # convert dictionary into json-formatted string prob = trajoptpy.ConstructProblem(s, self.env) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj()
def single_trial(inittraj, use_discrete_collision):
s = make_trajopt_request(n_steps, coll_coeff, dist_pen, end_joints,
inittraj, use_discrete_collision)
robot.SetActiveDOFValues(inittraj[0, :])
prob = trajoptpy.ConstructProblem(s, robot.GetEnv())
result = trajoptpy.OptimizeProblem(prob)
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
return traj, traj_is_safe(
traj, robot
) #and (use_discrete_collision or traj_no_self_collisions(traj, robot))
def run_opt(request,env):
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, env)
tic = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
toc = time.time()
print("Optimization is completed in {} s!".format(toc-tic))
duration = toc-tic
return duration, result
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():
env = openravepy.Environment()
env.Load(osp.join(trajoptpy.data_dir, "kitchen.env.xml"))
env.Load("robots/pr2-beta-static.zae")
env.StopSimulation()
robot = env.GetRobots()[0]
# Set up robot in initial state
robot.SetDOFValues([ 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00,
0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00,
0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00, 0.000e+00,
6.648e-01, -3.526e-01, 1.676e+00, -1.924e+00, 2.921e+00,
-1.217e+00, 1.343e+00, 0.000e+00, -4.163e-16, 0.000e+00,
-1.665e-16, 0.000e+00, -6.365e-01, 9.806e-02, -1.226e+00,
-2.026e+00, -3.012e+00, -1.396e+00, -1.929e+00, 0.000e+00,
2.776e-17, 0.000e+00, -3.331e-16, 0.000e+00])
robot.SetTransform(np.array([[-1. , 0.005, 0. , 2.93 ],
[-0.005, -1. , 0. , 0.575],
[ 0. , 0. , 1. , 0. ],
[ 0. , 0. , 0. , 1. ]]))
DOF = openravepy.DOFAffine
robot.SetActiveDOFs(np.r_[robot.GetJoint("torso_lift_joint").GetDOFIndex(), robot.GetManipulator("leftarm").GetArmIndices(), robot.GetManipulator("rightarm").GetArmIndices()],
DOF.X | DOF.Y | DOF.RotationAxis)
robot.SetActiveDOFValues([ 0. , 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166,
1.3425, -0.6365, 0.0981, -1.226 , -2.0264, -3.0125, -1.3958,
-1.9289, 2.9295, 0.5748, -3.137 ])
target_joints = [ 0. , 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005,
1.5994, -0.6929, -0.3338, -1.292 , -1.9048, -2.6915, -1.2908,
-1.7152, 1.3155, 0.6877, -0.0041]
# Planner assumes current state of robot in OpenRAVE is the initial state
request = make_fullbody_request(target_joints)
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do optimization
traj = result.GetTraj()
success = traj_is_safe(traj, robot)
if success:
print "trajectory is safe! :)"
else:
print "trajectory contains a collision :("
if args.interactive: animate_traj(traj, robot)
assert success
def planTrajopt(self, configStart, configEnd, arm):
'''TODO'''
print("Planning with trajopt ...")
trajLen = int(
(1.0 / self.maxCSpaceJump) * norm(configStart - configEnd)) + 2
request = {
"basic_info": {
"n_steps": trajLen,
"manip": arm.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": [{
"type": "joint",
"params": {
"vals": configEnd.tolist()
}
}],
"init_info": {
"type": "straight_line",
"endpoint": configEnd.tolist()
}
}
arm.robot.SetDOFValues(configStart, arm.manip.GetArmIndices())
s = json.dumps(
request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(s, arm.env)
result = trajoptpy.OptimizeProblem(prob)
trajectory = result.GetTraj()
return trajectory
def computeTrajToPose(self,target):
params = myIK(target)
request = {
"basic_info" : {
"n_steps" : 10,
"manip" : "grip",
"start_fixed" : True
},
"costs" : [
{
"type" : "joint_vel",
"params": {"coeffs" : [1]}
},
{
"type" : "collision",
"name" : "cont_coll",
"params" : {
"continuous" : True,
"coeffs" : [20],
"dist_pen" : [0.025]
},
}
],
"constraints" : [
{
"type" : "pose",
"params" : {"xyz" : params['xyz'].tolist(),
"wxyz" : params['quat'].tolist(),
"link" : "Palm",
"timestep" : 9
}
}
],
"init_info" : {
"type" : "straight_line",
"endpoint" : params['joints'].tolist()
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, self.env)
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob)
t_elapsed = time.time() - t_start
print "optimization took %.3f seconds"%t_elapsed
return result.GetTraj(), params
def solve_problem(self, sessionkey, jsonstr):
"""
Solve problem specified by a json file
"""
env = self.sesh2data[sessionkey]["env"]
prob = trajoptpy.ConstructProblem(jsonstr, env)
t_start = time.time()
result = trajoptpy.OptimizeProblem(prob) # do optimization
out = {}
out["solve_time"] = time.time() - t_start
traj = result.GetTraj()
costs = result.GetCosts()
constraints = result.GetConstraints()
out["traj"] = [row.tolist() for row in traj]
out["costs"] = costs
out["constraints"] = constraints
self._update_last_used(sessionkey)
return out
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 optimize(self, manip, joint_target, algorithm):
"""
Optimization of the robot via initialization of motion through different way points
We start by perform intermediate way point calculations using the OMPL planner with given algorithm
Optimization of the trajectory is done through a call to TrajOpt
Params:
manip <string> : either 'left_arm' or 'right_arm'
joint_target <list><list><float> : return type from IK.get_joint_DOF representing the goal state of manip
algorithm <string> : OMPL_*** algorithm. If 'RRTConnect', then we are using the 'OML_RRTConnect' algorithm
"""
self.__update_DOF() # Call to refresh DOF of the robot
trajectory = self.__init_traj(
manip=manip, joint_target=joint_target,
algorithm=algorithm) # Performs initial OMPL_*** planning
self.traj = []
trajectory_interp = self.__interpolate(
trajectory=trajectory, n=10
) # Interpolation between waypoints generated by OMPL for finer resolution
self.__set_request(manip=manip,
joint_goal=trajectory[-1],
n_steps=len(trajectory_interp))
self.request.update(
{"init_info": {
"type": "given_traj",
"data": trajectory_interp
}}) # Way point initialization after path planning
jd = json.dumps(
self.request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(
jd, self.env) # create object that stores optimization problem
result = trajoptpy.OptimizeProblem(prob) # do Optimization
if self.__check_safe(result.GetTraj()):
pass
else:
raise Exception('No path is safe')
self.traj = result.GetTraj().tolist()
return
def get_joint_trajectory(self, start_joints, target_pose, n_steps=20):
"""
Calls trajopt to plan collision-free trajectory
:param start_joints: list of initial joints
:param target_pose: desired pose of tool_frame (tfx.pose)
:param n_steps: trajopt discretization
:return list of joint values
"""
assert len(start_joints) == len(self.joint_indices)
assert target_pose.frame.count('base_link') == 1
self.sim.update()
# set start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
# initialize trajopt inputs
rave_pose = tfx.pose(self.sim.transform_from_to(target_pose.matrix, target_pose.frame, 'world'))
quat = rave_pose.orientation
xyz = rave_pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target])
#init_joint_target = ku.ik_for_link(rave_mat, self.manip, self.tool_frame, filter_options=rave.IkFilterOptions.CheckEnvCollisions)
#if init_joint_target is None:
# rospy.loginfo('get_traj: IK failed')
# return False
init_joint_target = None
request = self._get_trajopt_request(xyz_target, quat_target, init_joint_target, n_steps)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
return result.GetTraj()
# also valid: [1.9, 2, 3, 4, 5, 5, 4, 3, 2, 1]
}],
"constraints": [{
"type": "joint", # joint-space target
"params": {
"vals": joint_target
} # length of vals = # dofs of manip
}],
"init_info": {
"type": "straight_line", # straight line in joint space.
"endpoint": joint_target
}
}
s = json.dumps(request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(
s, env) # create object that stores optimization problem
print "--------------------------------------"
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
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
print result.GetTraj()
print np.shape(result.GetTraj())
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 _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)
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 trajOpt(self, start, goal, goal_pose, traj_seed=None):
"""
Computes a plan from start to goal using trajectory optimizer.
Reference: http://joschu.net/docs/trajopt-paper.pdf
---
Paramters:
start -- The start position.
goal -- The goal position.
goal_pose -- The goal pose (optional: can be None).
traj_seed [optiona] -- An optional initial trajectory seed.
Returns:
waypts_plan -- A downsampled trajectory resulted from the TrajOpt
optimization problem solution.
"""
# --- Initialization --- #
if len(start) < 10:
aug_start = np.append(start.reshape(7), np.array([0, 0, 0]))
self.environment.robot.SetDOFValues(aug_start)
# --- Linear interpolation seed --- #
if traj_seed is None:
# print("Using straight line initialization!")
init_waypts = np.zeros((self.num_waypts, 7))
for count in range(self.num_waypts):
init_waypts[count, :] = start + count / (self.num_waypts -
1.0) * (goal - start)
else:
print("Using trajectory seed initialization!")
init_waypts = traj_seed
# --- Request construction --- #
# If pose is given, must include pose constraint.
if goal_pose is not None:
# print("Using goal pose for trajopt computation.")
xyz_target = goal_pose
quat_target = [1, 0, 0, 0] #wxyz
constraint = [{
"type": "pose",
"params": {
"xyz": xyz_target,
"wxyz": quat_target,
"link": "j2s7s300_link_7",
"rot_coeffs": [0, 0, 0],
"pos_coeffs": [35, 35, 35],
}
}]
else:
# print("Using goal for trajopt computation.")
constraint = [{"type": "joint", "params": {"vals": goal.tolist()}}]
request = {
"basic_info": {
"n_steps": self.num_waypts,
"manip": "j2s7s300",
"start_fixed": True,
"max_iter": self.MAX_ITER
},
"costs": [{
"type": "joint_vel",
"params": {
"coeffs": [0.22]
}
}],
"constraints": constraint,
"init_info": {
"type": "given_traj",
"data": init_waypts.tolist()
}
}
s = json.dumps(request)
prob = trajoptpy.ConstructProblem(s, self.environment.env)
for t in range(1, self.num_waypts):
if 'coffee' in self.environment.feature_list:
prob.AddCost(self.coffee_cost,
[(t - 1, j)
for j in range(7)] + [(t, j) for j in range(7)],
"coffee%i" % t)
if 'table' in self.environment.feature_list:
prob.AddCost(self.table_cost,
[(t - 1, j)
for j in range(7)] + [(t, j) for j in range(7)],
"table%i" % t)
if 'laptop' in self.environment.feature_list:
prob.AddCost(self.laptop_cost,
[(t - 1, j)
for j in range(7)] + [(t, j) for j in range(7)],
"laptop%i" % t)
if 'origin' in self.environment.feature_list:
prob.AddCost(self.origin_cost,
[(t - 1, j)
for j in range(7)] + [(t, j) for j in range(7)],
"origin%i" % t)
if 'human' in self.environment.feature_list:
prob.AddCost(self.human_cost,
[(t - 1, j)
for j in range(7)] + [(t, j) for j in range(7)],
"human%i" % t)
if 'efficiency' in self.environment.feature_list:
prob.AddCost(self.efficiency_cost,
[(t - 1, j)
for j in range(7)] + [(t, j) for j in range(7)],
"efficiency%i" % t)
if 'learned_feature' in self.environment.feature_list:
prob.AddErrorCost(self.learned_feature_costs,
self.learned_feature_cost_derivatives,
[(t - 1, j)
for j in range(7)] + [(t, j)
for j in range(7)],
"ABS", "learned_features%i" % t)
# [(t-1, j) for j in range(7)]+
for t in range(1, self.num_waypts - 1):
prob.AddConstraint(self.environment.table_constraint,
[(t, j) for j in range(7)], "INEQ", "up%i" % t)
result = trajoptpy.OptimizeProblem(prob)
return result.GetTraj()
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 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 __construct_problem(request): jd = json.dumps(request) # convert dictionary into json-formatted string prob = trajoptpy.ConstructProblem(jd, self.env) # create object that stores optimization problem return trajoptpy.OptimizeProblem(prob) # do optimization
def get_grasp_joint_trajectory(self,
start_joints,
target_pose,
n_steps=40,
ignore_orientation=False,
link_name=None):
"""
Calls trajopt to plan grasp collision-free trajectory
:param start_joints: list of initial joints
:param target_pose: desired pose of tool_frame (tfx.pose)
:param n_steps: trajopt discretization
:return None if traj not collision free, else list of joint values
"""
link_name = link_name if link_name is not None else self.tool_frame
assert len(start_joints) == len(self.joint_indices)
assert target_pose.frame.count('base_link') == 1
self.sim.update()
# set active manipulator and start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
# initialize trajopt inputs
rave_pose = tfx.pose(
self.sim.transform_from_to(target_pose.matrix, target_pose.frame,
'world'))
quat = rave_pose.orientation
xyz = rave_pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target])
# init_joint_target = None
init_joint_target = self.sim.ik_for_link(rave_pose.matrix, self.manip,
link_name, 0)
if init_joint_target is not None:
init_joint_target = self._closer_joint_angles(
init_joint_target, start_joints)
init_traj = self.ik_point(start_joints,
xyz,
n_steps=n_steps,
link_name=link_name)
request = self._get_grasp_trajopt_request(
xyz_target,
quat_target,
n_steps,
ignore_orientation=ignore_orientation,
link_name=link_name,
init_traj=init_traj)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
# TODO: worth doing?
# tool_link = self.robot.GetLink(link_name)
# def point_at(x):
# self.robot.SetDOFValues(x, self.joint_indices, False)
# T = tool_link.GetTransform()
# local_dir = xyz.array - T[:3,3]
# return T[1:3,:3].dot(local_dir)
#
# for t in xrange(int(0.8*n_steps), n_steps-1):
# #prob.AddConstraint(point_at, [(t,j) for j in xrange(len(self.joint_indices))], "EQ", "POINT_AT_%i"%t)
# prob.AddErrorCost(point_at, [(t,j) for j in xrange(len(self.joint_indices))], "ABS", "POINT_AT_%i"%t)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
prob.SetRobotActiveDOFs(
) # set robot DOFs to DOFs in optimization problem
#num_upsampled_collisions = len(traj_collisions(result.GetTraj(), self.robot, n=100))
num_upsampled_collisions = self._num_collisions(result.GetTraj())
print('Number of collisions: {0}'.format(num_upsampled_collisions))
self.robot.SetDOFValues(start_joints, self.joint_indices)
if num_upsampled_collisions > 2:
#if not traj_is_safe(result.GetTraj()[:], self.robot): # Check that trajectory is collision free
return None
else:
return result.GetTraj()
def ik_point(self,
start_joints,
target_position,
n_steps=40,
link_name=None):
"""
Calls trajopt to get traj to point, no collision checking
:param start_joints: list of initial joints
:param target_position: 3d np.ndarray desired position of tool_frame in world frame
:param n_steps: trajopt discretization
:return None if traj not collision free, else list of joint values
"""
link_name = link_name if link_name is not None else self.tool_frame
assert len(start_joints) == len(self.joint_indices)
self.sim.update()
# set active manipulator and start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
request = {
"basic_info": {
"n_steps": n_steps,
"manip": str(self.manip.GetName()),
"start_fixed": True
},
"costs": [
{
"type": "joint_vel",
"params": {
"coeffs": [1]
}
},
],
"constraints": [
{
"type": "pose",
"name": "target_pose",
"params": {
"xyz": list(target_position),
"wxyz": [0, 0, 0, 1],
"link": link_name,
"rot_coeffs": [0, 0, 0],
"pos_coeffs": [1, 1, 1]
}
},
],
"init_info": {
"type": "stationary",
},
}
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
tool_link = self.robot.GetLink(self.tool_frame)
def penalize_low_height(x):
self.robot.SetDOFValues(x, self.joint_indices, False)
z = tool_link.GetTransform()[2, 3]
return max(0, 10.0 - z)
for t in xrange(n_steps - 2):
prob.AddErrorCost(penalize_low_height,
[(t, j)
for j in xrange(len(self.joint_indices))],
"ABS", "PENALIZE_LOW_HEIGHT_%i" % t)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
return result.GetTraj()
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
])
robot.SetTransform(rave.matrixFromPose([1, 0, 0, 0, -3.4, -1.4, 0.05]))
# BEGIN set_active
robot.SetActiveDOFs(
np.r_[robot.GetManipulator("rightarm").GetArmIndices(),
robot.GetJoint("torso_lift_joint").GetDOFIndex()],
rave.DOFAffine.X + rave.DOFAffine.Y + rave.DOFAffine.RotationAxis,
[0, 0, 1])
# END set_active
##################
success = False
for i_try in xrange(100):
request = position_base_request(robot, LINK_NAME, XYZ_TARGET, QUAT_TARGET)
s = json.dumps(request)
trajoptpy.SetInteractive(args.interactive)
prob = trajoptpy.ConstructProblem(s, env)
result = trajoptpy.OptimizeProblem(prob)
if check_result(result, robot):
success = True
break
if success:
print "succeeded on try %i" % (i_try)
print result
else:
print "failed to find a valid solution :("
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, start_config, goal_config):
""" Plan from a start configuration to goal configuration """
rospy.logdebug("Planning from config {} to {}...".format(
start_config, goal_config))
dofs = len(start_config)
start_config[2] += math.pi # TODO this seems to be a bug in OpenRAVE?
goal_config[2] += math.pi # TODO this seems to be a bug in OpenRAVE?
self.jaco.SetDOFValues(start_config + self.finger_joint_values)
request = {
"basic_info": {
"n_steps": self.trajopt_num_waypoints,
"manip": self.jaco.GetActiveManipulator().GetName(),
"start_fixed": True
},
"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
},
{
"type": "collision",
"params": {
"coeffs": [20],
# 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
"first_step": 0,
"last_step": self.trajopt_num_waypoints - 1,
"continuous": True
},
}
],
"constraints": [{
"type": "joint", # joint-space target
"params": {
"vals": goal_config
} # length of vals = # dofs of manip
}],
"init_info": {
"type": "straight_line", # straight line in joint space.
"endpoint": goal_config
}
}
s = json.dumps(
request) # convert dictionary into json-formatted string
prob = trajoptpy.ConstructProblem(
s, self.env) # create object that stores optimization problem
# Add the cost function
for i, cost_function in enumerate(self.cost_functions):
prob.AddCost(cost_function.get_cost,
[(t, j) for j in range(dofs)
for t in range(self.trajopt_num_waypoints)],
"cost_{}".format(i))
t_start = time.time()
rospy.loginfo("trajoptpy instance: {}".format(trajoptpy))
result = trajoptpy.OptimizeProblem(prob) # do optimization
t_elapsed = time.time() - t_start
rospy.logdebug("Planning took {} seconds".format(t_elapsed))
print(result)
return self._to_trajectory_msg(result.GetTraj())
def get_return_from_grasp_joint_trajectory(self,
start_joints,
target_pose,
n_steps=40):
"""
Calls trajopt to plan return from grasp collision-free trajectory
:param start_joints: list of initial joints
:param target_pose: desired pose of tool_frame (tfx.pose)
:param n_steps: trajopt discretization
:return None if traj not collision free, else list of joint values
"""
assert len(start_joints) == len(self.joint_indices)
assert target_pose.frame.count('base_link') == 1
# set active manipulator and start joint positions
self.robot.SetDOFValues(start_joints, self.joint_indices)
# initialize trajopt inputs
rave_pose = tfx.pose(
self.sim.transform_from_to(target_pose.matrix, target_pose.frame,
'world'))
quat = rave_pose.orientation
xyz = rave_pose.position
quat_target = [quat.w, quat.x, quat.y, quat.z]
xyz_target = [xyz.x, xyz.y, xyz.z]
rave_mat = rave.matrixFromPose(np.r_[quat_target, xyz_target])
request = self._get_return_from_grasp_trajopt_request(
xyz_target, quat_target, n_steps)
# convert dictionary into json-formatted string
s = json.dumps(request)
# create object that stores optimization problem
prob = trajoptpy.ConstructProblem(s, self.sim.env)
tool_link = self.robot.GetLink(self.tool_frame)
def penalize_low_height(x):
self.robot.SetDOFValues(x, self.joint_indices, False)
z = tool_link.GetTransform()[2, 3]
return max(0, 10.0 - z)
for t in xrange(n_steps - 2):
prob.AddErrorCost(penalize_low_height,
[(t, j)
for j in xrange(len(self.joint_indices))],
"ABS", "PENALIZE_LOW_HEIGHT_%i" % t)
# do optimization
result = trajoptpy.OptimizeProblem(prob)
self.robot.SetDOFValues(start_joints, self.joint_indices)
prob.SetRobotActiveDOFs(
) # set robot DOFs to DOFs in optimization problem
num_upsampled_collisions = self._num_collisions(result.GetTraj())
print('Number of collisions: {0}'.format(num_upsampled_collisions))
self.robot.SetDOFValues(start_joints, self.joint_indices)
if num_upsampled_collisions > 2:
return None
else:
return result.GetTraj()
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