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()