Merge pull request #109 from OpenSourceAWE/transform

Add the function reposition

Author: 1-Bart-1

.gitignore

@@ -1,4 +1,5 @@
 /LocalPreferences*
+.helix
 *.arrow
 *.bin
 *.bin.xz

bin/run_julia

@@ -110,6 +110,8 @@ if [[ "$copy_manifest" == "true" ]]; then
         else
             println("Copying default manifest: \n\t", src_manifest, " -> \n\t", dest_manifest)
             cp(src_manifest, dest_manifest, force=true)
+            # Ensure the copied manifest is not write-protected
+            chmod(dest_manifest, 0o644)
         end
     '
 

docs/src/exported_functions.md

@@ -16,6 +16,7 @@ These functions provide convenient wrappers for running common simulation scenar
 sim!
 sim_oscillate!
 sim_turn!
+sim_reposition!
 ```
 
 ## Low-Level Simulation and Analysis

docs/src/private_functions.md

@@ -28,6 +28,7 @@ SymbolicAWEModels.setstate!
 SymbolicAWEModels.set_measured!
 SymbolicAWEModels.copy!
 SymbolicAWEModels.reinit!
+SymbolicAWEModels.reposition!
 SymbolicAWEModels.update_sys_struct!
 SymbolicAWEModels.get_set_hash
 SymbolicAWEModels.get_sys_struct_hash

examples/reposition.jl

@@ -0,0 +1,29 @@
+# Copyright (c) 2025 Bart van de Lint
+# SPDX-License-Identifier: MPL-2.0
+
+using Pkg
+if ! ("ControlPlots" ∈ keys(Pkg.project().dependencies))
+    using TestEnv; TestEnv.activate()
+end
+using SymbolicAWEModels, KiteUtils, LinearAlgebra, ControlPlots, OrdinaryDiffEqBDF
+
+# Initialize the model
+set = Settings("system.yaml")
+set.sample_freq = 100
+sam = SymbolicAWEModel(set)
+init!(sam)
+find_steady_state!(sam)
+
+# Run the simulation with repositioning
+lg = SymbolicAWEModels.sim_reposition!(
+    sam,
+    total_time=10.0,
+    reposition_interval_s=0.5,
+    target_elevation_deg=50.0,
+    target_azimuth_deg=10.0
+)
+
+# Plot the results
+plot(sam.sys_struct, lg)
+
+

src/SymbolicAWEModels.jl

@@ -70,7 +70,7 @@ export DynamicsType, DYNAMIC, QUASI_STATIC, WING, STATIC
 export SegmentType, POWER_LINE, STEERING_LINE, BRIDLE
 
 # --- High-Level Simulation Functions (Workers) ---
-export sim!, sim_oscillate!, sim_turn!
+export sim!, sim_oscillate!, sim_turn!, sim_reposition!
 
 # --- Low-Level Simulation Functions ---
 export find_steady_state!

src/model_management.jl

@@ -237,10 +237,8 @@ Create and cache a simplified linear model if it does not exist or if the output
 # Returns
 - `true` if a new model was created, `false` otherwise.
 """
-function maybe_create_simple_lin_model!(sam, outputs; create_simple_lin_model=true,
-                                        outputs_changed=false, prn=true)
-    if create_simple_lin_model &&
-            (isnothing(sam.simple_lin_model) || outputs_changed)
+function maybe_create_simple_lin_model!(sam, outputs; create_simple_lin_model=true, prn=true)
+    if create_simple_lin_model && isnothing(sam.simple_lin_model)
         sys = sam.prob.sys
         time = @elapsed slm_attrs = generate_simple_lin_model(sam.sys_struct, sys, outputs)
         if !isnothing(slm_attrs.model)
@@ -267,10 +265,8 @@ Create and cache the `LinearizationProblem` if it does not exist or if the outpu
 # Returns
 - `true` if a new problem was created, `false` otherwise.
 """
-function maybe_create_lin_prob!(sam, outputs; create_lin_prob=true,
-                                    outputs_changed=false, prn=true)
-    if create_lin_prob &&
-            (isnothing(sam.lin_prob) || outputs_changed)
+function maybe_create_lin_prob!(sam, outputs; create_lin_prob=true, prn=true)
+    if create_lin_prob && isnothing(sam.lin_prob)
         time = @elapsed @suppress_err begin
             lin_fun, lin_sys = linearization_function(sam.full_sys, [sam.inputs...], outputs;
                                                     op=sam.defaults, guesses=sam.guesses)
@@ -300,10 +296,8 @@ Create and cache the control functions if they do not exist or if the outputs ha
 # Returns
 - `true` if new functions were created, `false` otherwise.
 """
-function maybe_create_control_functions!(sam, outputs; create_control_func=false,
-                                        outputs_changed=false, prn=true)
-    if create_control_func &&
-            (isnothing(sam.control_funcs) || outputs_changed)
+function maybe_create_control_functions!(sam, outputs; create_control_func=false, prn=true)
+    if create_control_func && isnothing(sam.control_funcs)
         inputs = [sam.inputs...]
         time = @elapsed result = generate_control_funcs(sam.full_sys, inputs, outputs)
         sam.control_funcs = ControlFuncWithAttributes(; result...)
@@ -388,16 +382,19 @@ function init!(sam::SymbolicAWEModel;
         outputs_changed = isnothing(sam.outputs) ||
                             length(outputs) != length(sam.outputs) ||
                             !all(string.(outputs) .== string.(sam.outputs))
+        if outputs_changed
+            sam.simple_lin_model = nothing
+            sam.lin_prob = nothing
+            sam.control_funcs = nothing
+        end
         sam.outputs = outputs
 
+        changed |= outputs_changed
         changed |= maybe_create_prob!(sam; create_prob, prn)
         changed |= maybe_create_simple_lin_model!(sam, outputs;
-                                                create_simple_lin_model=create_prob,
-                                                outputs_changed, prn)
-        changed |= maybe_create_lin_prob!(sam, outputs; create_lin_prob,
-                                        outputs_changed, prn)
-        changed |= maybe_create_control_functions!(sam, outputs; create_control_func,
-                                                    outputs_changed, prn)
+                                                  create_simple_lin_model=create_prob, prn)
+        changed |= maybe_create_lin_prob!(sam, outputs; create_lin_prob, prn)
+        changed |= maybe_create_control_functions!(sam, outputs; create_control_func, prn)
 
         if changed
             prn && @info "Serializing model to: \n\t$model_path"

src/precompile.jl

@@ -127,13 +127,16 @@ end
             data_path = joinpath(path, "data")
             set_data_path(data_path)
 
-            # Copy .default files to expected files
-            cp(joinpath(path, "Artifacts.toml.default"), joinpath(path, "Artifacts.toml"); force=true)
+            # Copy .default files to expected files and make them writable
+            artifacts_toml_path = joinpath(path, "Artifacts.toml")
+            cp(joinpath(path, "Artifacts.toml.default"), artifacts_toml_path; force=true)
+            chmod(artifacts_toml_path, 0o644)
 
             version_minor = VERSION.minor
             manifest_default = joinpath(path, "Manifest-v1.$version_minor.toml.default")
             manifest_actual = joinpath(path, "Manifest-v1.$version_minor.toml")
             cp(manifest_default, manifest_actual; force=true)
+            chmod(manifest_actual, 0o644)
 
             # Use explicit Artifacts API to get the artifact and extract it
             artifact_toml = joinpath(path, "Artifacts.toml")
@@ -144,7 +147,10 @@ end
                 model_dir = artifact_path(model_hash)
                 mkpath(data_path)
                 for f in readdir(model_dir)
-                    cp(joinpath(model_dir, f), joinpath(data_path, f); force=true)
+                    src_path = joinpath(model_dir, f)
+                    dest_path = joinpath(data_path, f)
+                    cp(src_path, dest_path; force=true)
+                    chmod(dest_path, 0o644) # Ensure copied artifact file is writable
                 end
                 @info "Downloaded and extracted $artifact_name to $data_path"
             else

src/simulate.jl

@@ -225,6 +225,113 @@ function sim_turn!(
 end
 
 
+"""
+    sim_reposition!(sam; dt, total_time, reposition_interval_s, target_elevation_deg,
+                    target_azimuth_deg, prn)
+
+Run a simulation that periodically resets the kite's elevation and azimuth.
+
+This function simulates the AWE model and, at a specified time interval, calls
+`reposition!` to reposition the kite to a target elevation and azimuth. It logs
+the entire simulation and returns a `SysLog`.
+
+# Arguments
+- `sam::SymbolicAWEModel`: Initialized AWE model.
+
+# Keywords
+- `dt::Float64`: Time step [s]. Defaults to `1/sam.set.sample_freq`.
+- `total_time::Float64`: Total simulation duration [s]. Defaults to 20.0.
+- `reposition_interval_s::Float64`: The interval in seconds at which to reset the pose. Defaults to 5.0.
+- `target_elevation::Float64`: The target elevation in rad for repositioning. Defaults to deg2rad(45.0).
+- `target_azimuth::Float64`: The target azimuth in rad for repositioning. Defaults to 0.0.
+- `target_heading::Float64`: The target heading in rad for repositioning. Defaults to 0.0.
+- `prn::Bool`: If true, prints status messages during the simulation. Defaults to true.
+
+# Returns
+- `SysLog`: A log of the complete simulation.
+"""
+function sim_reposition!(
+    sam::SymbolicAWEModel;
+    dt=1/sam.set.sample_freq,
+    total_time=20.0,
+    reposition_interval_s=5.0,
+    target_elevation=deg2rad(45.0),
+    target_azimuth=0.0,
+    target_heading=0.0,
+    prn=true
+)
+    # 1. --- Initialization ---
+    sys_struct = sam.sys_struct
+    steps = Int(round(total_time / dt))
+    reposition_interval_steps = Int(round(reposition_interval_s / dt))
+    set_values = zeros(Float64, steps, length(sys_struct.winches))
+    vsm_interval = 1 ÷ dt
+    
+    logger = Logger(length(sys_struct.points), steps)
+    sys_state = SysState(sam)
+
+    if prn
+        println("--- Starting simulation with periodic repositioning ---")
+        println("Total time: $(total_time)s, Reposition interval: $(reposition_interval_s)s")
+    end
+
+    # 2. --- Simulation Loop ---
+    time = @elapsed for step in 1:steps
+        t = (step-1) * dt
+        
+        # Hold the kite in place by countering the tether forces with winch torques
+        set_values[step, :] = -sam.set.drum_radius .* [norm(winch.force) for winch in sys_struct.winches]
+        
+        # --- Repositioning Logic ---
+        if step > 1 && (step - 1) % reposition_interval_steps == 0
+            if prn
+                println("\n>>> Time: $(round(t, digits=2))s. Repositioning kite...")
+                println(">>> Target Elevation: $(rad2deg(target_elevation))°,"*
+                        " Target Azimuth: $(rad2deg(target_azimuth))°")
+            end
+            
+            # Update the transform with the new target pose
+            sys_struct.transforms[1].elevation = target_elevation
+            sys_struct.transforms[1].azimuth   = target_azimuth
+            sys_struct.transforms[1].heading   = target_heading - sys_struct.wings[1].heading
+            
+            # Apply the transformation without changing velocities
+            SymbolicAWEModels.reposition!(sys_struct.transforms, sys_struct)
+            
+            # Reinitialize the solver to handle the state discontinuity
+            SymbolicAWEModels.reinit!(sam, sam.prob, FBDF())
+
+            if prn
+                # Verify the new pose after one step
+                next_step!(sam; dt=dt, set_values=set_values[step, :], vsm_interval)
+                updated_elevation_deg = rad2deg(sys_struct.wings[1].elevation)
+                println(">>> Pose updated. New Elevation is now: " *
+                        "$(round(updated_elevation_deg, digits=2)) degrees.\n")
+            end
+        else
+            # --- Normal simulation step ---
+            next_step!(sam; dt=dt, set_values=set_values[step, :], vsm_interval)
+        end
+        
+        # Log the state at the current time step
+        update_sys_state!(sys_state, sam)
+        sys_state.time = t
+        log!(logger, sys_state)
+    end
+
+    if prn
+        println("--- Simulation Finished ---")
+        println("Runtime: $time")
+        println("Times realtime: $(dt*steps/time)")
+    end
+
+    # Save and return the log
+    mkpath(get_data_path())
+    save_log(logger, "tmp_reposition_run")
+    return load_log("tmp_reposition_run")
+end
+
+
 """
     SysState(y::AbstractVector, sam::SymbolicAWEModel, t::Real; zoom=1.0)
 

src/system_structure.jl

@@ -866,6 +866,66 @@ function reinit!(transforms::Vector{Transform}, sys_struct::SystemStructure)
     end
 end
 
+"""
+    reposition!(transforms::Vector{Transform}, sys_struct::SystemStructure)
+
+Update the system's spatial orientation based on its current position, preserving velocities.
+
+This function adjusts the orientation of all components in the `SystemStructure` without
+altering their dynamic state. Unlike `reinit!`, it uses the current world positions (`pos_w`)
+as the starting point for rotations, rather than resetting from the CAD coordinates.
+
+This function is useful for making real-time adjustments to the system's pose during a simulation.
+Crucially, it **preserves the existing velocities (`vel_w`) of all points and wings**.
+
+NOTE: the transform.heading is applied relative to the current heading of the system.
+
+# Arguments
+- `sys_struct::SystemStructure`: The system model to update.
+"""
+function reposition!(transforms::Vector{Transform}, sys_struct::SystemStructure)
+    @unpack points, wings = sys_struct
+    for transform in transforms
+        # Get the current positions of the base and the rotating object
+        base_pos = points[transform.base_point_idx].pos_w
+        rot_pos = get_rot_pos(transform, wings, points)
+
+        # Calculate the current orientation in spherical coordinates
+        current_rel_pos = rot_pos - base_pos
+        curr_elevation = KiteUtils.calc_elevation(current_rel_pos)
+        curr_azimuth = -KiteUtils.azimuth_east(current_rel_pos)
+
+        # Get the rotation matrices for the current and target orientations
+        curr_R_t_w = calc_R_t_w(curr_elevation, curr_azimuth)
+        R_t_w = calc_R_t_w(transform.elevation, transform.azimuth)
+
+        # Apply the rotation to all relevant points
+        for point in points
+            if point.transform_idx == transform.idx
+                vec = point.pos_w - base_pos
+                point.pos_w .= base_pos + apply_heading(vec, R_t_w, curr_R_t_w, transform.heading)
+            end
+        end
+
+        # Apply the rotation to all relevant wings
+        for wing in wings
+            if wing.transform_idx == transform.idx
+                # Rotate the wing's position
+                vec = wing.pos_w - base_pos
+                wing.pos_w .= base_pos + apply_heading(vec, R_t_w, curr_R_t_w, transform.heading)
+
+                # Rotate the wing's orientation matrix
+                R_b_w = zeros(3,3)
+                current_R_b_w = wing.R_b_w
+                for i in 1:3
+                    R_b_w[:, i] .= apply_heading(current_R_b_w[:, i], R_t_w, curr_R_t_w, transform.heading)
+                end
+                wing.R_b_w = R_b_w
+            end
+        end
+    end
+end
+
 """
     calc_pos(wing::RamAirWing, gamma, frac)