([.2, .2, 1], np.array([-1, -1, 0]), rm([1, 0, 0], 0)),
([.2, .2, 1], np.array([-2, 1, 0]), rm([1, 0, 0], 0)),
]
START_STATE = np.zeros(12) #np.random.randn(12,)
START_STATE[0] = 4.0
GOAL_STATE = np.zeros(12)
GOAL_STATE[0] = -4.0
DURATION = 4.0
sample_times = np.linspace(0, DURATION, 8)
num_vars = 3
degree = 5
continuity_degree = 4
tf = DURATION
y_traj = PPTrajectory(sample_times, num_vars, degree, continuity_degree)
y_traj.add_constraint(t=0, derivative_order=0,
lb=START_STATE[:3]) # initial position
y_traj.add_constraint(t=0, derivative_order=1, lb=[0, 0,
0]) # initial velocity
y_traj.add_constraint(t=0, derivative_order=2, lb=[0, 0,
0]) # initial acceleration
# y_traj.add_constraint(t=.5, derivative_order=0, lb=[3, -1, 0])
y_traj.add_constraint(t=1, derivative_order=0, lb=[2, -.75, 0])
y_traj.add_constraint(t=2, derivative_order=0, lb=[1, 0, 0])
y_traj.add_constraint(t=2.5, derivative_order=0, lb=[0, 1, 0])
y_traj.add_constraint(t=3, derivative_order=0, lb=[-1.5, 0, 0])
y_traj.add_constraint(t=tf, derivative_order=0,
lb=GOAL_STATE[:3]) # end at zero
START_STATE[0] = 4.0 # x = 4.0
GOAL_STATE = np.zeros(12)
GOAL_STATE[0] = -4.0 # x = -4.0
DURATION = 4
assert type(DURATION) is int
dt = .1
n_segments = DURATION
degree = 5
H = np.zeros((len(REGIONS), n_segments))
H[0, 0] = 1
H[2, 1] = 1
H[1, 2] = 1
# start, goal, degree, n_segments, duration, regions, lb, ub, dt
y_traj = PPTrajectory(START_STATE[:3], GOAL_STATE[:3], degree, n_segments,
DURATION, REGIONS, H)
# y_traj = PPTrajectory(sample_times, num_vars, degree, continuity_degree)
# y_traj.add_constraint(t=0, derivative_order=0, lb=START_STATE[:3]) # initial position
# y_traj.add_constraint(t=0, derivative_order=1, lb=[0, 0, 0]) # initial velocity
# y_traj.add_constraint(t=0, derivative_order=2, lb=[0, 0, 0]) # initial acceleration
# y_traj.add_constraint(t=1, derivative_order=0, lb=[0, 1.0, 0])
# y_traj.add_constraint(t=2, derivative_order=0, lb=[0, 2.0, 0])
# y_traj.add_constraint(t=3, derivative_order=0, lb=[0, 3.0, 0])
# y_traj.add_constraint(t=tf, derivative_order=0, lb=[0, 4.0, 0]) # end at zero
# y_traj.add_constraint(t=tf, derivative_order=1, lb=[0, 0, 0])
# y_traj.add_constraint(t=tf, derivative_order=2, lb=[0, 0, 0])
# y_traj.generate()
# Build
builder = DiagramBuilder()