def test_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
num_time_samples = 21
dircol = DirectCollocation(
plant,
context,
num_time_samples=num_time_samples,
minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
prog = dircol.prog()
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(index=0)
x = dircol.state()
x2 = dircol.state(index=2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(index=2)
v = dircol.NewSequentialVariable(rows=1, name="test")
v2 = dircol.GetSequentialVariableAtIndex(name="test", index=2)
dircol.AddRunningCost(x.dot(x))
input_con = dircol.AddConstraintToAllKnotPoints(u[0] == 0)
self.assertEqual(len(input_con), 21)
interval_bound = dircol.AddTimeIntervalBounds(lower_bound=0.3,
upper_bound=0.4)
self.assertIsInstance(interval_bound.evaluator(),
mp.BoundingBoxConstraint)
equal_time_con = dircol.AddEqualTimeIntervalsConstraints()
self.assertEqual(len(equal_time_con), 19)
duration_bound = dircol.AddDurationBounds(lower_bound=.3 * 21,
upper_bound=0.4 * 21)
self.assertIsInstance(duration_bound.evaluator(), mp.LinearConstraint)
final_cost = dircol.AddFinalCost(2 * x.dot(x))
self.assertIsInstance(final_cost.evaluator(), mp.Cost)
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(traj_init_u=initial_u,
traj_init_x=initial_x)
was_called = dict(input=False, state=False, complete=False)
def input_callback(t, u):
was_called["input"] = True
def state_callback(t, x):
was_called["state"] = True
def complete_callback(t, x, u, v):
was_called["complete"] = True
dircol.AddInputTrajectoryCallback(callback=input_callback)
dircol.AddStateTrajectoryCallback(callback=state_callback)
dircol.AddCompleteTrajectoryCallback(callback=complete_callback,
names=["test"])
result = mp.Solve(dircol.prog())
self.assertTrue(was_called["input"])
self.assertTrue(was_called["state"])
self.assertTrue(was_called["complete"])
dircol.GetSampleTimes(result=result)
dircol.GetInputSamples(result=result)
dircol.GetStateSamples(result=result)
dircol.GetSequentialVariableSamples(result=result, name="test")
dircol.ReconstructInputTrajectory(result=result)
dircol.ReconstructStateTrajectory(result=result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1), prog)
# Test AddConstraintToAllKnotPoints variants.
nc = len(prog.bounding_box_constraints())
c = dircol.AddConstraintToAllKnotPoints(
constraint=mp.BoundingBoxConstraint([0], [1]), vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.BoundingBoxConstraint])
self.assertEqual(len(prog.bounding_box_constraints()),
nc + num_time_samples)
nc = len(prog.linear_equality_constraints())
c = dircol.AddConstraintToAllKnotPoints(
constraint=mp.LinearEqualityConstraint([1], [0]), vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.LinearEqualityConstraint])
self.assertEqual(len(prog.linear_equality_constraints()),
nc + num_time_samples)
nc = len(prog.linear_constraints())
c = dircol.AddConstraintToAllKnotPoints(constraint=mp.LinearConstraint(
[1], [0], [1]),
vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.LinearConstraint])
self.assertEqual(len(prog.linear_constraints()), nc + num_time_samples)
nc = len(prog.linear_equality_constraints())
# eq(x, 2) produces a 2-dimensional vector of Formula.
c = dircol.AddConstraintToAllKnotPoints(eq(x, 2))
self.assertIsInstance(c[0].evaluator(), mp.LinearEqualityConstraint)
self.assertEqual(len(prog.linear_equality_constraints()),
nc + 2 * num_time_samples)
def test_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant, context, num_time_samples=21, minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(index=0)
x = dircol.state()
x2 = dircol.state(index=2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(index=2)
v = dircol.NewSequentialVariable(rows=1, name="test")
v2 = dircol.GetSequentialVariableAtIndex(name="test", index=2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(lower_bound=0.3, upper_bound=0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(lower_bound=.3*21, upper_bound=0.4*21)
dircol.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(traj_init_u=initial_u,
traj_init_x=initial_x)
was_called = dict(
input=False,
state=False,
complete=False
)
def input_callback(t, u):
was_called["input"] = True
def state_callback(t, x):
was_called["state"] = True
def complete_callback(t, x, u, v):
was_called["complete"] = True
dircol.AddInputTrajectoryCallback(callback=input_callback)
dircol.AddStateTrajectoryCallback(callback=state_callback)
dircol.AddCompleteTrajectoryCallback(callback=complete_callback,
names=["test"])
result = mp.Solve(dircol)
self.assertTrue(was_called["input"])
self.assertTrue(was_called["state"])
self.assertTrue(was_called["complete"])
times = dircol.GetSampleTimes(result=result)
inputs = dircol.GetInputSamples(result=result)
states = dircol.GetStateSamples(result=result)
variables = dircol.GetSequentialVariableSamples(result=result,
name="test")
input_traj = dircol.ReconstructInputTrajectory(result=result)
state_traj = dircol.ReconstructStateTrajectory(result=result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def _do_test_direct_collocation(self, use_deprecated_solve):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant, context, num_time_samples=21, minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(0)
x = dircol.state()
x2 = dircol.state(2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(0.3, 0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(.3*21, 0.4*21)
dircol.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(initial_u, initial_x)
global input_was_called
input_was_called = False
global state_was_called
state_was_called = False
def input_callback(t, u):
global input_was_called
input_was_called = True
def state_callback(t, x):
global state_was_called
state_was_called = True
dircol.AddInputTrajectoryCallback(input_callback)
dircol.AddStateTrajectoryCallback(state_callback)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=1):
dircol.Solve()
result = None
else:
result = mp.Solve(dircol)
self.assertTrue(input_was_called)
self.assertTrue(state_was_called)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=5):
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
else:
times = dircol.GetSampleTimes(result)
inputs = dircol.GetInputSamples(result)
states = dircol.GetStateSamples(result)
input_traj = dircol.ReconstructInputTrajectory(result)
state_traj = dircol.ReconstructStateTrajectory(result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def _do_test_direct_collocation(self, use_deprecated_solve):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant, context, num_time_samples=21, minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(0)
x = dircol.state()
x2 = dircol.state(2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(0.3, 0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(.3*21, 0.4*21)
dircol.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(initial_u, initial_x)
global input_was_called
input_was_called = False
global state_was_called
state_was_called = False
def input_callback(t, u):
global input_was_called
input_was_called = True
def state_callback(t, x):
global state_was_called
state_was_called = True
dircol.AddInputTrajectoryCallback(input_callback)
dircol.AddStateTrajectoryCallback(state_callback)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=1):
dircol.Solve()
result = None
else:
result = mp.Solve(dircol)
self.assertTrue(input_was_called)
self.assertTrue(state_was_called)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=5):
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
else:
times = dircol.GetSampleTimes(result)
inputs = dircol.GetInputSamples(result)
states = dircol.GetStateSamples(result)
input_traj = dircol.ReconstructInputTrajectory(result)
state_traj = dircol.ReconstructStateTrajectory(result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def test_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.2,
maximum_timestep=0.5)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(0)
x = dircol.state()
x2 = dircol.state(2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(0.3, 0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(.3 * 21, 0.4 * 21)
dircol.AddFinalCost(2 * x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(initial_u, initial_x)
global input_was_called
input_was_called = False
global state_was_called
state_was_called = False
def input_callback(t, u):
global input_was_called
input_was_called = True
def state_callback(t, x):
global state_was_called
state_was_called = True
dircol.AddInputTrajectoryCallback(input_callback)
dircol.AddStateTrajectoryCallback(state_callback)
dircol.Solve()
self.assertTrue(input_was_called)
self.assertTrue(state_was_called)
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
0.0, 4.0, startposx, startposy, startposz,
initial_angle_ground[0], initial_angle_ground[1],
initial_angle_ground[2]
]
print("Sent starting position")
data_send = struct.pack('<8d', *sample)
sock_send = socket.socket(
socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
sock_send.sendto(data_send, (UDP_IP_SEND, UDP_PORT_SEND))
else:
print("Next iter on send")
dircol = DirectCollocation(plant,
context,
num_time_samples=N,
minimum_timestep=0.01,
maximum_timestep=max_dt)
#Constraint that all time steps have equal duration
dircol.AddEqualTimeIntervalsConstraints()
#Input u values to be 2x1 matrix
u = dircol.input()
#Add constraints to break points
dircol.AddConstraintToAllKnotPoints(u[0] <= umax)
dircol.AddConstraintToAllKnotPoints(u[0] >= -umax)
#The state
x = dircol.state()