Skip to content

Commit

Permalink
Fixed pipeline
Browse files Browse the repository at this point in the history
  • Loading branch information
@ricburli
ricburli committed Jan 8, 2025
1 parent 7d01100 commit e3f40b1
Show file tree
Hide file tree
Showing 5 changed files with 165 additions and 418 deletions.
6 changes: 3 additions & 3 deletions examples/generate_position_trajectory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,12 @@
boundary_constraints = {"position": {"initial": [0, 0, 0], "final": [2.5, 0.25, 0.5]}}

# Calculate the translation trajectory given the invariants data
invariants, trajectory, mf, progress_values = generate_trajectory_translation(invariant_model, boundary_constraints)
invariants, trajectory, mf, progress_values = generate_trajectory_translation(invariant_model, boundary_constraints,len(invariant_model))

# Plotting results
plotters.plot_trajectory_and_bounds(boundary_constraints, trajectory) # generated trajectory and boundary constraints
plotters.plot_moving_frames(trajectory, mf) # calculated moving frames along trajectory
plotters.animate_moving_frames(trajectory, mf) # animated moving frames along trajectory
plotters.plot_moving_frames(trajectory, mf.T) # calculated moving frames along trajectory
plotters.animate_moving_frames(trajectory, mf.T) # animated moving frames along trajectory

# Plot the invariant model and the calculated invariants
plotters.compare_invariants(invariants, invariant_model[:,1:], progress_values, invariant_model[:,0])
73 changes: 63 additions & 10 deletions examples/trajectory_generation/online_generation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@
import invariants_py.spline_handler as spline_handler
import invariants_py.plotting_functions.plotters as plotters

solver = 'fatrop'

"""Input data"""

data_location = dh.find_data_path("sine_wave.txt")
Expand Down Expand Up @@ -74,12 +76,38 @@
R_FS_start = movingframes[current_index]
R_FS_end = movingframes[-1]

boundary_constraints = {
"position": {
"initial": p_obj_start,
"final": p_obj_end
},
"moving-frame": {
"translational": {
"initial": R_FS_start,
"final": R_FS_end
}
},
}
initial_values = {
"trajectory": {
"position": calculate_trajectory
},
"moving-frame": {
"translational": movingframes,
},
"invariants": {
"translational": model_invariants,
}
}

weights = {}
weights['w_invars'] = np.array([5 * 10 ** 1, 1.0, 1.0])

# specify optimization problem symbolically
FS_online_generation_problem = OCP_gen_pos(N=number_samples,w_invars = 10**2*np.array([10**1, 1.0, 1.0]))
FS_online_generation_problem = OCP_gen_pos(boundary_constraints,number_samples,solver=solver)

# Solve
new_invars, new_trajectory, new_movingframes = FS_online_generation_problem.generate_trajectory(U_demo = model_invariants, p_obj_init = calculate_trajectory, R_t_init = movingframes, R_t_start = R_FS_start, R_t_end = R_FS_end, p_obj_start = p_obj_start, p_obj_end = p_obj_end, step_size = new_stepsize)
new_invars, new_trajectory, new_movingframes = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values=initial_values)


fig = plt.figure(figsize=(14,8))
Expand Down Expand Up @@ -112,10 +140,35 @@

#%% Visualization

window_len = 20
window_len = 40

boundary_constraints = {
"position": {
"initial": calculate_trajectory[current_index],
"final": calculate_trajectory[current_index] # will be updated later
},
"moving-frame": {
"translational": {
"initial": movingframes[current_index],
"final": movingframes[-1]
},
},
}

initial_values = {
"trajectory": {
"position": calculate_trajectory,
},
"moving-frame": {
"translational": movingframes,
},
"invariants": {
"translational": model_invariants,
}
}

# specify optimization problem symbolically
FS_online_generation_problem = OCP_gen_pos(N=window_len,w_invars = 10**1*np.array([10**1, 1.0, 1.0]))
FS_online_generation_problem = OCP_gen_pos(boundary_constraints, number_samples, solver = solver)



Expand All @@ -139,14 +192,14 @@
# Boundary constraints
current_index = round( (current_progress - old_progress) * len(calculate_trajectory))
#print(current_index)
p_obj_start = calculate_trajectory[current_index]
p_obj_end = trajectory[-1] - current_progress*np.array([-0.2, 0.0, 0.0])
R_FS_start = movingframes[current_index]
R_FS_end = movingframes[-1]
boundary_constraints["position"]["initial"] = calculate_trajectory[current_index]
boundary_constraints["position"]["final"] = trajectory[-1] - current_progress*np.array([-0.2, 0.0, 0.0])
boundary_constraints["moving-frame"]["translational"]["initial"] = movingframes[current_index]
boundary_constraints["moving-frame"]["translational"]["final"] = movingframes[-1]

# Calculate remaining trajectory
new_invars, calculate_trajectory, movingframes = FS_online_generation_problem.generate_trajectory(U_demo = model_invariants, p_obj_init = calculate_trajectory, R_t_init = movingframes, R_t_start = R_FS_start, R_t_end = R_FS_end, p_obj_start = p_obj_start, p_obj_end = p_obj_end, step_size = new_stepsize)

new_invars, calculate_trajectory, movingframes = FS_online_generation_problem.generate_trajectory(model_invariants,boundary_constraints,new_stepsize,weights,initial_values)
movingframes = movingframes.T
# Visualization
#clear_output(wait=True)

Expand Down
62 changes: 31 additions & 31 deletions invariants_py/generate_trajectory/opti_generate_position_traj_from_vector_invars.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,7 @@ def generate_trajectory(self, invariant_model, boundary_constraints, step_size,
if i!= N-1:
invars[i,:] = self.solution[i].T
p_obj_sol[i,:] = self.solution[N-1+i].T
R_t_sol = self.solution[2*N-1+i]
R_t_sol[:,:,i] = self.solution[2*N-1+i]

# Extract the solved variables
invariants = np.array(invars)
Expand Down Expand Up @@ -268,46 +268,46 @@ def generate_trajectory(self, invariant_model, boundary_constraints, step_size,

# return invariants, calculated_trajectory, calculated_movingframe

def generate_trajectory_global(self,invars_demo,initial_values,boundary_constraints,step_size):
# def generate_trajectory_global(self,invars_demo,initial_values,boundary_constraints,step_size):

N = self.N
# N = self.N

# Initialize states
for k in range(N):
self.opti.set_initial(self.R_t[k], initial_values["moving-frame"]["translational"][k])
self.opti.set_initial(self.p_obj[k], initial_values["trajectory"]["position"][k])
# # Initialize states
# for k in range(N):
# self.opti.set_initial(self.R_t[k], initial_values["moving-frame"]["translational"][k])
# self.opti.set_initial(self.p_obj[k], initial_values["trajectory"]["position"][k])

# Initialize controls
for k in range(N-1):
self.opti.set_initial(self.invars[:,k], invars_demo[k,:])

# Set values boundary constraints
#self.opti.set_value(self.R_t_start,boundary_constraints["moving-frame"]["translational"]["initial"])
#self.opti.set_value(self.R_t_end,boundary_constraints["moving-frame"]["translational"]["final"])
self.opti.set_value(self.p_obj_start,boundary_constraints["position"]["initial"])
self.opti.set_value(self.p_obj_end,boundary_constraints["position"]["final"])
# # Initialize controls
# for k in range(N-1):
# self.opti.set_initial(self.invars[:,k], invars_demo[k,:])

# # Set values boundary constraints
# #self.opti.set_value(self.R_t_start,boundary_constraints["moving-frame"]["translational"]["initial"])
# #self.opti.set_value(self.R_t_end,boundary_constraints["moving-frame"]["translational"]["final"])
# self.opti.set_value(self.p_obj_start,boundary_constraints["position"]["initial"])
# self.opti.set_value(self.p_obj_end,boundary_constraints["position"]["final"])

# Set values parameters
self.opti.set_value(self.h,step_size)
for k in range(N-1):
self.opti.set_value(self.invars_demo[:,k], invars_demo[k,:])
# # Set values parameters
# self.opti.set_value(self.h,step_size)
# for k in range(N-1):
# self.opti.set_value(self.invars_demo[:,k], invars_demo[k,:])

# Solve the NLP
sol = self.opti.solve_limited()
self.sol = sol
# # Solve the NLP
# sol = self.opti.solve_limited()
# self.sol = sol

# Extract the solved variables
invariants = sol.value(self.invars).T
invariants = np.vstack((invariants,[invariants[-1,:]]))
calculated_trajectory = np.array([sol.value(i) for i in self.p_obj])
calculated_movingframe = np.array([sol.value(i) for i in self.R_t])
# # Extract the solved variables
# invariants = sol.value(self.invars).T
# invariants = np.vstack((invariants,[invariants[-1,:]]))
# calculated_trajectory = np.array([sol.value(i) for i in self.p_obj])
# calculated_movingframe = np.array([sol.value(i) for i in self.R_t])

return invariants, calculated_trajectory, calculated_movingframe
# return invariants, calculated_trajectory, calculated_movingframe

def generate_trajectory_translation(invariant_model, boundary_constraints, N=100):

# Specify optimization problem symbolically
OCP = OCP_gen_pos(N = N)
OCP = OCP_gen_pos(boundary_constraints,N = N)

# Initial values
initial_values, initial_values_dict = generate_initvals_from_constraints_opti(boundary_constraints, N)
Expand All @@ -318,7 +318,7 @@ def generate_trajectory_translation(invariant_model, boundary_constraints, N=100
model_invariants,progress_step = sh.interpolate_invariants(spline_invariant_model, progress_values)

# Calculate remaining trajectory
invariants, trajectory, mf = OCP.generate_trajectory_global(model_invariants,initial_values_dict,boundary_constraints,progress_step)
invariants, trajectory, mf = OCP.generate_trajectory(model_invariants,boundary_constraints,progress_step,initial_values=initial_values_dict)

return invariants, trajectory, mf, progress_values

Expand Down
2 changes: 1 addition & 1 deletion invariants_py/ocp_initialization.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,7 +126,7 @@ def generate_initvals_from_constraints_opti(boundary_constraints,N, skip = {}, q
"translational": initial_movingframes
},
"invariants": {
"translational": initial_invariants
"translational": initial_invariants.T
}
}

Expand Down
Loading

0 comments on commit e3f40b1

Please sign in to comment.