# print("M_out:", Mout) # Minimum and maximum eigenvalues for Q and Mout used for CLF-QP constraint eval_Q = np.linalg.eigvals(Q) eval_P = np.linalg.eigvals(Mout) min_e_Q = np.min(eval_Q) max_e_P = np.max(eval_P) print("Evalues: ", [min_e_Q, max_e_P]) # Set up for QP problem prog = MathematicalProgram() u_var = prog.NewContinuousVariables(4, "u_var") c_var = prog.NewContinuousVariables(1, "c_var") solverid = prog.GetSolverId() tol = 1e-10 prog.SetSolverOption(mp.SolverType.kIpopt, "tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "constr_viol_tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "acceptable_tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "acceptable_constr_viol_tol", tol) prog.SetSolverOption(mp.SolverType.kIpopt, "print_level", 5) # CAUTION: Assuming that solver used Ipopt # #-[2] Linearization and get (K,S) for LQR # A, B =robobee_plantBS.GetLinearizedDynamics(uf, xf) # print("A:", A, "B:", B)
def optimal_trajectory(joints, start_position, end_position, signed_dist_fn, nc, time=10, knot_points=100): assert len(joints) == len(start_position) assert len(joints) == len(end_position) h = time / (knot_points - 1) nq = len(joints) prog = MathematicalProgram() q_var = [] v_var = [] for ii in range(knot_points): q_var.append(prog.NewContinuousVariables(nq, "q[" + str(ii) + "]")) v_var.append(prog.NewContinuousVariables(nq, "v[" + str(ii) + "]")) # --------------------------------------------------------------- # Initial & Final Pose Constraints ------------------------------ x_i = np.append(start_position, np.zeros(nq)) x_i_vars = np.append(q_var[0], v_var[0]) prog.AddLinearEqualityConstraint(np.eye(2 * nq), x_i, x_i_vars) tol = 0.01 * np.ones(2 * nq) x_f = np.append(end_position, np.zeros(nq)) x_f_vars = np.append(q_var[-1], v_var[-1]) prog.AddBoundingBoxConstraint(x_f - tol, x_f + tol, x_f_vars) # --------------------------------------------------------------- # Dynamics Constraints ------------------------------------------ for ii in range(knot_points - 1): dyn_con1 = np.hstack((np.eye(nq), np.eye(nq), -np.eye(nq))) dyn_var1 = np.concatenate((q_var[ii], v_var[ii], q_var[ii + 1])) prog.AddLinearEqualityConstraint(dyn_con1, np.zeros(nq), dyn_var1) # --------------------------------------------------------------- # Joint Limit Constraints --------------------------------------- q_min = np.array([j.lower_limits() for j in joints]) q_max = np.array([j.upper_limits() for j in joints]) for ii in range(knot_points): prog.AddBoundingBoxConstraint(q_min, q_max, q_var[ii]) # --------------------------------------------------------------- # Collision Constraints ----------------------------------------- for ii in range(knot_points): prog.AddConstraint(signed_dist_fn, np.zeros(nc), 1e8 * np.ones(nc), q_var[ii]) # --------------------------------------------------------------- # Dynamics Constraints ------------------------------------------ for ii in range(knot_points): prog.AddQuadraticErrorCost(np.eye(nq), np.zeros(nq), v_var[ii]) xi = np.array(start_position) xf = np.array(end_position) for ii in range(knot_points): prog.SetInitialGuess(q_var[ii], ii * (xf - xi) / (knot_points - 1) + xi) prog.SetInitialGuess(v_var[ii], np.zeros(nq)) result = prog.Solve() print prog.GetSolverId().name() if result != SolutionResult.kSolutionFound: print result return None q_path = [] # print signed_dist_fn(prog.GetSolution(q_var[0])) for ii in range(knot_points): q_path.append(prog.GetSolution(q_var[ii])) return q_path