Merge pull request #10 from CLeARoboticsLab/improve-opt-ex

Improve examples/optimal_feedback_control_example.jl

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"

examples/optimal_feedback_control_example.jl

@@ -19,7 +19,11 @@ 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)
+
+    # 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)))
@@ -32,7 +36,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
@@ -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],

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

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
@@ -17,6 +17,8 @@ function initialize_model(goal_state)
 
     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)
     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])