def vel_accel_robustaccel(request): "Velocity + Acceleration + Robust Acceleration constraint" dtype_a, dtype_ra = request.param vlims = np.array([[-1, 1], [-1, 2], [-1, 4]], dtype=float) alims = np.array([[-1, 1], [-1, 2], [-1, 4]], dtype=float) vel_cnst = constraint.JointVelocityConstraint(vlims) accl_cnst = constraint.JointAccelerationConstraint(alims, dtype_a) robust_accl_cnst = constraint.RobustCanonicalLinearConstraint( accl_cnst, [0.5, 0.1, 2.0], dtype_ra) yield vel_cnst, accl_cnst, robust_accl_cnst
def basic_constraints(request): """ Return a set of relatively simple constraints. """ dtype_a, dtype_ra = request.param vlims = np.array([[-1, 1], [-1, 2], [-1, 4], [-3, 4], [-2, 4], [-3, 4], [-2, 5]], dtype=float) * 10 alims = np.array([[-1, 1], [-1, 2], [-1, 4], [-3, 4], [-2, 4], [-3, 4], [-2, 5]], dtype=float) * 10 vel_cnst = constraint.JointVelocityConstraint(vlims) accl_cnst = constraint.JointAccelerationConstraint(alims, dtype_a) robust_accl_cnst = constraint.RobustCanonicalLinearConstraint( accl_cnst, [1e-4, 1e-4, 5e-4], dtype_ra) yield vel_cnst, accl_cnst, robust_accl_cnst
def main(): parser = argparse.ArgumentParser(description="An example showcasing the usage of robust constraints." "A velocity constraint and a robust acceleration constraint" "are considered in this script.") parser.add_argument("-N", "--N", type=int, help="Number of segments in the discretization.", default=100) parser.add_argument("-v", "--verbose", action="store_true", default=False) parser.add_argument("-du", "--du", default=1e-3, type=float) parser.add_argument("-dx", "--dx", default=5e-2, type=float) parser.add_argument("-dc", "--dc", default=9e-3, type=float) parser.add_argument("-so", "--solver_wrapper", default='ecos') parser.add_argument("-i", "--interpolation_scheme", default=1, type=int) args = parser.parse_args() if args.verbose: ta.setup_logging("DEBUG") else: ta.setup_logging("INFO") # Parameters N_samples = 5 dof = 7 # Random waypoints used to obtain a random geometric path. np.random.seed(9) way_pts = np.random.randn(N_samples, dof) # Create velocity bounds, then velocity constraint object vlim_ = np.random.rand(dof) * 20 vlim = np.vstack((-vlim_, vlim_)).T # Create acceleration bounds, then acceleration constraint object alim_ = np.random.rand(dof) * 2 alim = np.vstack((-alim_, alim_)).T path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts) pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) robust_pc_acc = constraint.RobustCanonicalLinearConstraint( pc_acc, [args.du, args.dx, args.dc], args.interpolation_scheme) instance = algo.TOPPRA([pc_vel, robust_pc_acc], path, gridpoints=np.linspace(0, 1, args.N + 1), solver_wrapper=args.solver_wrapper) X = instance.compute_feasible_sets() K = instance.compute_controllable_sets(0, 0) _, sd_vec, _ = instance.compute_parameterization(0, 0) X = np.sqrt(X) K = np.sqrt(K) plt.plot(X[:, 0], c='green', label="Feasible sets") plt.plot(X[:, 1], c='green') plt.plot(K[:, 0], '--', c='red', label="Controllable sets") plt.plot(K[:, 1], '--', c='red') plt.plot(sd_vec, label="Velocity profile") plt.legend() plt.title("Path-position path-velocity plot") plt.show() jnt_traj, aux_traj = instance.compute_trajectory(0, 0) ts_sample = np.linspace(0, jnt_traj.get_duration(), 100) qs_sample = jnt_traj.evaldd(ts_sample) plt.plot(ts_sample, qs_sample) plt.show()