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