From db3533fc5e531ac6a4ee0081fe0b8687e5e3eecd Mon Sep 17 00:00:00 2001 From: jake-levy Date: Mon, 2 Jan 2023 16:09:09 -0600 Subject: [PATCH 1/3] use timestep to initialize solver --- examples/optimal_feedback_control_example.jl | 4 ++-- examples/utils/nlp.jl | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/optimal_feedback_control_example.jl b/examples/optimal_feedback_control_example.jl index c9171fe..6d8edfd 100644 --- a/examples/optimal_feedback_control_example.jl +++ b/examples/optimal_feedback_control_example.jl @@ -19,7 +19,8 @@ function run_example() goal_state = [x_f,y_f,v_f,θ_f] # initialize the solver - model = initialize_model(goal_state) + timestep = 0.1 # duration of each timestep (sec) + model = initialize_model(goal_state, timestep) # initialize a plotter to display trajectory solution real-time display(plot(xlims=(-1,4),ylims=(-1,4),size=(600,600))) @@ -32,7 +33,6 @@ function run_example() # open a connection to the ROS velocity control node ip = "192.168.1.135" # ip address of the host of the ROS node control_port = 42421 # port to connect on - timestep = 0.1 # duration of each timestep (sec) robot_connection = open_robot_connection(ip, control_port) # obtain initial state of the robot diff --git a/examples/utils/nlp.jl b/examples/utils/nlp.jl index 154a237..de100d4 100644 --- a/examples/utils/nlp.jl +++ b/examples/utils/nlp.jl @@ -1,4 +1,4 @@ -function initialize_model(goal_state) +function initialize_model(goal_state, timestep) x_f = goal_state[1] y_f = goal_state[2] v_f = goal_state[3] @@ -8,7 +8,7 @@ function initialize_model(goal_state) R = 2 T = 400 - dt = .1 + dt = timestep times = [k*dt for k in 0:T-1] v_max = 0.22 From 314940677a14b5ebdb096ab591bc935a6f5cfe61 Mon Sep 17 00:00:00 2001 From: jake-levy Date: Tue, 3 Jan 2023 18:37:03 -0600 Subject: [PATCH 2/3] correct velocity direction calculation --- examples/utils/common.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/utils/common.jl b/examples/utils/common.jl index 4040357..c8b3d1b 100644 --- a/examples/utils/common.jl +++ b/examples/utils/common.jl @@ -10,7 +10,7 @@ function to_state_vec(feedback_state::FeedbackData) θ_0 = atan(heading[2],heading[1]) v_0 = norm(feedback_state.linear_vel[1:2]) - if abs(tan(feedback_state.linear_vel[2]/feedback_state.linear_vel[2]) - + if abs(atan(feedback_state.linear_vel[2],feedback_state.linear_vel[1]) - θ_0) > π/2 v_0 = -v_0 end From 326a5cbbfc67349f12fe30ab63891c0097e17473 Mon Sep 17 00:00:00 2001 From: jake-levy Date: Tue, 3 Jan 2023 18:44:29 -0600 Subject: [PATCH 3/3] warm start solver; make solver faster --- Project.toml | 2 +- examples/optimal_feedback_control_example.jl | 8 +++++++- examples/utils/nlp.jl | 10 +++++++++- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/Project.toml b/Project.toml index b1d1488..e5ca7a0 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "RosSockets" uuid = "f2b1035b-1fed-4502-a057-be66ed18c291" authors = ["Jacob Levy"] -version = "0.2.1" +version = "0.2.2" [deps] JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" diff --git a/examples/optimal_feedback_control_example.jl b/examples/optimal_feedback_control_example.jl index 6d8edfd..74dcf3e 100644 --- a/examples/optimal_feedback_control_example.jl +++ b/examples/optimal_feedback_control_example.jl @@ -22,6 +22,9 @@ function run_example() timestep = 0.1 # duration of each timestep (sec) model = initialize_model(goal_state, timestep) + # begin without warm start so that first solve is cold + warm_start = false + # initialize a plotter to display trajectory solution real-time display(plot(xlims=(-1,4),ylims=(-1,4),size=(600,600))) @@ -45,8 +48,11 @@ function run_example() feedback_state = receive_feedback_data(feedback_connection, timeout) # compute control commands and send them - commands, trajectory = solve!(model,feedback_state) + commands, trajectory = solve!(model,feedback_state,warm_start) send_control_commands(robot_connection, commands) + + # future calls to solve! will be warm started + warm_start = true # plot solution trajectory display(plot(trajectory[:,1],trajectory[:,2], diff --git a/examples/utils/nlp.jl b/examples/utils/nlp.jl index de100d4..747da54 100644 --- a/examples/utils/nlp.jl +++ b/examples/utils/nlp.jl @@ -17,6 +17,8 @@ function initialize_model(goal_state, timestep) model = Model(Ipopt.Optimizer) set_optimizer_attribute(model, "max_iter", 100) + set_optimizer_attribute(model, "print_level", 1) + set_optimizer_attribute(model, "mu_strategy", "adaptive") @variables(model, begin x[1:T] @@ -62,10 +64,16 @@ function initialize_model(goal_state, timestep) return model end -function solve!(model, feedback_state::FeedbackData) +function solve!(model, feedback_state::FeedbackData, warm_start=false) state = to_state_vec(feedback_state) + if warm_start + x = all_variables(model) + x_solution = value.(x) + set_start_value.(x, x_solution) + end + fix(model[:x][1], state[1]) fix(model[:y][1], state[2]) fix(model[:v][1], state[3])