def test_cubic(self):
t = [0., 1., 2.]
x = np.diag((4., 5., 6.))
# Just test the spelling for these.
pp1 = PiecewisePolynomial.Cubic(t, x)
pp2 = PiecewisePolynomial.Cubic(t, x, np.identity(3))
pp3 = PiecewisePolynomial.Cubic(t, x, [0., 0., 0.], [0., 0., 0.])
def MoveEndEffectorAlongStraightLine(
p_WQ_start, p_WQ_end, duration, get_orientation, q_initial_guess, num_knots):
"""
p_WQ_start is the starting position of point Q (the point Q we defined earlier!)
p_WQ_end is the end position of point Q.
duration is the duration of the trajectory measured in seconds.
get_orientation(i, n) is a function that returns the interpolated orientation R_WL7 at the i-th knot point out of n knot points. It can simply return a constant orientation, but changing the orientation of link 7 should be helpful for placing the object on the shelf.
num_knots is the number of knot points in the trajectory. Generally speaking,the larger num_knots is, the smoother the trajectory, but solving for more knot points also takes longer. You can start by setting num_knots to 10, which should be sufficient for the task.
"""
t_knots = np.linspace(0, duration, num_knots+1)
q_knots = np.zeros((num_knots, plant.num_positions()))
q_knots[0] = q_initial_guess
n = len(q_initial_guess)
for i in range(num_knots):
ik = inverse_kinematics.InverseKinematics(plant)
prog = ik.prog()
p_AQ = p_WQ_start + (p_WQ_end - p_WQ_start) * i / (num_knots - 1)
ik.AddPositionConstraint(frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_AQ - 0.01, p_AQ_upper=p_AQ + 0.01) # interpolate between p_WQ_start and p_WQ_end
ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=get_orientation(i, num_knots), frameBbar=l7_frame, R_BbarB=RotationMatrix(), theta_bound=theta_bound) # call get_orientation(i, num_knots).
if i == 0:
prog.SetInitialGuess(ik.q(), q_initial_guess)
else:
# This is very important for the smoothness of the whole trajectory.
prog.SetInitialGuess(ik.q(), q_knots[i - 1])
result = mp.Solve(ik.prog())
assert result.get_solution_result() == mp.SolutionResult.kSolutionFound
q_knots[i] = result.GetSolution(ik.q())
return PiecewisePolynomial.Cubic(t_knots[1:], q_knots.T, np.zeros(n), np.zeros(n)) # return a cubic spline that connects all q_knots.
def rrt(self, star=False, max_iters=1000, goal_sample_prob=0.05, position_tolerance=0.09, duration=5):
""" Motion Planning by RRT and RRT*
:param star: True if RRT*
:param max_iters: max number of iterations
:param goal_sample_prob: probability to sample goal position
:param position_tolerance: tolerance to goal position
:param duration: duration of motion
:return: path found or raise error
"""
util = self.util
cspace = self.cspace
rrt = RRT(TreeNode(self.q_initial), cspace)
q_goals = []
for i in range(max_iters):
print("iter: ", i)
sample = self.sample_config(util, goal_sample_prob)
neighbor = rrt.nearest(sample)
path = self.safe_path(cspace, neighbor.value, sample, util)
if len(path) > 1:
q_end = path[-1]
# If pure RRT, include intermediate points as nodes
q_new_node = neighbor
add_middle_nodes = not star
if add_middle_nodes:
middle_path = path[1:-1:10]
else:
middle_path = []
for q in middle_path + [q_end]:
if not star:
q_new_node = self.rrt_extend(rrt, q_new_node, q)
else:
q_new_node = self.rrt_star_extend(rrt, q_new_node, q)
# check for goal
translation = util.FK(q_end).translation()
if self.within_tol(translation, self.p_goal, position_tolerance):
q_goals.append(q_new_node)
if ((not star and len(q_goals) > 0) or
(star and len(q_goals) > 0 and i == max_iters-1)):
min_cost = None
best_path = None
for q_goal_node in q_goals:
path = np.array(rrt.bfs(q_goal_node.value))
cost = rrt.cost(q_goal_node)
if min_cost is None or cost < min_cost:
min_cost = cost
best_path = path
num_knot_points = best_path.shape[0]
t_knots = np.linspace(0, duration, num_knot_points)
qtraj = PiecewisePolynomial.Cubic(t_knots, best_path.T, np.zeros(7), np.zeros(7))
print("Path cost: {}".format(min_cost))
print("Total number of rewires: {}".format(self.rewires))
return [JointSpacePlan(qtraj)], [0.1]
raise Exception("Did not find path to goal")
def polynomial(self):
d, n = self.q_knots.shape
return PiecewisePolynomial.Cubic(
breaks=self.t_knots,
knots=self.q_knots,
knot_dot_start=np.zeros(d),
knot_dot_end=np.zeros(d))
def ConnectPointsWithCubicPolynomial(x_start, x_end, duration):
t_knots = [0, duration / 2, duration]
n = len(x_start)
assert n == len(x_end)
x_knots = np.zeros((3, n))
x_knots[0] = x_start
x_knots[2] = x_end
x_knots[1] = (x_knots[0] + x_knots[2]) / 2
return PiecewisePolynomial.Cubic(t_knots, x_knots.T, np.zeros(n),
np.zeros(n))
def __init__(self,
angle_start,
angle_end=np.pi / 4,
duration=10.0,
type=None):
t_knots = [0, duration / 2, duration]
door_angle_knots = np.zeros((3, 1))
door_angle_knots[0] = angle_start
door_angle_knots[2] = angle_end
door_angle_knots[1] = (door_angle_knots[0] + door_angle_knots[2]) / 2
angle_traj = PiecewisePolynomial.Cubic(t_knots, door_angle_knots.T,
np.zeros(1), np.zeros(1))
PlanBase.__init__(self, type=type, trajectory=angle_traj)
def GoFromPointToPoint(p_WQ_start,
p_WQ_end,
duration,
num_knot_points,
q_initial_guess,
InterpolatePosition=InterpolateStraightLine,
position_tolerance=0.005):
# The first knot point is the zero posture.
# The second knot is the pre-pre-grasp posture q_val_0
# The rest are solved for in the for loop below.
# The hope is that using more knot points makes the trajectory
# smoother.
q_knots = np.zeros((num_knot_points + 1, plant.num_positions()))
q_knots[0] = q_initial_guess
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# ik.AddOrientationConstraint(
# frameAbar=world_frame, R_AbarA=R_WEa_ref,
# frameBbar=gripper_frame, R_BbarB=R_EEa,
# theta_bound=theta_bound)
p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points,
i)
ik.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ - position_tolerance,
p_AQ_upper=p_WQ + position_tolerance)
prog = ik.prog()
# use the robot posture at the previous knot point as
# an initial guess.
prog.SetInitialGuess(q_variables, q_knots[i])
result = prog.Solve()
if is_printing:
print i, ": ", result
q_knots[i + 1] = prog.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points + 1)
q_knots_kuka = GetKukaQKnots(q_knots)
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7),
np.zeros(7))
return qtraj, q_knots
def spline(self):
distance_fn = get_distance_fn(self.joints)
path = [q.positions[:len(self.joints)] for q in self.path]
q_knots_kuka = np.vstack(path).T
distances = [0.] + [
distance_fn(q1, q2) for q1, q2 in zip(path, path[1:])
]
# TODO: increase time for pick/place & hold
t_knots = np.cumsum(
distances) / RADIANS_PER_SECOND # TODO: this should be a max
d, n = q_knots_kuka.shape
return PiecewisePolynomial.Cubic(breaks=t_knots,
knots=q_knots_kuka,
knot_dot_start=np.zeros(d),
knot_dot_end=np.zeros(d))
def InverseKinPointwise(p_WQ_start,
p_WQ_end,
duration,
num_knot_points,
q_initial_guess,
InterpolatePosition=None,
position_tolerance=0.005,
theta_bound=0.005 * np.pi,
is_printing=True):
q_knots = np.zeros((num_knot_points + 1, plant.num_positions()))
q_knots[0] = q_initial_guess
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# Orientation constraint
ik.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WEa_ref,
frameBbar=gripper_frame,
R_BbarB=R_EEa,
theta_bound=theta_bound)
# Position constraint
p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points, i)
p_WQ_lower = p_WQ - position_tolerance
p_WQ_upper = p_WQ + position_tolerance
ik.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ_lower,
p_AQ_upper=p_WQ_upper)
prog = ik.prog()
prog.SetInitialGuess(q_variables, q_knots[i])
result = prog.Solve()
if is_printing:
print(i, ": ", result)
q_knots[i + 1] = prog.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points + 1)
q_knots_kuka = GetKukaQKnots(q_knots)
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7),
np.zeros(7))
return qtraj, q_knots
def GetCurrentPlan(self, context):
if not self.kuka_plans_list:
return
t = context.get_time()
if self.kuka_plans_list[0].traj is None:
q_current = self.EvalVectorInput(
context,
self.iiwa_position_input_port.get_index()).get_value()
q0 = self.kuka_plans_list[2].traj.value(0).flatten()
# zero order hold
q_knots_kuka = np.zeros((2, 7))
q_knots_kuka[0] = q_current
qtraj = PiecewisePolynomial.ZeroOrderHold(
[0, self.zero_order_hold_duration_sec], q_knots_kuka.T)
self.kuka_plans_list[0] = JointSpacePlan(qtraj)
# move to the starting position of trajectory plans
t_knots = np.array([
0., self.move_to_home_duration_sec / 2,
self.move_to_home_duration_sec
])
q_knots_kuka = np.zeros((3, 7))
q_knots_kuka[0] = q_current
q_knots_kuka[2] = q0
q_knots_kuka[1] = (q_knots_kuka[0] + q_knots_kuka[2]) / 2
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T,
np.zeros(7), np.zeros(7))
self.kuka_plans_list[1] = JointSpacePlan(qtraj)
# set current plan
self.current_plan_idx = 0
self.current_plan = self.kuka_plans_list[0]
self.current_plan.set_start_time(0.)
new_plan_idx = 0
for i in range(self.num_plans):
if t >= self.t_plan[i] and t < self.t_plan[i + 1]:
new_plan_idx = i
break
if self.current_plan_idx < new_plan_idx:
self.current_plan_idx = new_plan_idx
self.current_plan = self.kuka_plans_list[new_plan_idx]
self.current_plan.set_start_time(t)
def reconstruct_path(self, node):
"""
Piece together trajectories between nodes in the path to selected node (generally self.best_goal_node).
"""
current = node
utraj, xtraj, ttraj, res, cost = self.plant.trajOptRRT(
current.parent.state,
current.state,
goal=current.goal_node,
verbose=False)
urrt = utraj
xrrt = xtraj
trrt = ttraj
current = current.parent
while current.parent is not None:
utraj, xtraj, ttraj, res, cost = self.plant.trajOptRRT(
current.parent.state,
current.state,
goal=current.goal_node,
verbose=False)
urrt = np.vstack((utraj, urrt))
xrrt = np.vstack((xtraj[:-1, :], xrrt))
trrt = np.hstack((ttraj[:-1], trrt + ttraj.max()))
current = current.parent
# save traj
self.plant.udtraj = urrt
self.plant.xdtraj = xrrt
self.plant.ttraj = trrt
self.plant.mp_result = res
self.plant.udtraj_poly = PiecewisePolynomial.FirstOrderHold(
trrt[0:-1], urrt.T)
self.plant.xdtraj_poly = PiecewisePolynomial.Cubic(trrt, xrrt.T)
return urrt, xrrt, trrt
def convert_splines(mbp, robot, gripper, context, trajectories):
# TODO: move to trajectory class
print()
splines, gripper_setpoints = [], []
for i, traj in enumerate(trajectories):
traj.path[-1].assign(context)
joints = traj.path[0].joints
if len(joints) == 8: # TODO: fix this
joints = joints[:7]
if len(joints) == 2:
q_knots_kuka = np.zeros((2, 7))
q_knots_kuka[0] = get_configuration(mbp, context, robot) # Second is velocity
splines.append(PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots_kuka.T))
elif len(joints) == 7:
# TODO: adjust timing based on distance & velocities
# TODO: adjust number of waypoints
distance_fn = get_distance_fn(joints)
#path = [traj.path[0].positions, traj.path[-1].positions]
path = [q.positions[:len(joints)] for q in traj.path]
path = waypoints_from_path(joints, path) # TODO: increase time for pick/place & hold
q_knots_kuka = np.vstack(path).T
distances = [0.] + [distance_fn(q1, q2) for q1, q2 in zip(path, path[1:])]
t_knots = np.cumsum(distances) / RADIANS_PER_SECOND # TODO: this should be a max
d, n = q_knots_kuka.shape
print('{}) d={}, n={}, duration={:.3f}'.format(i, d, n, t_knots[-1]))
splines.append(PiecewisePolynomial.Cubic(
breaks=t_knots,
knots=q_knots_kuka,
knot_dot_start=np.zeros(d),
knot_dot_end=np.zeros(d)))
# RuntimeError: times must be in increasing order.
else:
raise ValueError(joints)
_, gripper_setpoint = get_configuration(mbp, context, gripper)
gripper_setpoints.append(gripper_setpoint)
return splines, gripper_setpoints
def lazy_sp(self, max_iters=1000, goal_sample_prob=0.05, position_tolerance=0.09, duration=5):
""" Motion Planning by LazySP
:param max_iters: max number of iterations
:param goal_sample_prob: probability to sample goal in each iteration
:param position_tolerance: tolerance in goal position
:param duration: duration of motion
:return: path found or raise error
"""
util = self.util
cspace = self.cspace
rrt = RRT(TreeNode(self.q_initial), cspace)
q_goals = []
for i in range(max_iters):
print("iter: ", i)
sample = self.sample_config(util, goal_sample_prob)
neighbor = rrt.nearest(sample)
# Find best node for reaching q_new
min_cost = rrt.cost(neighbor) + cspace.distance(neighbor.value, sample)
q_min_node = neighbor
# Stores q near nodes, and dist to q_new
Q_near = [[q_node, None] for q_node in rrt.near(sample, max_cost=10)]
print("num near nodes: {}".format(len(Q_near)))
for i, v in enumerate(Q_near):
q_near_node, _ = v
dist_to_new = cspace.distance(q_near_node.value, sample)
Q_near[i][1] = dist_to_new
cost = rrt.cost(q_near_node) + dist_to_new
if cost < min_cost:
min_cost = cost
q_min_node = q_near_node
q_new_node = rrt.add_configuration(q_min_node, sample)
# Rewiring existing nodes
new_node_cost = rrt.cost(q_new_node)
rewires = 0
for q_near_node, dist_to_new in Q_near:
if (q_near_node != q_min_node and
rrt.cost(q_near_node) > new_node_cost + dist_to_new):
rewires += 1
rrt.change_parent(q_new_node, q_near_node)
print("Num rewires: {}".format(rewires))
self.rewires += rewires
# check for goal
translation = util.FK(sample).translation()
if self.within_tol(translation, self.p_goal, position_tolerance):
q_goals.append(q_new_node)
def filter(goals):
""" Filter out Nones in goals
"""
return [g for g in goals if g is not None]
def in_free_edges(path, edges):
"""
:param path: list of nodes
:param edges: all free edges
:return: True if all edges between nodes are in free edges
"""
for s, e in zip(path[:-1], path[1:]):
if (s, e) not in edges:
return False
return True
def edge_selector(path, collision_free_edges):
"""
:param path: list of nodes
:param collision_free_edges: list of edges
:return: first edge in path that is in free edges
"""
for s, e in zip(path[:-1], path[1:]):
if (s, e) not in collision_free_edges:
return s, e
raise Exception("Unexpected that all edges are collision free")
collision_free_edges = []
while len(q_goals) > 0:
best_path, min_cost = rrt.shortest_path(q_goals)
if best_path is None:
break
# Get shortest path and check for collision
q_goals = filter(q_goals)
if in_free_edges(best_path, collision_free_edges):
best_path = np.array([node.value for node in best_path])
num_knot_points = best_path.shape[0]
t_knots = np.linspace(0, duration, num_knot_points)
qtraj = PiecewisePolynomial.Cubic(t_knots, best_path.T, np.zeros(7), np.zeros(7))
print("Path cost: {}".format(min_cost))
print("Total number of rewires: {}".format(self.rewires))
return [JointSpacePlan(qtraj)], [0.1]
# If no collision, add to free edges; otherwise, remove from tree
start, end = edge_selector(best_path, collision_free_edges)
if self.obstacle_free(cspace, start.value, end.value, util):
collision_free_edges.append((start, end))
else:
rrt.remove_edge(start, end)
raise Exception("Did not find path to goal")
def trajOpt(self, state_initial, dircol=0, second_pass=False):
"""
Perform trajectory optimization, using either direct transcription or direct collocation.
trajOptRRT() is neater and more useful -- just keeping this around to avoid rewriting sims for class project.
"""
# stopwatch for solver time
tsolve_pre = time.time()
(x_goal, V_goal, gamma_goal, q_goal) = (200.0, state_initial[2], 0.0,
0.0)
# number of knot points - proportional to x-distance seems to work well
if not dircol:
N = int(np.floor(0.8 * np.abs(x_goal - state_initial[0])))
else:
N = 30
# optimization problem: variables t_f, u[k], x[k]
mp = MathematicalProgram()
t_f = mp.NewContinuousVariables(1, "t_f")
dt = t_f[0] / N
k = 0
u = mp.NewContinuousVariables(2, "u_%d" % k)
input_trajectory = u
x = mp.NewContinuousVariables(6, "x_%d" % k)
state_trajectory = x
for k in range(1, N):
u = mp.NewContinuousVariables(2, "u_%d" % k)
x = mp.NewContinuousVariables(6, "x_%d" % k)
input_trajectory = np.vstack((input_trajectory, u))
state_trajectory = np.vstack((state_trajectory, x))
x = mp.NewContinuousVariables(6, "x_%d" % N)
state_trajectory = np.vstack((state_trajectory, x))
# for dircol we can use u_N and first-order hold
if dircol:
u = mp.NewContinuousVariables(2, "u_%d" % N)
input_trajectory = np.vstack((input_trajectory, u))
print "Number of decision vars", mp.num_vars()
# cost function: penalize time and control effort
thrust = input_trajectory[:, 0]
elev = input_trajectory[:, 1]
vel = state_trajectory[:, 2]
allvars = np.hstack((t_f[0], thrust, elev, vel))
# TODO: use u of length n+1 for dircol
def totalcost(X):
dt = X[0] / N
u0 = X[1:N + 1]
u1 = X[N + 1:2 * N + 1]
v = X[2 * N + 1:3 * N + 1] # cut last item if dirtrans
return dt * (1.0 * u0.dot(u0) +
1.0 * u1.dot(u1)) + 1.0 * X[0] * (u0.dot(v))
# return dt * (1.0 * u0.dot(u0) + 1.0 * u1.dot(u1) + 10.0 * X[0] * (u0.dot(v)))
mp.AddCost(totalcost, allvars)
# initial state constraint
for i in range(len(state_initial)):
mp.AddLinearConstraint(state_trajectory[0, i] == state_initial[i])
# final state constraint (x position)
mp.AddLinearConstraint(state_trajectory[-1, 0] == x_goal)
# final state constraint (z position) NOTE: range is acceptable
mp.AddLinearConstraint(state_trajectory[-1, 1] <= 1.5)
mp.AddLinearConstraint(state_trajectory[-1, 1] >= 0.5)
# final state constraint (velocity) NOTE: range is acceptable
mp.AddLinearConstraint(state_trajectory[-1, 2] <= 1.5 * V_goal)
mp.AddLinearConstraint(state_trajectory[-1, 2] >= V_goal)
# final state constraint (flight path angle) NOTE: small range here
mp.AddLinearConstraint(
state_trajectory[-1, 3] <= gamma_goal + 1.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[-1, 3] >= gamma_goal - 1.0 * np.pi / 180.0)
# final state constraint (pitch rate)
mp.AddLinearConstraint(state_trajectory[-1, 5] == q_goal)
# input constraints
for i in range(len(input_trajectory[:, 0])):
mp.AddLinearConstraint(input_trajectory[i, 0] >= 0.0)
mp.AddLinearConstraint(
input_trajectory[i, 0] <= 1.2 * self.m * self.g)
mp.AddLinearConstraint(input_trajectory[i, 1] >= -30.0)
mp.AddLinearConstraint(input_trajectory[i, 1] <= 30.0)
# state constraints
for i in range(len(state_trajectory[:, 0])):
# x position
mp.AddLinearConstraint(state_trajectory[i, 0] >= state_initial[0])
mp.AddLinearConstraint(state_trajectory[i, 0] <= x_goal)
# z position
mp.AddLinearConstraint(state_trajectory[i, 1] >= 0.3)
mp.AddLinearConstraint(state_trajectory[i, 1] <= 2.0)
# velocity
mp.AddLinearConstraint(state_trajectory[i, 2] >= 1.0)
mp.AddLinearConstraint(
state_trajectory[i, 2] <= 3.0 * state_initial[2])
# flight path angle
mp.AddLinearConstraint(
state_trajectory[i, 3] >= -30.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[i, 3] <= 30.0 * np.pi / 180.0)
# pitch angle
mp.AddLinearConstraint(
state_trajectory[i, 4] >= -20.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[i, 4] <= 40.0 * np.pi / 180.0)
# pitch rate
mp.AddLinearConstraint(
state_trajectory[i, 5] >= -20.0 * np.pi / 180.0)
mp.AddLinearConstraint(
state_trajectory[i, 5] <= 20.0 * np.pi / 180.0)
# dynamic constraints
if not dircol:
# direct transcription
for j in range(1, N + 1):
dynamic_prop = dt * self.airplaneLongDynamics(
state_trajectory[j - 1, :], input_trajectory[j - 1, :])
for k in range(len(state_initial)):
mp.AddConstraint(
state_trajectory[j, k] == state_trajectory[j - 1, k] +
dynamic_prop[k])
else:
# direct collocation
for j in range(1, N + 1):
x0 = state_trajectory[j - 1, :]
x1 = state_trajectory[j, :]
xdot0 = self.airplaneLongDynamics(x0,
input_trajectory[j - 1, :])
xdot1 = self.airplaneLongDynamics(x1, input_trajectory[j, :])
xc = 0.5 * (x1 + x0) + dt * (xdot0 - xdot1) / 8.0
xdotc = -1.5 * (x0 - x1) / dt - 0.25 * (xdot0 + xdot1)
uc = 0.5 * (input_trajectory[j - 1, :] +
input_trajectory[j, :])
f_xc = self.airplaneLongDynamics(xc, uc)
for k in range(len(state_initial)):
# TODO: why does "==" cause "kUnknownError"?
# mp.AddConstraint(xdotc[k] - f_xc[k] == 0.0)
mp.AddConstraint(xdotc[k] <= f_xc[k] + 0.001)
mp.AddConstraint(xdotc[k] >= f_xc[k] - 0.001)
# allow for warm start of dircol program with output of dirtrans program
if (second_pass) and (self.mp_result == SolutionResult.kSolutionFound):
# warm start using previous output
print 'warm start to traj opt'
t_guess = self.ttraj[-1]
mp.SetInitialGuess(t_f[0], t_guess)
for i in range(len(state_trajectory[:, 0])):
for j in range(len(state_initial)):
mp.SetInitialGuess(state_trajectory[i, j], self.xdtraj[i,
j])
for i in range(N):
mp.SetInitialGuess(input_trajectory[i, 0], self.udtraj[i, 0])
mp.SetInitialGuess(input_trajectory[i, 1], self.udtraj[i, 1])
# time constraints
mp.AddLinearConstraint(t_f[0] <= 1.25 * t_guess)
mp.AddLinearConstraint(t_f[0] >= 0.8 * t_guess)
else:
# initial guesses
t_guess = np.abs(x_goal -
state_initial[0]) / (0.5 *
(V_goal + state_initial[2]))
mp.SetInitialGuess(t_f[0], t_guess)
z_final_dummy = state_initial[1]
theta_final_dummy = state_initial[4]
state_final_dummy = np.array([
x_goal, z_final_dummy, V_goal, gamma_goal, theta_final_dummy,
q_goal
])
for i in range(len(state_trajectory[:, 0])):
state_guess = (
(N - i) / N) * state_initial + (i / N) * state_final_dummy
for j in range(len(state_guess)):
mp.SetInitialGuess(state_trajectory[i, j], state_guess[j])
for i in range(N):
mp.SetInitialGuess(input_trajectory[i, 0],
self.m * self.g / 3.5)
mp.SetInitialGuess(input_trajectory[i, 1], 0.01)
# time constraints
mp.AddLinearConstraint(t_f[0] <= 2.0 * t_guess)
mp.AddLinearConstraint(t_f[0] >= 0.5 * t_guess)
# set SNOPT iteration limit
it_limit = int(max(20000, 40 * mp.num_vars()))
mp.SetSolverOption(SolverType.kSnopt, 'Iterations limit', it_limit)
print("** solver begin with N = %d **" % N)
# solve nonlinear optimization problem (w/SNOPT)
result = mp.Solve()
print result
# convert from symbolic to float
input_trajectory = mp.GetSolution(input_trajectory)
t_f = mp.GetSolution(t_f)
state_trajectory_approx = mp.GetSolution(state_trajectory)
time_array = t_f[0] * np.linspace(0.0, 1.0, (N + 1))
tsolve_post = time.time()
tsolve = tsolve_post - tsolve_pre
solver_id = mp.GetSolverId()
print("** %s solver finished in %.1f seconds **\n" %
(solver_id.name(), tsolve))
print("t_f computed: %.3f seconds" % t_f[0])
# get total cost of solution
if result == SolutionResult.kSolutionFound:
thrust = input_trajectory[:, 0]
elev = input_trajectory[:, 1]
vel = state_trajectory_approx[:, 2]
allvars = np.hstack((t_f[0], thrust, elev, vel))
print("cost computed: %.3f" % totalcost(allvars))
# save traj (this is a bit sloppy and redundant but scripts for visualization currently rely on this)
self.udtraj = input_trajectory
self.xdtraj = state_trajectory_approx
self.ttraj = time_array
self.mp_result = result
# save polynomials of input, state trajectories
if not dircol:
self.udtraj_poly = PiecewisePolynomial.FirstOrderHold(
time_array[0:-1], input_trajectory.T)
else:
self.udtraj_poly = PiecewisePolynomial.FirstOrderHold(
time_array, input_trajectory.T)
self.xdtraj_poly = PiecewisePolynomial.Cubic(time_array,
state_trajectory_approx.T)
return input_trajectory, state_trajectory_approx, time_array
def InverseKinPointwise(InterpolatePosition,
InterpolateOrientation,
p_L7Q,
duration,
num_knot_points,
q_initial_guess,
position_tolerance=0.005,
theta_bound=0.005 * np.pi):
"""
Calculates a joint space trajectory for iiwa by repeatedly calling IK.
The returned trajectory has (num_knot_points) knot points. To improve
the continuity of the trajectory, the IK from which q_knots[i] is
solved is initialized with q_knots[i-1].
Positions for point Q (p_EQ) and orientations for the end effector, generated
respectively by InterpolatePosition and InterpolateOrientation,
are added to the IKs as constraints.
:param InterpolatePosition: A function with signature (tau) with
0 <= tau <= 1.
It returns p_WQ, a (3,) numpy array which describes the desired
position of Q. For example, to get p_WQ for knot point i, use
tau = i / (num_knot_points - 1).
:param InterpolateOrientation: A function with the same signature as
InterpolatePosition.
It returns R_WL7, a RotationMatrix which describes the desired
orientation of frame L7.
:param num_knot_points: number of knot points in the trajectory.
:param q_initial_guess: initial guess for the first IK.
:param position_tolerance: tolerance for IK position constraints in meters.
:param theta_bound: tolerance for IK orientation constraints in radians.
:param is_printing: whether the solution results of IKs are printed.
:return: qtraj: a 7-dimensional cubic polynomial that describes a
trajectory for the iiwa arm.
:return: q_knots: a (n, num_knot_points) numpy array (where n =
plant.num_positions()) that stores solutions returned by all IKs. It
can be used to initialize IKs for the next trajectory.
"""
q_knots = np.zeros((num_knot_points, plant.num_positions()))
################### Your code here ###################
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# Orientation constraint
R_WL7_ref = InterpolateOrientation(i / (num_knot_points - 1))
ik.AddOrientationConstraint(
frameAbar=world_frame, R_AbarA=R_WL7_ref,
frameBbar=l7_frame, R_BbarB=RotationMatrix.Identity(),
theta_bound=theta_bound)
# Position constraint
p_WQ = InterpolatePosition(i / (num_knot_points - 1))
ik.AddPositionConstraint(
frameB=l7_frame, p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ - position_tolerance,
p_AQ_upper=p_WQ + position_tolerance)
prog = ik.prog()
# use the robot configuration at the previous knot point as
# an initial guess.
if i > 0:
prog.SetInitialGuess(q_variables, q_knots[i-1])
else:
prog.SetInitialGuess(q_variables, q_initial_guess)
result = mp.Solve(prog)
# throw if no solution found.
if result.get_solution_result() != SolutionResult.kSolutionFound:
print(i, result.get_solution_result())
raise RuntimeError
q_knots[i] = result.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points)
qtraj = PiecewisePolynomial.Cubic(
t_knots, q_knots.T, np.zeros(7), np.zeros(7))
return qtraj, q_knots
########################################################
raise NotImplementedError
