def CheckLevelSet(self, prev_x, x0, Vs, Vsdot, rho, multiplier_degree):
prog = MathematicalProgram()
x = prog.NewIndeterminates(len(prev_x),'x')
V = Vs.Substitute(dict(zip(prev_x, x)))
Vdot = Vsdot.Substitute(dict(zip(prev_x, x)))
slack = prog.NewContinuousVariables(1,'a')[0]
#mapping = dict(zip(x, np.ones(len(x))))
#V_norm = 0.0*V
#for i in range(len(x)):
# basis = np.ones(len(x))
# V_norm = V_norm + V.Substitute(dict(zip(x, basis)))
#V = V/V_norm
#Vdot = Vdot/V_norm
#prog.AddConstraint(V_norm == 0)
# in relative state (lambda(xbar)
Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression()
Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar)
prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V)
prog.AddCost(-slack)
#import pdb; pdb.set_trace()
result = Solve(prog)
if(not result.is_success()):
print('%s, %s' %(result.get_solver_id().name(),result.get_solution_result()) )
print('slack = %f' %(result.GetSolution(slack)) )
print('Rho = %f' %(rho))
#assert result.is_success()
return -1.0
return result.GetSolution(slack)
def runDircol(self,x0,xf,tf0):
N = 15 # constant
#N = np.int(tf0 * 10) # "10Hz" / samples per second
context = self.CreateDefaultContext()
dircol = DirectCollocation(self, context, num_time_samples=N,
minimum_timestep=0.05, maximum_timestep=1.0)
u = dircol.input()
# set some constraints on inputs
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(u[1] <= self.slewmax)
dircol.AddConstraintToAllKnotPoints(u[1] >= -self.slewmax)
dircol.AddConstraintToAllKnotPoints(u[0] <= self.umax)
dircol.AddConstraintToAllKnotPoints(u[0] >= -self.umax)
# constrain the last input to be zero (at least for the u input)
#import pdb; pdb.set_trace()
dv = dircol.decision_variables()
for i in range(3, self.nX*N, 4):
alfa_state = dv[i] #u[t_end]
dircol.AddBoundingBoxConstraint(-self.alfamax, self.alfamax, alfa_state)
#final_u_decision_var = dv[self.nX*N + self.nU*N - 1] #u[t_end]
#dircol.AddLinearEqualityConstraint(final_u_decision_var, 0.0)
#first_u_decision_var = dv[self.nX*N + 1 ] #u[t_0]
#dircol.AddLinearEqualityConstraint(first_u_decision_var, 0.0)
# set some constraints on start and final pose
eps = 0.0 * np.ones(self.nX) # relaxing factor
dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state())
dircol.AddBoundingBoxConstraint(xf-eps, \
xf+eps, dircol.final_state())
R = 1.0*np.eye(self.nU) # Cost on input "effort".
dircol.AddRunningCost( u.transpose().dot(R.dot(u)) )
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
# guess initial trajectory
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
# optimize
result = Solve(dircol)
print('******\nRunning trajectory optimization:')
print('w/ solver %s' %(result.get_solver_id().name()))
print(result.get_solution_result())
assert(result.is_success())
xtraj = dircol.ReconstructStateTrajectory(result)
utraj = dircol.ReconstructInputTrajectory(result)
# return nominal trajectory
return utraj,xtraj
def runDircol(self, x0, xf, tf0):
N = 21 #np.int(tf0 * 10) # "10Hz" samples per second
context = self.CreateDefaultContext()
dircol = DirectCollocation(self,
context,
num_time_samples=N,
minimum_timestep=0.05,
maximum_timestep=1.0)
u = dircol.input()
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(u[0] <= 0.5 * self.omegamax)
dircol.AddConstraintToAllKnotPoints(u[0] >= -0.5 * self.omegamax)
dircol.AddConstraintToAllKnotPoints(u[1] <= 0.5 * self.umax)
dircol.AddConstraintToAllKnotPoints(u[1] >= -0.5 * self.umax)
eps = 0.0
dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state())
dircol.AddBoundingBoxConstraint(xf - np.array([eps, eps, eps]),
xf + np.array([eps, eps, eps]),
dircol.final_state())
R = 1.0 * np.eye(2) # Cost on input "effort".
dircol.AddRunningCost(u.transpose().dot(R.dot(u)))
#dircol.AddRunningCost(R*u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf)))
dircol.SetInitialTrajectory(PiecewisePolynomial(),
initial_x_trajectory)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
#import pdb; pdb.set_trace()
xtraj = dircol.ReconstructStateTrajectory(result)
utraj = dircol.ReconstructInputTrajectory(result)
return utraj, xtraj
R = 10 # Cost on input "effort".
dircol.AddRunningCost(R * u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
x_trajectory = dircol.ReconstructStateTrajectory(result)
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])
ani = vis.animate(x_trajectory, repeat=True)
u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
plt.figure()
R = 10 # Cost on input "effort".
dircol.AddRunningCost(R*u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert(result.is_success())
x_trajectory = dircol.ReconstructStateTrajectory(result)
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])
ani = vis.animate(x_trajectory, repeat=True)
u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
plt.figure()