diff --git a/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx b/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx new file mode 100644 index 000000000..d1c96570c Binary files /dev/null and b/.cache/clangd/index/LQR_setup.cpp.2C597C3359569B5F.idx differ diff --git a/.cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx b/.cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx new file mode 100644 index 000000000..3980a6ee8 Binary files /dev/null and b/.cache/clangd/index/LQR_setup.hpp.7B50E8B15298D255.idx differ diff --git a/.cache/clangd/index/NMPC_acados.cpp.326F2FC10CEB64DA.idx b/.cache/clangd/index/NMPC_acados.cpp.326F2FC10CEB64DA.idx new file mode 100644 index 000000000..2cfa2196a Binary files /dev/null and b/.cache/clangd/index/NMPC_acados.cpp.326F2FC10CEB64DA.idx differ diff --git a/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx b/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx new file mode 100644 index 000000000..c4754ccd0 Binary files /dev/null and b/.cache/clangd/index/NMPC_acados.hpp.29D23C04AD60A55C.idx differ diff --git a/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx b/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx new file mode 100644 index 000000000..ee7f67abe Binary files /dev/null and b/.cache/clangd/index/NMPC_setup.cpp.86F9F0381989E206.idx differ diff --git a/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx b/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx new file mode 100644 index 000000000..4f031ff59 Binary files /dev/null and b/.cache/clangd/index/NMPC_setup.hpp.CAD16FEB6583910A.idx differ diff --git a/.cache/clangd/index/PID_setup.cpp.3C9DCE2555B8A965.idx b/.cache/clangd/index/PID_setup.cpp.3C9DCE2555B8A965.idx new file mode 100644 index 000000000..bda65f6c0 Binary files /dev/null and b/.cache/clangd/index/PID_setup.cpp.3C9DCE2555B8A965.idx differ diff --git a/.cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx b/.cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx new file mode 100644 index 000000000..7a301cdda Binary files /dev/null and b/.cache/clangd/index/PID_setup.hpp.486A8D0A6ED861A2.idx differ diff --git a/.cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx b/.cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx new file mode 100644 index 000000000..795aba4db Binary files /dev/null and b/.cache/clangd/index/acados_sim_solver_auv_model.c.53A9281F3B2EA7B4.idx differ diff --git a/.cache/clangd/index/acados_sim_solver_auv_model.h.995A50C901E2AF1C.idx b/.cache/clangd/index/acados_sim_solver_auv_model.h.995A50C901E2AF1C.idx new file mode 100644 index 000000000..44fe5dcba Binary files /dev/null and b/.cache/clangd/index/acados_sim_solver_auv_model.h.995A50C901E2AF1C.idx differ diff --git a/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx b/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx new file mode 100644 index 000000000..3b988c84a Binary files /dev/null and b/.cache/clangd/index/acados_solver_auv_model.c.5778764B3A2FDFAE.idx differ diff --git a/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx b/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx new file mode 100644 index 000000000..b6757e3c5 Binary files /dev/null and b/.cache/clangd/index/acados_solver_auv_model.h.F9AC8187D4C1016F.idx differ diff --git a/.cache/clangd/index/auv_model_impl_dae_fun.c.CDE429F376604CAB.idx b/.cache/clangd/index/auv_model_impl_dae_fun.c.CDE429F376604CAB.idx new file mode 100644 index 000000000..b15ce8e19 Binary files /dev/null and b/.cache/clangd/index/auv_model_impl_dae_fun.c.CDE429F376604CAB.idx differ diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx new file mode 100644 index 000000000..4d1476f88 Binary files /dev/null and b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u.c.6132B1FDE79246FD.idx differ diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx new file mode 100644 index 000000000..661e76b4e Binary files /dev/null and b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_u_z.c.ED4BE4E791391EA0.idx differ diff --git a/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx new file mode 100644 index 000000000..c81bc5f73 Binary files /dev/null and b/.cache/clangd/index/auv_model_impl_dae_fun_jac_x_xdot_z.c.59CA37EE71B4BA5D.idx differ diff --git a/.cache/clangd/index/auv_model_impl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx b/.cache/clangd/index/auv_model_impl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx new file mode 100644 index 000000000..298d4f362 Binary files /dev/null and b/.cache/clangd/index/auv_model_impl_dae_jac_x_xdot_u_z.c.A6D29D52260DC082.idx differ diff --git a/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx b/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx new file mode 100644 index 000000000..1bf1d1617 Binary files /dev/null and b/.cache/clangd/index/auv_model_model.h.60CCFC5FA93A2F4B.idx differ diff --git a/.cache/clangd/index/ct_instantiations.cpp.CECDE40637351DC9.idx b/.cache/clangd/index/ct_instantiations.cpp.CECDE40637351DC9.idx new file mode 100644 index 000000000..03d59ecae Binary files /dev/null and b/.cache/clangd/index/ct_instantiations.cpp.CECDE40637351DC9.idx differ diff --git a/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx b/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx new file mode 100644 index 000000000..01481f6f9 Binary files /dev/null and b/.cache/clangd/index/main_auv_model.c.FC541DFFAD75A3A0.idx differ diff --git a/.cache/clangd/index/main_sim_auv_model.c.E637641F2593FF77.idx b/.cache/clangd/index/main_sim_auv_model.c.E637641F2593FF77.idx new file mode 100644 index 000000000..874d6be71 Binary files /dev/null and b/.cache/clangd/index/main_sim_auv_model.c.E637641F2593FF77.idx differ diff --git a/.cache/clangd/index/test_LQR.cpp.46B8F24A8C36EC93.idx b/.cache/clangd/index/test_LQR.cpp.46B8F24A8C36EC93.idx new file mode 100644 index 000000000..528733b0b Binary files /dev/null and b/.cache/clangd/index/test_LQR.cpp.46B8F24A8C36EC93.idx differ diff --git a/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx b/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx new file mode 100644 index 000000000..c950a9668 Binary files /dev/null and b/.cache/clangd/index/test_PID.cpp.575590D7897A814B.idx differ diff --git a/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx b/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx new file mode 100644 index 000000000..5c01a6f3a Binary files /dev/null and b/.cache/clangd/index/test_VC.cpp.74BA115DB14CB17C.idx differ diff --git a/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx b/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx new file mode 100644 index 000000000..27b853ec8 Binary files /dev/null and b/.cache/clangd/index/test_VC.hpp.C3EBE494A18C2184.idx differ diff --git a/.cache/clangd/index/utilities.cpp.7F99D4E1DE20E3AB.idx b/.cache/clangd/index/utilities.cpp.7F99D4E1DE20E3AB.idx new file mode 100644 index 000000000..a9df09968 Binary files /dev/null and b/.cache/clangd/index/utilities.cpp.7F99D4E1DE20E3AB.idx differ diff --git a/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx b/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx new file mode 100644 index 000000000..8b16d9f92 Binary files /dev/null and b/.cache/clangd/index/utilities.hpp.77C0A5FDF681DAA0.idx differ diff --git a/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx b/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx new file mode 100644 index 000000000..7a23290a5 Binary files /dev/null and b/.cache/clangd/index/velocity_controller.cpp.DFC34CF5F86A4B55.idx differ diff --git a/.cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx b/.cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx new file mode 100644 index 000000000..fc7431d5e Binary files /dev/null and b/.cache/clangd/index/velocity_controller.hpp.3E0346F5513060F5.idx differ diff --git a/.gitignore b/.gitignore index 41eed4018..68e1f5f0c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,9 +1,9 @@ # Created by https://www.gitignore.io/api/ros # Edit at https://www.gitignore.io/?templates=ros - + # CMake cmake-build-debug/ - + ### ROS ### install/ log/ @@ -21,41 +21,39 @@ msg/*Result.msg msg/_*.py build_isolated/ devel_isolated/ -*.cache/ - + # Generated by dynamic reconfigure *.cfgc /cfg/cpp/ /cfg/*.py - + # Ignore generated docs *.dox *.wikidoc - + # IDE stuff .project .cproject .vscode .DS_Store .idea - + # qcreator stuff CMakeLists.txt.user - + srv/_*.py *.pcd *.pyc qtcreator-* *.user -*hintmux.sh - + /planning/cfg /planning/docs /planning/src - + *~ - + # Emacs .#* - -# End of https://www.gitignore.io/api/ros + .cache +# End of https://www.gitignore.io/api/ros \ No newline at end of file diff --git a/control/velocity_controller_lqr/velocity_controller_lqr/__init__.py b/.ignore similarity index 100% rename from control/velocity_controller_lqr/velocity_controller_lqr/__init__.py rename to .ignore diff --git a/auv_setup/config/robots/orca.yaml b/auv_setup/config/robots/orca.yaml index 47dfd3ca9..5fd09ccf8 100644 --- a/auv_setup/config/robots/orca.yaml +++ b/auv_setup/config/robots/orca.yaml @@ -155,6 +155,7 @@ set_killswitch: "set_killswitch" toggle_killswitch: "toggle_killswitch" get_operation_mode: "get_operation_mode" + los_mode: "set_los_mode" waypoint_addition: "waypoint_addition" fsm: diff --git a/auv_setup/launch/autopilot.launch.py b/auv_setup/launch/autopilot.launch.py index 2e07d2e1c..69267c3d4 100644 --- a/auv_setup/launch/autopilot.launch.py +++ b/auv_setup/launch/autopilot.launch.py @@ -9,44 +9,65 @@ ) from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration - - +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode def launch_setup(context, *args, **kwargs): - drone = LaunchConfiguration("drone").perform(context) - - los_guidance_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("los_guidance"), - "launch", - "los_guidance.launch.py", - ) - ), - launch_arguments={"drone": drone}.items(), + drone, namespace = resolve_drone_and_namespace(context) + drone_params = os.path.join( + get_package_share_directory("auv_setup"), + "config", + "robots", + f"{drone}.yaml", ) - - autopilot_controller_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory("velocity_controller_lqr"), - "launch", - "velocity_controller_lqr.launch.py", - ) - ), - launch_arguments={"drone": drone}.items(), + VC_params = os.path.join( + get_package_share_directory("velocity_controller"), + "config", + "parameters.yaml", ) + adapt_params = os.path.join( + get_package_share_directory("los_guidance"), + "config", + "guidance_params.yaml", + ) + container=ComposableNodeContainer( + name="autopilot_container", + namespace=namespace, + package="rclcpp_components", + executable="component_container_mt", + composable_node_descriptions=[ + ComposableNode( + package="velocity_controller", + plugin="velocity_controller_node", + name="velocity_controller_node", + namespace=namespace, + parameters=[VC_params,drone_params], + extra_arguments=[{"use_intra_process_comms":True}], + ), + ComposableNode( + package="los_guidance", + plugin="los_guidance_node", + name="los_guidance_node", + namespace=namespace, + parameters=[adapt_params,drone_params], + extra_arguments=[{"use_intra_process_comms":True}], + ), - return [los_guidance_launch, autopilot_controller_launch] + ], + output="screen", + arguments=["--ros-args","--log-level","error"], + ) + return [container] + -def generate_launch_description() -> LaunchDescription: +def generate_launch_description(): return LaunchDescription( - [ - DeclareLaunchArgument( - "drone", - default_value="orca", - description="Drone name / namespace", - ), + declare_drone_and_namespace_args() + + [ OpaqueFunction(function=launch_setup), ] ) diff --git a/control/velocity_controller/CMakeLists.txt b/control/velocity_controller/CMakeLists.txt new file mode 100644 index 000000000..30fd54084 --- /dev/null +++ b/control/velocity_controller/CMakeLists.txt @@ -0,0 +1,113 @@ +cmake_minimum_required(VERSION 3.8) +project(velocity_controller) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(vortex_utils REQUIRED) +find_package(Eigen3 REQUIRED) +#find_package(CasADi REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(ct_optcon REQUIRED) +find_package(ct_core REQUIRED) +find_package(casadi REQUIRED) + +set(ACADOS_ROOT "$ENV{ACADOS_SOURCE_DIR}" CACHE PATH "acados root") +include_directories( + ${ACADOS_ROOT}/include + ${ACADOS_ROOT}/include/acados + ${ACADOS_ROOT}/include/acados/ocp_nlp + ${ACADOS_ROOT}/include/acados_c + ${ACADOS_ROOT}/include/blasfeo/include + ${ACADOS_ROOT}/include/hpipm/include + ${ACADOS_ROOT}/include/qpOASES_e + ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver + + include + +) +link_directories(${ACADOS_ROOT}/lib) + + +# After setting ACADOS_ROOT etc. +file(GLOB_RECURSE GEN_SRCS + ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*.c + ${CMAKE_CURRENT_SOURCE_DIR}/scripts/build_auv_solver/*/*.c +) + + + +add_library(auv_nmpc STATIC + src/NMPC_acados.cpp + ${GEN_SRCS} +) + + +add_executable(velocity_controller_node + src/velocity_controller.cpp + src/PID_setup.cpp + src/LQR_setup.cpp + src/utilities.cpp + src/ct_instantiations.cpp + src/NMPC_setup.cpp + #src/NMPC_acados.cpp +) + + + +#add_executable(test_LQR_node +# src/LQR_test.cpp +# src/LQR_setup.cpp +# src/utilities.cpp +# src/ct_instantiations.cpp +# ) +ament_target_dependencies(velocity_controller_node + rclcpp + rclcpp_lifecycle + lifecycle_msgs + std_msgs + vortex_msgs + geometry_msgs + # CasADi + nav_msgs + vortex_utils +) +#target_include_directories(velocity_controller_node PRIVATE casadi Eigen3) +target_link_libraries(auv_nmpc acados hpipm blasfeo qpOASES_e m) +target_link_libraries(velocity_controller_node Eigen3::Eigen casadi::casadi ct_optcon ct_core auv_nmpc) +install(TARGETS + velocity_controller_node + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) +if(BUILD_TESTING) + add_subdirectory(tests) +endif() + + + + +ament_package() diff --git a/control/velocity_controller/config/nautilus_params.yaml b/control/velocity_controller/config/nautilus_params.yaml new file mode 100644 index 000000000..3d183cae5 --- /dev/null +++ b/control/velocity_controller/config/nautilus_params.yaml @@ -0,0 +1,33 @@ +/**: + ros__parameters: + + PID_params: + surge: [300.0,10.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] + + LQR_params: + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] + R: [0.02,3.1,3.10] + NMPCA_params: + Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi + N: 20 + NMPC_params: + Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi + N: 20 + #inertia_matrix: [ 30.0, 0.0, 0.0, 0.0, 0.0, 0.60, 0.0, 30.0, 0.0, 0.0, -0.60, 0.30, 0.0, 0.0, 30.0, 0.60, 0.30, 0.0, 0.0, 0.0, 0.60, 0.68, 0.0, 0.0, 0.0, -0.60, 0.30, 0.0, 3.32, 0.0, 0.60, 0.30, 0.0, 0.0, 0.0, 3.34] + dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + + publish_rate: 100 #ms + #Clamp parameter + max_force: 99.5 + controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast + + #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi + # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi + + #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + diff --git a/control/velocity_controller/config/orca_params.yaml b/control/velocity_controller/config/orca_params.yaml new file mode 100644 index 000000000..b19015c95 --- /dev/null +++ b/control/velocity_controller/config/orca_params.yaml @@ -0,0 +1,34 @@ +/**: + ros__parameters: + + PID_params: + surge: [300.0,10.0,5.0] + pitch: [60.0,8.0,12.0] + yaw: [10.0,1.0,5.0] + + LQR_params: + #Q: [200.0,62.84,62.84,15.0,15.0,10.0,3.84,3.84] + #R: [8.0,100.0,100.0] + Q: [200.0,32.84,32.84,15.0,15.0,100.0,32.84,32.84] + R: [0.02,3.1,3.10] + NMPCA_params: + Q: [1.0,1.0,1.0,5.0,5.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi + N: 20 + NMPC_params: + Q: [100.0,0.0,0.0,0.0,1.0,1.0,0.0,5.0,50.0] # u,q,r,theta,psi + R: [0.1,0.1,0.1] # u_surge, u_theta, u_psi + N: 20 + dampening_matrix_low: [23.0,0.0,0.0,0.0,0.0,0.0, 0.0,46.0,0.0,0.0,0.0,0.0, 0.0,0.0,46.0,0.0,0.0,0.0, 0.0,0.0,0.0,46.0,0.0,0.0, 0.0,0.0,0.0,0.0,46.0,0.0, 0.0,0.0,0.0,0.0,0.0,46.0] + dampening_matrix_high: [1.0,0.0,0.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0,0.0,0.0, 0.0,0.0,1.0,0.0,0.0,0.0, 0.0,0.0,0.0,1.0,0.0,0.0, 0.0,0.0,0.0,0.0,1.0,0.0, 0.0,0.0,0.0,0.0,0.0,1.0] + + publish_rate: 100 #ms + #Clamp parameter + max_force: 99.5 + controller_type: 2 #1 PID 2 LQR 3 NMPC 4NMPC fast + + #Q: [300.0,0.01,0.01,0.01,32.84,32.84,32.84,32.84,32.84] # u,v,w,p,q,r,phi,theta,psi + # R: [0.02,3.1,3.10] # u_surge, u_theta, u_psi + + #Fixes: reduce oscillations, follows angles close to wrap around, model restoring forces in pitch, test different references, try to find out why the fuck it went backwards?, + diff --git a/control/velocity_controller/include/velocity_controller/LQR_setup.hpp b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp new file mode 100644 index 000000000..3ac78e980 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/LQR_setup.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include +#include +#include +#include "PID_setup.hpp" +#include +#include "velocity_controller/utilities.hpp" + + + +/*class Guidance_values{ + //Dataclass to store guidance values for LQR controller + public: + double surge=0.0; double pitch=0.0; double yaw=0.0; + double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; +}; +*/ +class LQRparameters{ + //Dataclass to store LQR parameters + public: + double q_surge=0.0; double q_pitch=0.0; double q_yaw=0.0; + double r_surge=0.0; double r_pitch=0.0; double r_yaw=0.0; + double i_surge=0.0; double i_pitch=0.0; double i_yaw=0.0; + double i_weight=0.0; double max_force=0.0; +}; + +/*class angle{ + public: + double phit=0.0; + double thetat=0.0; + double psit=0.0; + +};*/ +/* +struct LQRsolveResult{ + Eigen::MatrixXd K; + Eigen::MatrixXd P; + int INFO=0; + LQRsolveResult(Eigen::MatrixXd K=Eigen::MatrixXd::Zero(),Eigen::MatrixXd P=Eigen::MatrixXd::Zero(), int INFO=0):K(K),P(P),INFO(INFO) {}; +};*/ +class LQRController{ + + public: + LQRController(); + bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); + void reset_controller(int nr=0); + bool calculate_thrust(State states, Guidance_data guidance_values); + bool set_interval(double interval); + Eigen::Vector get_thrust(); + private: + Eigen::Matrix linearize(State states); + Eigen::Matrix coriolis(const State& s); + + + std::tuple saturate (double value, bool windup, double limit); + double anti_windup(double error, double integral_sum, bool windup); + Eigen::Vector saturate_input(Eigen::Vector u); + + Eigen::Vector update_error(const Guidance_data& error, const State& state); + + double interval_; + double integral_error_surge; double integral_error_pitch; double integral_error_yaw; + bool surge_windup; bool pitch_windup; bool yaw_windup; + Eigen::Matrix Q; Eigen::Matrix R; Eigen::Matrix B; + Eigen::Matrix D_low; Eigen::Matrix D_high; + double max_force, mass, Ixx, Iyy,Izz; + + Eigen::Matrix inertia_matrix_inv; + Eigen::Matrix state_weight_matrix; + Eigen::Matrix3d input_weight_matrix; + Eigen::Matrix augmented_system_matrix; + Eigen::Matrix augmented_input_matrix; + Eigen::Vector u; + + + + +}; + +//Extra operations +//Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix); +//std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat); +//std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat); +//Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix); diff --git a/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp new file mode 100644 index 000000000..a54c495a6 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/NMPC_acados.hpp @@ -0,0 +1,90 @@ + +#pragma once +#include +#include +#include +#include +#include +#include + +// acados C API (generated) +extern "C" { +#include "acados_c/ocp_nlp_interface.h" +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/utils/print.h" + +// Generated solver headers +#include "acados_solver_auv_model.h" // <-- from build_auv_solver/ +} + +class AuvNMPC { +public: + // Adjust sizes if your model differs + static constexpr int NX = AUV_MODEL_NX; // [u v w p q r phi theta psi] + static constexpr int NU = AUV_MODEL_NU; // [Fx My Mz] + static constexpr int NY = AUV_MODEL_NY; + static constexpr int NY_E = AUV_MODEL_NYN; + + +// Pass N if your generated header does not provide _acados_get_N() + explicit AuvNMPC(int N_horizon_override = -1) + : N_override_(N_horizon_override) {} + + ~AuvNMPC(); + + + // Not copyable + AuvNMPC(const AuvNMPC&) = delete; + AuvNMPC& operator=(const AuvNMPC&) = delete; + + // Lifecycle + bool init(); + + + void set_weights(const std::vector& W_diag, const std::vector& W_e_diag); + void set_max_force(double max_force); // updates bound on con_h: Fx^2+My^2+Mz^2 <= max^2 + + // Inputs + void setState(const std::array& x); + void setReference(const std::array& x_ref); + void setWeights(const std::vector& W_diag, const std::vector& W_e_diag); // sizes: NY, NY_E + void setMaxForce(double max_force); // updates con_h bounds + + + // One-shot solve: provide current state, desired state & input refs, get u0 back. + // Returns solver status (0 == success). + int solve_once(); + bool initialize_guess(std::array x,std::array u_init); + + // Outputs + std::vector getU0(); + + private: + + // generated capsule type (from acados_solver_auv_model.h) + auv_model_solver_capsule* capsule_ = nullptr; + + + // acados solver + ocp_nlp_solver* solver_ = nullptr; + ocp_nlp_config* config_ = nullptr; + ocp_nlp_dims* dims_ = nullptr; + ocp_nlp_in* nlp_in_ = nullptr; + ocp_nlp_out* nlp_out_= nullptr; + + int N_=20; + int N_override_=-1; + + std::array W_; // length NY + std::vector W_e_{NY_E*NY_E,0.0}; // length NY_E + double max_force2_ = 100*100; // squared constraint, default 100^2 + + + static void set_diag(double* M, int n, const std::vector& diag); + //U out + std::vector u0_out={0,0,0}; + //Recorded states states + std::array x0; + std::array xr; + std::array ur={0,0,0}; +}; diff --git a/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp new file mode 100644 index 000000000..19208d44c --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/NMPC_setup.hpp @@ -0,0 +1,40 @@ +#pragma once +//#include +#include +#include +#include "velocity_controller/utilities.hpp" + +class NMPC_controller{ + public: + Eigen::Matrix get_thrust(); + bool calculate_thrust(Guidance_data guidance_values, State state); + bool set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high); + void reset_controller(); + bool set_interval(double interval); + bool initialize_MPC(); + private: + Eigen::Matrix Q_; + Eigen::MatrixR_; + Eigen::MatrixM_inv; + Eigen::MatrixD_low; + Eigen::MatrixD_high; + //Eigen::MatrixB_; + double interval_; + double mass; + double Iz; + double Ix; + double Iy; + int N=3; + int n=9; + int m=3; + casadi::DM Z0_next; //For warm start + casadi::DM lbx; + casadi::DM ubx; + casadi::DM lbg; + casadi::DM ubg; + casadi::DM Pval; + casadi::Function solver; + Eigen::Matrixthrust; + + +}; \ No newline at end of file diff --git a/control/velocity_controller/include/velocity_controller/PID_setup.hpp b/control/velocity_controller/include/velocity_controller/PID_setup.hpp new file mode 100644 index 000000000..8168c90a0 --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/PID_setup.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +class PID_controller { + public: + //PID_controller(double Kp=0, double Ki=0, double Kd=0, double max_output=0, double min_output=0, double dt=0); + //PID_controller(double k_p, double k_i, double k_d) : PID_controller(k_p, k_i, k_d, 100.0, -100.0) {}; + bool calculate_thrust(double error); + bool calculate_thrust(double error, double error_d); + void reset_controller(); + double get_output(); + bool set_output_limits(double min_output, double max_output); + bool set_parameters(double k_p,double k_i, double k_d, double dt); + bool set_parameters(std::vector& params,double dt); + bool set_dt(double dt); + private: + double Kp_, Ki_, Kd_, dt_; + double integral=0; + double previous_error=0; + double output=0; + double max_output_, min_output_; + bool init=false; +}; + diff --git a/control/velocity_controller/include/velocity_controller/utilities.hpp b/control/velocity_controller/include/velocity_controller/utilities.hpp new file mode 100644 index 000000000..c59efcc5a --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/utilities.hpp @@ -0,0 +1,55 @@ +#pragma once +#include +#include +#include +#include "std_msgs/msg/float64_multi_array.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" +#include +#include + + +struct angle{ + double phit=0.0; + double thetat=0.0; + double psit=0.0; +}; +angle quaternion_to_euler_angle(double w, double x, double y, double z); +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); + +class State{ + //Dataclass to store state values for LQR controller + public: + double surge=0.0, sway=0.0, heave=0.0, roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; //roll_rate=0.0, pitch_rate=0.0, yaw_rate=0.0; + double roll=0.0, pitch=0.0, yaw=0.0; //phi, theta, psi + double w=0.0, x=0.0,y=0.0,z=0.0; + //double integral_surge=0.0; double integral_pitch=0.0; double integral_yaw=0.0; + State(double surge=0,double pitch=0, double yaw=0):surge{surge}, pitch{pitch},yaw{yaw}{}; + + //State(){}; + State operator=(int n){if (n){surge=0.0,sway=0.0,heave=0.0,roll_rate=0.0,pitch_rate=0.0,yaw_rate=0.0,roll=0.0,pitch=0.0,yaw=0.0;} return *this;}; + State operator=(nav_msgs::msg::Odometry::SharedPtr rhs); + angle get_angle(); +}; +//TODO: fix these so that changing the quaternions changes the angles, so that the state is always consistent +class Guidance_data{ + public: + double surge=0.0; double pitch=0.0; double yaw=0.0; + //Guidance_data(vortex_msgs::msg::LOSGuidance msg):State{msg.surge,msg.pitch,msg.yaw}{}; + Guidance_data(double surge, double pitch, double yaw):surge{surge},pitch{pitch}, yaw{yaw} {}; + Guidance_data():surge{0.0}, pitch{0.0}, yaw{0.0} {}; + //Guidance_data():State{0, 0, 0} {}; + + //Guidance_data operator-(const Guidance_data& other) const; + Guidance_data& operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg); +}; + +//angle NED_to_BODY(const angle &a,const State &s); +//Eigen::Vector3d NED_to_BODY(const Eigen::Vector3d &a, const State &s); +//Eigen::Matrix3d R_ned_to_body(double roll, double pitch, double yaw); +//Eigen::Matrix3d T_euler_to_body(double roll, double pitch); +Eigen::Vector3d body_rates_to_euler_rates(double roll, double pitch,double p, double q, double r); +angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des, double roll, double pitch, double yaw); +angle angle_NED_to_body(angle desired, angle state); +inline angle angle_NED_to_body(angle desired, angle state){return angle_NED_to_body(desired.phit, desired.thetat, desired.psit, state.phit, state.thetat, state.psit);} + + diff --git a/control/velocity_controller/include/velocity_controller/velocity_controller.hpp b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp new file mode 100644 index 000000000..eff738c7c --- /dev/null +++ b/control/velocity_controller/include/velocity_controller/velocity_controller.hpp @@ -0,0 +1,120 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "LQR_setup.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "vortex_msgs/msg/los_guidance.hpp" +#include "velocity_controller/NMPC_setup.hpp" +#include "velocity_controller/NMPC_acados.hpp" +#include +#include + + + +class Velocity_node : public rclcpp_lifecycle::LifecycleNode{ + public: + explicit Velocity_node(); + Velocity_node(const Velocity_node&)=delete; //no copy constructor + Velocity_node& operator=(const Velocity_node&) = delete; //no copy assignment + //TODO: decide if i one should be allowed to move or transfer ownership if the class + //Different initializatin functions + void get_new_parameters(); + + //Timer functions + void calc_thrust(); + + //Callback functions + void guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + + //Publisher instance + rclcpp::Publisher::SharedPtr publisher_thrust; + + //Timer instance + rclcpp::TimerBase::SharedPtr timer_calculation; + rclcpp::TimerBase::SharedPtr startup_timer_; + //Subscriber instance + rclcpp::Subscription::SharedPtr subscriber_Odometry; + rclcpp::Subscription::SharedPtr subscriber_guidance; + //rclcpp::Subscription::SharedPtr subscriber_killswitch; + + //Variables for topics + + std::string topic_thrust; + std::string topic_guidance; + std::string topic_killswitch; + std::string topic_odometry; + + //Variables for timers + int publish_rate; + double max_force; + + //Stored wrenches values + vortex_msgs::msg::LOSGuidance reference_in; + Guidance_data guidance_values; + State current_state; + geometry_msgs::msg::WrenchStamped thrust_out; + + + int controller_type; //1 PID, 2 LQR, 3 NMPC, 4 NMPC acados + + //PID controllers + PID_controller PID_surge; + std::vector surge_params; + PID_controller PID_yaw; + std::vector yaw_params; + PID_controller PID_pitch; + std::vector pitch_params; + + + //LQR Controller + LQRController lqr_controller; + //LQRparameters lqr_parameters; + std::vector Q; + std::vector R; + //std::vector Qi; + //std::vector Ri; + std::vector inertia_matrix; + std::vector dampening_matrix_low; + std::vector dampening_matrix_high; + //NMPC controller + NMPC_controller NMPC; + //NMPC acados + AuvNMPC NMPC_acados; + std::vector Q2; + std::vector R2; + //NMPC parameters + std::vector Q3; + std::vector R3; + + std::atomic_bool should_exit_{false}; + //VC settings + bool reset_on_new_ref=true; + bool anti_overshoot=false; + bool auto_start=false; + bool odometry_dropout_guard=true; + int publish_counter=0; + bool first_start=true; + + //States + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + + void reset_controllers(int nr=0); + rclcpp::QoS pub_QoS; + rclcpp::QoS sub_QoS; + + +}; + + diff --git a/control/velocity_controller/launch/VCnTest.launch.py b/control/velocity_controller/launch/VCnTest.launch.py new file mode 100644 index 000000000..44ff3a8a4 --- /dev/null +++ b/control/velocity_controller/launch/VCnTest.launch.py @@ -0,0 +1,74 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch.actions import OpaqueFunction + +def launch_setup(context,*args,**kwargs): + drone, namespace=resolve_drone_and_namespace(context) + global_share = get_package_share_directory('auv_setup') + config_path_global = os.path.join(global_share,'config','robots',f'{drone}.yaml') + common_launch_args = { + "drone": drone, + "namespace": namespace, + }.items() + + stonefish_dir = get_package_share_directory('stonefish_sim') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'simulation.launch.py') + ), + launch_arguments={'rendering_quality': 'low','rendering':'true'}.items(), + ) + orca_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, 'launch', 'drone_sim.launch.py') + ) + ) + ] + ) + operation_mode_manager_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("operation_mode_manager"), + "launch", + "operation_mode_manager.launch.py", + ) + ), + launch_arguments=common_launch_args, + ) + + node_name_arg = DeclareLaunchArgument( + 'node_name_1', default_value='test_VC_node', + description='Name of the test VC node' + ) + test_VC_name = LaunchConfiguration('node_name_1') + + return [ + stonefish_sim, + orca_sim, + node_name_arg, + #operation_mode_manager_launch, + Node(package='velocity_controller', + executable='test_VC_node', + name=test_VC_name, + namespace=namespace, + output='screen', + parameters=[config_path_global]) + ] + +def generate_launch_description(): + return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/control/velocity_controller/launch/velocity_controller.launch.py b/control/velocity_controller/launch/velocity_controller.launch.py new file mode 100644 index 000000000..00ad9cb81 --- /dev/null +++ b/control/velocity_controller/launch/velocity_controller.launch.py @@ -0,0 +1,33 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +#from launch.actions import IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument +#from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) +from launch.actions import OpaqueFunction + + +def launch_setup(context,*args,**kwargs): + drone, namespace=resolve_drone_and_namespace(context) + + pkg_share = get_package_share_directory('velocity_controller') + global_share = get_package_share_directory('auv_setup') + config_path_local = os.path.join(pkg_share, 'config', f'{drone}_params.yaml') + config_path_global = os.path.join(global_share,'config','robots',f"{drone}.yaml") + + return [ + Node(package='velocity_controller', + executable='velocity_controller_node', + name="velocity_controller_node", + namespace=namespace, + output='screen', + parameters=[config_path_local,config_path_global]) + ] +def generate_launch_description(): + return LaunchDescription(declare_drone_and_namespace_args()+[OpaqueFunction(function=launch_setup)]) \ No newline at end of file diff --git a/control/velocity_controller/package.xml b/control/velocity_controller/package.xml new file mode 100644 index 000000000..f138d6dbe --- /dev/null +++ b/control/velocity_controller/package.xml @@ -0,0 +1,31 @@ + + + + velocity_controller + 0.0.0 + TODO: Package description + henrik + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + vortex_msgs + geometry_msgs + nav_msgs + ct_optcon + ct_core + vortex_utils + rclcpp_lifecycle + lifecycle_msgs + auv_setup + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/control/velocity_controller/scripts/auv_ocp.json b/control/velocity_controller/scripts/auv_ocp.json new file mode 100644 index 000000000..1becb5b1f --- /dev/null +++ b/control/velocity_controller/scripts/auv_ocp.json @@ -0,0 +1,1145 @@ +{ + "code_gen_opts": { + "acados_include_path": "/home/henrik/ros2_ws_v/src/acados/include", + "acados_lib_path": "/home/henrik/ros2_ws_v/src/acados/lib", + "acados_link_libs": { + "clarabel": "", + "daqp": "", + "hpmpc": "", + "ooqp": "", + "openmp": "-fopenmp", + "osqp": "-losqp", + "qpdunes": "", + "qpoases": "-lqpOASES_e" + }, + "acados_version": "508482dac", + "code_export_directory": "/home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver", + "cython_include_dirs": [ + "/usr/lib/python3/dist-packages/numpy/core/include", + "/usr/include/python3.10" + ], + "json_file": "auv_ocp.json", + "os": "unix", + "shared_lib_ext": ".so" + }, + "constraints": { + "C": [], + "C_e": [], + "D": [], + "constr_type": "BGH", + "constr_type_0": "BGH", + "constr_type_e": "BGH", + "constr_types": [ + "BGH", + "BGP" + ], + "has_x0": true, + "idxbu": [ + 0, + 1, + 2 + ], + "idxbx": [ + 7 + ], + "idxbx_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8 + ], + "idxbx_e": [], + "idxbxe_0": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8 + ], + "idxs_rev": [], + "idxs_rev_0": [], + "idxs_rev_e": [], + "idxsbu": [], + "idxsbx": [], + "idxsbx_e": [], + "idxsg": [], + "idxsg_e": [], + "idxsh": [], + "idxsh_0": [], + "idxsh_e": [], + "idxsphi": [], + "idxsphi_0": [], + "idxsphi_e": [], + "lbu": [ + -99.5, + -99.5, + -99.5 + ], + "lbx": [ + -1.4 + ], + "lbx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "lbx_e": [], + "lg": [], + "lg_e": [], + "lh": [], + "lh_0": [], + "lh_e": [], + "lphi": [], + "lphi_0": [], + "lphi_e": [], + "ls": [], + "ls_0": [], + "ls_e": [], + "lsbu": [], + "lsbx": [], + "lsbx_e": [], + "lsg": [], + "lsg_e": [], + "lsh": [], + "lsh_0": [], + "lsh_e": [], + "lsphi": [], + "lsphi_0": [], + "lsphi_e": [], + "ubu": [ + 99.5, + 99.5, + 99.5 + ], + "ubx": [ + 1.4 + ], + "ubx_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "ubx_e": [], + "ug": [], + "ug_e": [], + "uh": [], + "uh_0": [], + "uh_e": [], + "uphi": [], + "uphi_0": [], + "uphi_e": [], + "us": [], + "us_0": [], + "us_e": [], + "usbu": [], + "usbx": [], + "usbx_e": [], + "usg": [], + "usg_e": [], + "ush": [], + "ush_0": [], + "ush_e": [], + "usphi": [], + "usphi_0": [], + "usphi_e": [] + }, + "cost": { + "Vu": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "Vu_0": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 1.0, + 0.0, + 0.0 + ], + [ + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 1.0 + ] + ], + "Vx": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Vx_0": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "Vx_e": [ + [ + 1.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "Vz": [], + "Vz_0": [], + "W": [ + [ + 100.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1 + ] + ], + "W_0": [ + [ + 100.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.1 + ] + ], + "W_e": [ + [ + 100.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0, + 0.0, + 1.0 + ] + ], + "Zl": [], + "Zl_0": [], + "Zl_e": [], + "Zu": [], + "Zu_0": [], + "Zu_e": [], + "cost_ext_fun_type": "casadi", + "cost_ext_fun_type_0": "casadi", + "cost_ext_fun_type_e": "casadi", + "cost_ext_fun_types": [ + "casadi", + "generic" + ], + "cost_function_ext_cost": null, + "cost_function_ext_cost_0": null, + "cost_function_ext_cost_e": null, + "cost_source_ext_cost": null, + "cost_source_ext_cost_0": null, + "cost_source_ext_cost_e": null, + "cost_type": "LINEAR_LS", + "cost_type_0": "LINEAR_LS", + "cost_type_e": "LINEAR_LS", + "cost_types": [ + "LINEAR_LS", + "NONLINEAR_LS", + "EXTERNAL", + "CONVEX_OVER_NONLINEAR", + "AUTO" + ], + "yref": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "yref_0": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "yref_e": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "zl": [], + "zl_0": [], + "zl_e": [], + "zu": [], + "zu_0": [], + "zu_e": [] + }, + "dims": { + "N": 20, + "n_global_data": 0, + "nbu": 3, + "nbx": 1, + "nbx_0": 9, + "nbx_e": 0, + "nbxe_0": 9, + "ng": 0, + "ng_e": 0, + "nh": 0, + "nh_0": 0, + "nh_e": 0, + "np": 0, + "np_global": 0, + "nphi": 0, + "nphi_0": 0, + "nphi_e": 0, + "nr": 0, + "nr_0": 0, + "nr_e": 0, + "ns": 0, + "ns_0": 0, + "ns_e": 0, + "nsbu": 0, + "nsbx": 0, + "nsbx_e": 0, + "nsg": 0, + "nsg_e": 0, + "nsh": 0, + "nsh_0": 0, + "nsh_e": 0, + "nsphi": 0, + "nsphi_0": 0, + "nsphi_e": 0, + "nu": 3, + "nx": 9, + "nx_next": 9, + "ny": 8, + "ny_0": 8, + "ny_e": 5, + "nz": 0 + }, + "external_function_files_model": [ + "auv_model_model/auv_model_impl_dae_fun.c", + "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c", + "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c", + "auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c", + "auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c" + ], + "external_function_files_ocp": [], + "hash": "78dcc9b0fd627d3908081bfa461a62e0", + "model": { + "con_h_expr": [], + "con_h_expr_0": [], + "con_h_expr_e": [], + "con_phi_expr": [], + "con_phi_expr_0": [], + "con_phi_expr_e": [], + "con_r_expr": [], + "con_r_expr_0": [], + "con_r_expr_e": [], + "con_r_in_phi": [], + "con_r_in_phi_0": [], + "con_r_in_phi_e": [], + "cost_conl_custom_outer_hess": [], + "cost_conl_custom_outer_hess_0": [], + "cost_conl_custom_outer_hess_e": [], + "cost_expr_ext_cost": [], + "cost_expr_ext_cost_0": [], + "cost_expr_ext_cost_custom_hess": [], + "cost_expr_ext_cost_custom_hess_0": [], + "cost_expr_ext_cost_custom_hess_e": [], + "cost_expr_ext_cost_e": [], + "cost_psi_expr": [], + "cost_psi_expr_0": [], + "cost_psi_expr_e": [], + "cost_r_in_psi_expr": [], + "cost_r_in_psi_expr_0": [], + "cost_r_in_psi_expr_e": [], + "cost_y_expr": [], + "cost_y_expr_0": [], + "cost_y_expr_e": [], + "disc_dyn_custom_hess_ux_expr": [], + "disc_dyn_custom_jac_ux_expr": [], + "disc_dyn_expr": [], + "dyn_disc_fun": null, + "dyn_disc_fun_jac": null, + "dyn_disc_fun_jac_hess": null, + "dyn_ext_fun_type": "casadi", + "dyn_generic_source": null, + "dyn_impl_dae_fun": null, + "dyn_impl_dae_fun_jac": null, + "dyn_impl_dae_jac": null, + "expression_names": [ + "f_expl_expr", + "f_impl_expr", + "p", + "p_global", + "pi", + "u", + "x", + "xdot", + "z" + ], + "f_expl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9)), ((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9)), ((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9)), ((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9)), ((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9)), ((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9)), ((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r)), ((cos(phi)*q)-(sin(phi)*r)), (((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r))])", + "f_impl_expr": "SX(@1=-30, @2=30, @3=((Fx-(((@1*r)*v)+((@2*q)*w)))-((23+fabs(u))*u)), @4=46, @5=((((@2*r)*u)+((@1*p)*w))+((@4+fabs(v))*v)), @6=((((@1*q)*u)+((@2*p)*v))+((@4+fabs(w))*w)), @7=((((3.34*r)*q)+((-3.32*q)*r))+((@4+fabs(p))*p)), @8=((My-(((-3.34*r)*p)+((0.68*p)*r)))-((@4+fabs(q))*q)), @9=((Mz-(((3.32*q)*p)+((-0.68*p)*q)))-((@4+fabs(r))*r)), @10=-0.000546005, [(xdot_0-((((((0.0334536*@3)-(6.0369e-05*@5))-(-1.11163e-07*@6))-(9.80847e-08*@7))+(1.09201e-05*@8))+(-0.00601506*@9))), (xdot_1-((((((6.0369e-05*@3)-(0.0334847*@5))-(-6.16582e-05*@6))-(5.44043e-05*@7))+(0.00605702*@8))+(-0.00301845*@9))), (xdot_2-((((((-1.11163e-07*@3)-(-6.16582e-05*@5))-(0.0339635*@6))-(-0.0299678*@7))+(-0.00308013*@8))+(5.55814e-06*@9))), (xdot_3-((((((9.80847e-08*@3)-(5.44043e-05*@5))-(-0.0299678*@6))-(1.49703*@7))+(0.00271776*@8))+(-4.90424e-06*@9))), (xdot_4-((((((1.09201e-05*@3)-(0.00605702*@5))-(-0.00308013*@6))-(0.00271776*@7))+(0.302578*@8))+(@10*@9))), (xdot_5-((((((-0.00601506*@3)-(-0.00301845*@5))-(5.55814e-06*@6))-(-4.90424e-06*@7))+(@10*@8))+(0.300753*@9))), (xdot_6-((p+((sin(phi)*tan(theta))*q))+((cos(phi)*tan(theta))*r))), (xdot_7-((cos(phi)*q)-(sin(phi)*r))), (xdot_8-(((sin(phi)/cos(theta))*q)+((cos(phi)/cos(theta))*r)))])", + "gnsf_model": null, + "name": "auv_model", + "nu_original": null, + "p": "SX(0x1)", + "p_global": "SX(0x1)", + "pi": "SX([pi_0, pi_1, pi_2, pi_3, pi_4, pi_5, pi_6, pi_7, pi_8])", + "serialized_expressions": "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", + "t": [], + "t0": null, + "t_label": "t", + "u": "SX([Fx, My, Mz])", + "u_labels": [ + "u0", + "u1", + "u2" + ], + "x": "SX([u, v, w, p, q, r, phi, theta, psi])", + "x_labels": [ + "x0", + "x1", + "x2", + "x3", + "x4", + "x5", + "x6", + "x7", + "x8" + ], + "xdot": "SX([xdot_0, xdot_1, xdot_2, xdot_3, xdot_4, xdot_5, xdot_6, xdot_7, xdot_8])", + "z": "SX(0x1)" + }, + "name": "auv_model", + "p_global_values": [], + "parameter_values": [], + "problem_class": "OCP", + "ros_opts": null, + "simulink_opts": null, + "solver_options": { + "N_horizon": 20, + "Tsim": 0.1, + "adaptive_levenberg_marquardt_lam": 5.0, + "adaptive_levenberg_marquardt_mu0": 0.001, + "adaptive_levenberg_marquardt_mu_min": 1e-16, + "adaptive_levenberg_marquardt_obj_scalar": 2.0, + "allow_direction_mode_switch_to_nominal": true, + "anderson_activation_threshold": 10.0, + "as_rti_iter": 1, + "as_rti_level": 4, + "byrd_omojokon_slack_relaxation_factor": 1.00001, + "collocation_type": "GAUSS_LEGENDRE", + "cost_discretization": "EULER", + "cost_scaling": [ + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 1.0 + ], + "custom_templates": [], + "custom_update_copy": true, + "custom_update_filename": "", + "custom_update_header_filename": "", + "eval_residual_at_max_iter": false, + "exact_hess_constr": 1, + "exact_hess_cost": 1, + "exact_hess_dyn": 1, + "ext_cost_num_hess": 0, + "ext_fun_compile_flags": "-O2", + "ext_fun_expand_constr": false, + "ext_fun_expand_cost": false, + "ext_fun_expand_dyn": false, + "ext_fun_expand_precompute": false, + "fixed_hess": 0, + "globalization": "FIXED_STEP", + "globalization_alpha_min": 0.05, + "globalization_alpha_reduction": 0.7, + "globalization_eps_sufficient_descent": 0.0001, + "globalization_fixed_step_length": 1.0, + "globalization_full_step_dual": 0, + "globalization_funnel_fraction_switching_condition": 0.001, + "globalization_funnel_init_increase_factor": 15.0, + "globalization_funnel_init_upper_bound": 1.0, + "globalization_funnel_initial_penalty_parameter": 1.0, + "globalization_funnel_kappa": 0.9, + "globalization_funnel_sufficient_decrease_factor": 0.9, + "globalization_funnel_use_merit_fun_only": false, + "globalization_line_search_use_sufficient_descent": 0, + "globalization_use_SOC": 0, + "hessian_approx": "GAUSS_NEWTON", + "hpipm_mode": "BALANCE", + "integrator_type": "IRK", + "levenberg_marquardt": 0.01, + "log_dual_step_norm": false, + "log_primal_step_norm": false, + "model_external_shared_lib_dir": null, + "model_external_shared_lib_name": null, + "nlp_qp_tol_min_comp": 1e-11, + "nlp_qp_tol_min_eq": 1e-10, + "nlp_qp_tol_min_ineq": 1e-10, + "nlp_qp_tol_min_stat": 1e-09, + "nlp_qp_tol_reduction_factor": 0.1, + "nlp_qp_tol_safety_factor": 0.1, + "nlp_qp_tol_strategy": "FIXED_QP_TOL", + "nlp_solver_ext_qp_res": 0, + "nlp_solver_max_iter": 100, + "nlp_solver_tol_comp": 1e-06, + "nlp_solver_tol_eq": 1e-06, + "nlp_solver_tol_ineq": 1e-06, + "nlp_solver_tol_min_step_norm": 0.0, + "nlp_solver_tol_stat": 1e-06, + "nlp_solver_type": "SQP", + "nlp_solver_warm_start_first_qp": false, + "nlp_solver_warm_start_first_qp_from_nlp": false, + "print_level": 2, + "qp_solver": "FULL_CONDENSING_HPIPM", + "qp_solver_cond_N": 20, + "qp_solver_cond_block_size": null, + "qp_solver_cond_ric_alg": 1, + "qp_solver_iter_max": 50, + "qp_solver_mu0": 0.0, + "qp_solver_ric_alg": 1, + "qp_solver_t0_init": 2, + "qp_solver_tol_comp": null, + "qp_solver_tol_eq": null, + "qp_solver_tol_ineq": null, + "qp_solver_tol_stat": null, + "qp_solver_warm_start": 1, + "qpscaling_lb_norm_inf_grad_obj": 0.0001, + "qpscaling_scale_constraints": "NO_CONSTRAINT_SCALING", + "qpscaling_scale_objective": "NO_OBJECTIVE_SCALING", + "qpscaling_ub_max_abs_eig": 100000.0, + "reg_adaptive_eps": false, + "reg_epsilon": 0.0001, + "reg_max_cond_block": 10000000.0, + "reg_min_epsilon": 1e-08, + "regularize_method": "NO_REGULARIZE", + "rti_log_only_available_residuals": 0, + "rti_log_residuals": 0, + "search_direction_mode": "NOMINAL_QP", + "sens_forw_p": false, + "shooting_nodes": [ + 0.0, + 0.1, + 0.2, + 0.30000000000000004, + 0.4, + 0.5, + 0.6, + 0.7, + 0.7999999999999999, + 0.8999999999999999, + 0.9999999999999999, + 1.0999999999999999, + 1.2, + 1.3, + 1.4000000000000001, + 1.5000000000000002, + 1.6000000000000003, + 1.7000000000000004, + 1.8000000000000005, + 1.9000000000000006, + 2.0000000000000004 + ], + "sim_method_jac_reuse": [ + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0, + 0 + ], + "sim_method_newton_iter": 3, + "sim_method_newton_tol": 0.0, + "sim_method_num_stages": [ + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2, + 2 + ], + "sim_method_num_steps": [ + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4, + 4 + ], + "solution_sens_qp_t_lam_min": 1e-09, + "store_iterates": false, + "tau_min": 0.0, + "tf": 2.0, + "time_steps": [ + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1, + 0.1 + ], + "timeout_heuristic": "LAST", + "timeout_max_time": 0.0, + "use_constraint_hessian_in_feas_qp": false, + "with_adaptive_levenberg_marquardt": false, + "with_anderson_acceleration": false, + "with_batch_functionality": false, + "with_solution_sens_wrt_params": false, + "with_value_sens_wrt_params": false + }, + "zoro_description": null +} \ No newline at end of file diff --git a/control/velocity_controller/scripts/build_auv_solver/Makefile b/control/velocity_controller/scripts/build_auv_solver/Makefile new file mode 100644 index 000000000..7e20d181d --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/Makefile @@ -0,0 +1,208 @@ +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + + + + + +# define sources and use make's implicit rules to generate object files (*.o) + +# model +MODEL_SRC= + +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c +MODEL_SRC+= auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c + +MODEL_OBJ := $(MODEL_SRC:.c=.o) +# optimal control problem - mostly CasADi exports +OCP_SRC= + + + +OCP_SRC+= acados_solver_auv_model.c + +OCP_OBJ := $(OCP_SRC:.c=.o) +# for sim solver +SIM_SRC= acados_sim_solver_auv_model.c +SIM_OBJ := $(SIM_SRC:.c=.o) + +# for target example_sim +EX_SIM_SRC= main_sim_auv_model.c +EX_SIM_OBJ := $(EX_SIM_SRC:.c=.o) +EX_SIM_EXE := $(EX_SIM_SRC:.c=) +# for target example +EX_SRC= main_auv_model.c +EX_OBJ := $(EX_SRC:.c=.o) +EX_EXE := $(EX_SRC:.c=) + + +# combine model, (potentially) sim and ocp object files +OBJ= +OBJ+= $(MODEL_OBJ) +OBJ+= $(SIM_OBJ) +OBJ+= $(OCP_OBJ) + +EXTERNAL_DIR= +EXTERNAL_LIB= + +INCLUDE_PATH = /home/henrik/ros2_ws_v/src/acados/include +LIB_PATH = /home/henrik/ros2_ws_v/src/acados/lib + +# preprocessor flags for make's implicit rules +CPPFLAGS+= -I$(INCLUDE_PATH) +CPPFLAGS+= -I$(INCLUDE_PATH)/acados +CPPFLAGS+= -I$(INCLUDE_PATH)/blasfeo/include +CPPFLAGS+= -I$(INCLUDE_PATH)/hpipm/include + + +# define the c-compiler flags for make's implicit rules +CFLAGS = -fPIC -std=c99 -O2 + +# # Debugging +# CFLAGS += -g3 -fno-diagnostics-show-line-numbers -g + +# linker flags +LDFLAGS+= -L$(LIB_PATH) + + +# link to libraries +LDLIBS+= -lacados +LDLIBS+= -lhpipm +LDLIBS+= -lblasfeo +LDLIBS+= -lm +LDLIBS+= + +# libraries +LIBACADOS_SOLVER=libacados_solver_auv_model.so +LIBACADOS_OCP_SOLVER=libacados_ocp_solver_auv_model.so +LIBACADOS_SIM_SOLVER=lib$(SIM_SRC:.c=.so) + +# virtual targets +.PHONY : all clean + + all: clean example_sim example +shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib + +# some linker targets +example: $(EX_OBJ) $(OBJ) + $(CC) $^ -o $(EX_EXE) $(LDFLAGS) $(LDLIBS) + +example_sim: $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) + $(CC) $^ -o $(EX_SIM_EXE) $(LDFLAGS) $(LDLIBS) + +sim_shared_lib: $(SIM_OBJ) $(MODEL_OBJ) + $(CC) -shared $^ -o $(LIBACADOS_SIM_SOLVER) $(LDFLAGS) $(LDLIBS) +bundled_shared_lib: $(OBJ) + $(CC) -shared $^ -o $(LIBACADOS_SOLVER) $(LDFLAGS) $(LDLIBS) + +ocp_shared_lib: $(OCP_OBJ) $(MODEL_OBJ) + $(CC) -shared $^ -o $(LIBACADOS_OCP_SOLVER) $(LDFLAGS) $(LDLIBS) \ + -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) + + +# Cython targets +ocp_cython_c: ocp_shared_lib + cython \ + -o acados_ocp_solver_pyx.c \ + -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ + $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \ + -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ + +ocp_cython_o: ocp_cython_c + $(CC) $(ACADOS_FLAGS) -c -O2 \ + -fPIC \ + -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ + -o acados_ocp_solver_pyx.o \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -I /usr/lib/python3/dist-packages/numpy/core/include \ + -I /usr/include/python3.10 \ + acados_ocp_solver_pyx.c \ + +ocp_cython: ocp_cython_o + $(CC) $(ACADOS_FLAGS) -shared \ + -o acados_ocp_solver_pyx.so \ + -Wl,-rpath=$(LIB_PATH) \ + acados_ocp_solver_pyx.o \ + $(abspath .)/libacados_ocp_solver_auv_model.so \ + $(LDFLAGS) $(LDLIBS) + + +# Sim Cython targets +sim_cython_c: sim_shared_lib + cython \ + -o acados_sim_solver_pyx.c \ + -I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \ + $(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_sim_solver_pyx.pyx \ + -I /home/henrik/ros2_ws_v/src/vortex-auv/control/velocity_controller/scripts/build_auv_solver \ + +sim_cython_o: sim_cython_c + $(CC) $(ACADOS_FLAGS) -c -O2 \ + -fPIC \ + -DNPY_NO_DEPRECATED_API=NPY_1_7_API_VERSION \ + -o acados_sim_solver_pyx.o \ + -I $(INCLUDE_PATH)/blasfeo/include/ \ + -I $(INCLUDE_PATH)/hpipm/include/ \ + -I $(INCLUDE_PATH) \ + -I /usr/lib/python3/dist-packages/numpy/core/include \ + -I /usr/include/python3.10 \ + acados_sim_solver_pyx.c \ + +sim_cython: sim_cython_o + $(CC) $(ACADOS_FLAGS) -shared \ + -o acados_sim_solver_pyx.so \ + -Wl,-rpath=$(LIB_PATH) \ + acados_sim_solver_pyx.o \ + $(abspath .)/libacados_sim_solver_auv_model.so \ + $(LDFLAGS) $(LDLIBS) + +clean: + $(RM) $(OBJ) $(EX_OBJ) $(EX_SIM_OBJ) + $(RM) $(LIBACADOS_SOLVER) $(LIBACADOS_OCP_SOLVER) $(LIBACADOS_SIM_SOLVER) + $(RM) $(EX_EXE) $(EX_SIM_EXE) +clean_ocp_shared_lib: + $(RM) $(LIBACADOS_OCP_SOLVER) + $(RM) $(OCP_OBJ) + +clean_ocp_cython: + $(RM) libacados_ocp_solver_auv_model.so + $(RM) acados_solver_auv_model.o + $(RM) acados_ocp_solver_pyx.so + $(RM) acados_ocp_solver_pyx.o + +clean_sim_cython: + $(RM) libacados_sim_solver_auv_model.so + $(RM) acados_sim_solver_auv_model.o + $(RM) acados_sim_solver_pyx.so + $(RM) acados_sim_solver_pyx.o diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c new file mode 100644 index 000000000..3da4f7b13 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.c @@ -0,0 +1,310 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ +// standard +#include +#include + +// acados +#include "acados_c/external_function_interface.h" +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/print.h" + + +// example specific +#include "auv_model_model/auv_model_model.h" +#include "acados_sim_solver_auv_model.h" + + +// ** solver data ** + +auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule() +{ + void* capsule_mem = malloc(sizeof(auv_model_sim_solver_capsule)); + auv_model_sim_solver_capsule *capsule = (auv_model_sim_solver_capsule *) capsule_mem; + + return capsule; +} + + +int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule * capsule) +{ + free(capsule); + return 0; +} + + +int auv_model_acados_sim_create(auv_model_sim_solver_capsule * capsule) +{ + // initialize + const int nx = AUV_MODEL_NX; + const int nu = AUV_MODEL_NU; + const int nz = AUV_MODEL_NZ; + const int np = AUV_MODEL_NP; + bool tmp_bool; + + double Tsim = 0.1; + + capsule->acados_sim_mem = NULL; + + external_function_opts ext_fun_opts; + external_function_opts_set_to_default(&ext_fun_opts); + ext_fun_opts.external_workspace = false; + + + capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)); + + + capsule->sim_impl_dae_jac_p = NULL; + + // external functions (implicit model) + capsule->sim_impl_dae_fun->casadi_fun = &auv_model_impl_dae_fun; + capsule->sim_impl_dae_fun->casadi_work = &auv_model_impl_dae_fun_work; + capsule->sim_impl_dae_fun->casadi_sparsity_in = &auv_model_impl_dae_fun_sparsity_in; + capsule->sim_impl_dae_fun->casadi_sparsity_out = &auv_model_impl_dae_fun_sparsity_out; + capsule->sim_impl_dae_fun->casadi_n_in = &auv_model_impl_dae_fun_n_in; + capsule->sim_impl_dae_fun->casadi_n_out = &auv_model_impl_dae_fun_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_fun, np, &ext_fun_opts); + + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &auv_model_impl_dae_fun_jac_x_xdot_z; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &auv_model_impl_dae_fun_jac_x_xdot_z_work; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &auv_model_impl_dae_fun_jac_x_xdot_z_n_in; + capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &auv_model_impl_dae_fun_jac_x_xdot_z_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np, &ext_fun_opts); + + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &auv_model_impl_dae_jac_x_xdot_u_z; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &auv_model_impl_dae_jac_x_xdot_u_z_work; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &auv_model_impl_dae_jac_x_xdot_u_z_n_in; + capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &auv_model_impl_dae_jac_x_xdot_u_z_n_out; + external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np, &ext_fun_opts); + + + + + + // sim plan & config + sim_solver_plan_t plan; + plan.sim_solver = IRK; + + // create correct config based on plan + sim_config * auv_model_sim_config = sim_config_create(plan); + capsule->acados_sim_config = auv_model_sim_config; + + // sim dims + void *auv_model_sim_dims = sim_dims_create(auv_model_sim_config); + capsule->acados_sim_dims = auv_model_sim_dims; + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nx", &nx); + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nu", &nu); + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "nz", &nz); + sim_dims_set(auv_model_sim_config, auv_model_sim_dims, "np", &np); + + + // sim opts + sim_opts *auv_model_sim_opts = sim_opts_create(auv_model_sim_config, auv_model_sim_dims); + capsule->acados_sim_opts = auv_model_sim_opts; + int tmp_int = 3; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_iter", &tmp_int); + double tmp_double = 0; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "newton_tol", &tmp_double); + sim_collocation_type collocation_type = GAUSS_LEGENDRE; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "collocation_type", &collocation_type); + + + tmp_int = 2; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_stages", &tmp_int); + tmp_int = 4; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "num_steps", &tmp_int); + tmp_bool = 0; + sim_opts_set(auv_model_sim_config, auv_model_sim_opts, "jac_reuse", &tmp_bool); + + + // sim in / out + sim_in *auv_model_sim_in = sim_in_create(auv_model_sim_config, auv_model_sim_dims); + capsule->acados_sim_in = auv_model_sim_in; + sim_out *auv_model_sim_out = sim_out_create(auv_model_sim_config, auv_model_sim_dims); + capsule->acados_sim_out = auv_model_sim_out; + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "T", &Tsim); + + // model functions + auv_model_sim_config->model_set(auv_model_sim_in->model, + "impl_ode_fun", capsule->sim_impl_dae_fun); + auv_model_sim_config->model_set(auv_model_sim_in->model, + "impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z); + auv_model_sim_config->model_set(auv_model_sim_in->model, + "impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z); + + + // sim solver + sim_solver *auv_model_sim_solver = sim_solver_create(auv_model_sim_config, + auv_model_sim_dims, auv_model_sim_opts, auv_model_sim_in); + capsule->acados_sim_solver = auv_model_sim_solver; + + capsule->acados_sim_mem = auv_model_sim_solver->mem; + + + + /* initialize input */ + // x + double x0[9]; + for (int ii = 0; ii < 9; ii++) + x0[ii] = 0.0; + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "x", x0); + + + // u + double u0[3]; + for (int ii = 0; ii < 3; ii++) + u0[ii] = 0.0; + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "u", u0); + + // S_forw + double S_forw[108]; + for (int ii = 0; ii < 108; ii++) + S_forw[ii] = 0.0; + for (int ii = 0; ii < 9; ii++) + S_forw[ii + ii * 9 ] = 1.0; + + + sim_in_set(auv_model_sim_config, auv_model_sim_dims, + auv_model_sim_in, "S_forw", S_forw); + + int status = sim_precompute(auv_model_sim_solver, auv_model_sim_in, auv_model_sim_out); + + return status; +} + + +int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule) +{ + // integrate dynamics using acados sim_solver + int status = sim_solve(capsule->acados_sim_solver, + capsule->acados_sim_in, capsule->acados_sim_out); + if (status != 0) + printf("error in auv_model_acados_sim_solve()! Exiting.\n"); + + return status; +} + + + + +int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule) +{ + // free memory + sim_solver_destroy(capsule->acados_sim_solver); + sim_in_destroy(capsule->acados_sim_in); + sim_out_destroy(capsule->acados_sim_out); + sim_opts_destroy(capsule->acados_sim_opts); + sim_dims_destroy(capsule->acados_sim_dims); + sim_config_destroy(capsule->acados_sim_config); + + // free external function + external_function_param_casadi_free(capsule->sim_impl_dae_fun); + external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z); + external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z); + + free(capsule->sim_impl_dae_fun); + free(capsule->sim_impl_dae_fun_jac_x_xdot_z); + free(capsule->sim_impl_dae_jac_x_xdot_u_z); + + + return 0; +} + + +int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *p, int np) +{ + int status = 0; + int casadi_np = AUV_MODEL_NP; + + if (casadi_np != np) { + printf("auv_model_acados_sim_update_params: trying to set %i parameters for external functions." + " External function has %i parameters. Exiting.\n", np, casadi_np); + exit(1); + } + capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p); + capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p); + capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p); + + + return status; +} + +/* getters pointers to C objects*/ +sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_config; +}; + +sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_in; +}; + +sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_out; +}; + +void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_dims; +}; + +sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_opts; +}; + +sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_solver; +}; + +void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule) +{ + return capsule->acados_sim_mem; +}; + diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h new file mode 100644 index 000000000..beed43e59 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/acados_sim_solver_auv_model.h @@ -0,0 +1,105 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SIM_auv_model_H_ +#define ACADOS_SIM_auv_model_H_ + +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#define AUV_MODEL_NX 9 +#define AUV_MODEL_NZ 0 +#define AUV_MODEL_NU 3 +#define AUV_MODEL_NP 0 + +#ifdef __cplusplus +extern "C" { +#endif + + +// ** capsule for solver data ** +typedef struct auv_model_sim_solver_capsule +{ + // acados objects + sim_in *acados_sim_in; + sim_out *acados_sim_out; + sim_solver *acados_sim_solver; + sim_opts *acados_sim_opts; + sim_config *acados_sim_config; + void *acados_sim_dims; + void *acados_sim_mem; + + /* external functions */ + // ERK + external_function_param_casadi * sim_expl_vde_forw; + external_function_param_casadi * sim_vde_adj_casadi; + external_function_param_casadi * sim_expl_ode_fun_casadi; + external_function_param_casadi * sim_expl_ode_hess; + external_function_param_casadi * sim_expl_vde_forw_p; + + // IRK + external_function_param_casadi * sim_impl_dae_fun; + external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z; + external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z; + external_function_param_casadi * sim_impl_dae_hess; + external_function_param_casadi * sim_impl_dae_jac_p; + + // GNSF + external_function_param_casadi * sim_gnsf_phi_fun; + external_function_param_casadi * sim_gnsf_phi_fun_jac_y; + external_function_param_casadi * sim_gnsf_phi_jac_y_uhat; + external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z; + external_function_param_casadi * sim_gnsf_get_matrices_fun; + +} auv_model_sim_solver_capsule; + + +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_create(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solve(auv_model_sim_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_free(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_update_params(auv_model_sim_solver_capsule *capsule, double *value, int np); + +ACADOS_SYMBOL_EXPORT sim_config * auv_model_acados_get_sim_config(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_in * auv_model_acados_get_sim_in(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_out * auv_model_acados_get_sim_out(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_dims(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_opts * auv_model_acados_get_sim_opts(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_solver * auv_model_acados_get_sim_solver(auv_model_sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT void * auv_model_acados_get_sim_mem(auv_model_sim_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT auv_model_sim_solver_capsule * auv_model_acados_sim_solver_create_capsule(void); +ACADOS_SYMBOL_EXPORT int auv_model_acados_sim_solver_free_capsule(auv_model_sim_solver_capsule *capsule); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_SIM_auv_model_H_ diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd b/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd new file mode 100644 index 000000000..a6a039341 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/acados_solver.pxd @@ -0,0 +1,63 @@ +# +# Copyright (c) The acados authors. +# +# This file is part of acados. +# +# The 2-Clause BSD License +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE.; +# + +cimport acados_solver_common + +cdef extern from "acados_solver_auv_model.h": + ctypedef struct nlp_solver_capsule "auv_model_solver_capsule": + pass + + nlp_solver_capsule * acados_create_capsule "auv_model_acados_create_capsule"() + int acados_free_capsule "auv_model_acados_free_capsule"(nlp_solver_capsule *capsule) + + int acados_create "auv_model_acados_create"(nlp_solver_capsule * capsule) + + int acados_create_with_discretization "auv_model_acados_create_with_discretization"(nlp_solver_capsule * capsule, int n_time_steps, double* new_time_steps) + int acados_update_time_steps "auv_model_acados_update_time_steps"(nlp_solver_capsule * capsule, int N, double* new_time_steps) + int acados_update_qp_solver_cond_N "auv_model_acados_update_qp_solver_cond_N"(nlp_solver_capsule * capsule, int qp_solver_cond_N) + + int acados_update_params "auv_model_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_) + int acados_update_params_sparse "auv_model_acados_update_params_sparse"(nlp_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) + int acados_set_p_global_and_precompute_dependencies "auv_model_acados_set_p_global_and_precompute_dependencies"(nlp_solver_capsule * capsule, double *value, int data_len) + int acados_solve "auv_model_acados_solve"(nlp_solver_capsule * capsule) + int acados_reset "auv_model_acados_reset"(nlp_solver_capsule * capsule, int reset_qp_solver_mem) + int acados_free "auv_model_acados_free"(nlp_solver_capsule * capsule) + void acados_print_stats "auv_model_acados_print_stats"(nlp_solver_capsule * capsule) + + int acados_custom_update "auv_model_acados_custom_update"(nlp_solver_capsule* capsule, double * data, int data_len) + + acados_solver_common.ocp_nlp_in *acados_get_nlp_in "auv_model_acados_get_nlp_in"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_out *acados_get_nlp_out "auv_model_acados_get_nlp_out"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_out *acados_get_sens_out "auv_model_acados_get_sens_out"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "auv_model_acados_get_nlp_solver"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_config *acados_get_nlp_config "auv_model_acados_get_nlp_config"(nlp_solver_capsule * capsule) + void *acados_get_nlp_opts "auv_model_acados_get_nlp_opts"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "auv_model_acados_get_nlp_dims"(nlp_solver_capsule * capsule) + acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "auv_model_acados_get_nlp_plan"(nlp_solver_capsule * capsule) diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c new file mode 100644 index 000000000..6fbe2da06 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.c @@ -0,0 +1,1187 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +// standard +#include +#include +#include +// acados +// #include "acados/utils/print.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +// example specific + +#include "auv_model_model/auv_model_model.h" + + + + + +#include "acados_solver_auv_model.h" + +#define NX AUV_MODEL_NX +#define NZ AUV_MODEL_NZ +#define NU AUV_MODEL_NU +#define NP AUV_MODEL_NP +#define NP_GLOBAL AUV_MODEL_NP_GLOBAL +#define NY0 AUV_MODEL_NY0 +#define NY AUV_MODEL_NY +#define NYN AUV_MODEL_NYN + +#define NBX AUV_MODEL_NBX +#define NBX0 AUV_MODEL_NBX0 +#define NBU AUV_MODEL_NBU +#define NG AUV_MODEL_NG +#define NBXN AUV_MODEL_NBXN +#define NGN AUV_MODEL_NGN + +#define NH AUV_MODEL_NH +#define NHN AUV_MODEL_NHN +#define NH0 AUV_MODEL_NH0 +#define NPHI AUV_MODEL_NPHI +#define NPHIN AUV_MODEL_NPHIN +#define NPHI0 AUV_MODEL_NPHI0 +#define NR AUV_MODEL_NR + +#define NS AUV_MODEL_NS +#define NS0 AUV_MODEL_NS0 +#define NSN AUV_MODEL_NSN + +#define NSBX AUV_MODEL_NSBX +#define NSBU AUV_MODEL_NSBU +#define NSH0 AUV_MODEL_NSH0 +#define NSH AUV_MODEL_NSH +#define NSHN AUV_MODEL_NSHN +#define NSG AUV_MODEL_NSG +#define NSPHI0 AUV_MODEL_NSPHI0 +#define NSPHI AUV_MODEL_NSPHI +#define NSPHIN AUV_MODEL_NSPHIN +#define NSGN AUV_MODEL_NSGN +#define NSBXN AUV_MODEL_NSBXN + + + +// ** solver data ** + +auv_model_solver_capsule * auv_model_acados_create_capsule(void) +{ + void* capsule_mem = malloc(sizeof(auv_model_solver_capsule)); + auv_model_solver_capsule *capsule = (auv_model_solver_capsule *) capsule_mem; + + return capsule; +} + + +int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule) +{ + free(capsule); + return 0; +} + + +int auv_model_acados_create(auv_model_solver_capsule* capsule) +{ + int N_shooting_intervals = AUV_MODEL_N; + double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps + return auv_model_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps); +} + + +int auv_model_acados_update_time_steps(auv_model_solver_capsule* capsule, int N, double* new_time_steps) +{ + + if (N != capsule->nlp_solver_plan->N) { + fprintf(stderr, "auv_model_acados_update_time_steps: given number of time steps (= %d) " \ + "differs from the currently allocated number of " \ + "time steps (= %d)!\n" \ + "Please recreate with new discretization and provide a new vector of time_stamps!\n", + N, capsule->nlp_solver_plan->N); + return 1; + } + + ocp_nlp_config * nlp_config = capsule->nlp_config; + ocp_nlp_dims * nlp_dims = capsule->nlp_dims; + ocp_nlp_in * nlp_in = capsule->nlp_in; + + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]); + } + return 0; + +} + +/** + * Internal function for auv_model_acados_create: step 1 + */ +void auv_model_acados_create_set_plan(ocp_nlp_plan_t* nlp_solver_plan, const int N) +{ + assert(N == nlp_solver_plan->N); + + /************************************************ + * plan + ************************************************/ + + nlp_solver_plan->nlp_solver = SQP; + + nlp_solver_plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; + nlp_solver_plan->relaxed_ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; + nlp_solver_plan->nlp_cost[0] = LINEAR_LS; + for (int i = 1; i < N; i++) + nlp_solver_plan->nlp_cost[i] = LINEAR_LS; + + nlp_solver_plan->nlp_cost[N] = LINEAR_LS; + + for (int i = 0; i < N; i++) + { + nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL; + nlp_solver_plan->sim_solver_plan[i].sim_solver = IRK; + } + + nlp_solver_plan->nlp_constraints[0] = BGH; + + for (int i = 1; i < N; i++) + { + nlp_solver_plan->nlp_constraints[i] = BGH; + } + nlp_solver_plan->nlp_constraints[N] = BGH; + + nlp_solver_plan->regularization = NO_REGULARIZE; + + nlp_solver_plan->globalization = FIXED_STEP; +} + + +static ocp_nlp_dims* auv_model_acados_create_setup_dimensions(auv_model_solver_capsule* capsule) +{ + ocp_nlp_plan_t* nlp_solver_plan = capsule->nlp_solver_plan; + const int N = nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + + /************************************************ + * dimensions + ************************************************/ + #define NINTNP1MEMS 18 + int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS ); + + int* nx = intNp1mem + (N+1)*0; + int* nu = intNp1mem + (N+1)*1; + int* nbx = intNp1mem + (N+1)*2; + int* nbu = intNp1mem + (N+1)*3; + int* nsbx = intNp1mem + (N+1)*4; + int* nsbu = intNp1mem + (N+1)*5; + int* nsg = intNp1mem + (N+1)*6; + int* nsh = intNp1mem + (N+1)*7; + int* nsphi = intNp1mem + (N+1)*8; + int* ns = intNp1mem + (N+1)*9; + int* ng = intNp1mem + (N+1)*10; + int* nh = intNp1mem + (N+1)*11; + int* nphi = intNp1mem + (N+1)*12; + int* nz = intNp1mem + (N+1)*13; + int* ny = intNp1mem + (N+1)*14; + int* nr = intNp1mem + (N+1)*15; + int* nbxe = intNp1mem + (N+1)*16; + int* np = intNp1mem + (N+1)*17; + + for (int i = 0; i < N+1; i++) + { + // common + nx[i] = NX; + nu[i] = NU; + nz[i] = NZ; + ns[i] = NS; + // cost + ny[i] = NY; + // constraints + nbx[i] = NBX; + nbu[i] = NBU; + nsbx[i] = NSBX; + nsbu[i] = NSBU; + nsg[i] = NSG; + nsh[i] = NSH; + nsphi[i] = NSPHI; + ng[i] = NG; + nh[i] = NH; + nphi[i] = NPHI; + nr[i] = NR; + nbxe[i] = 0; + np[i] = NP; + } + + // for initial state + nbx[0] = NBX0; + nsbx[0] = 0; + ns[0] = NS0; + + nbxe[0] = 9; + + ny[0] = NY0; + nh[0] = NH0; + nsh[0] = NSH0; + nsphi[0] = NSPHI0; + nphi[0] = NPHI0; + + + // terminal - common + nu[N] = 0; + nz[N] = 0; + ns[N] = NSN; + // cost + ny[N] = NYN; + // constraint + nbx[N] = NBXN; + nbu[N] = 0; + ng[N] = NGN; + nh[N] = NHN; + nphi[N] = NPHIN; + nr[N] = 0; + + nsbx[N] = NSBXN; + nsbu[N] = 0; + nsg[N] = NSGN; + nsh[N] = NSHN; + nsphi[N] = NSPHIN; + + /* create and set ocp_nlp_dims */ + ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config); + + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns); + ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "np", np); + + ocp_nlp_dims_set_global(nlp_config, nlp_dims, "np_global", 0); + ocp_nlp_dims_set_global(nlp_config, nlp_dims, "n_global_data", 0); + + for (int i = 0; i <= N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]); + } + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]); + for (int i = 1; i < N; i++) + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nh", &nh[0]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, 0, "nsh", &nsh[0]); + + for (int i = 1; i < N; i++) + { + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]); + } + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]); + ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]); + ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); + free(intNp1mem); + + return nlp_dims; +} + + +/** + * Internal function for auv_model_acados_create: step 3 + */ +void auv_model_acados_create_setup_functions(auv_model_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + + /************************************************ + * external functions + ************************************************/ + +#define MAP_CASADI_FNC(__CAPSULE_FNC__, __MODEL_BASE_FNC__) do{ \ + capsule->__CAPSULE_FNC__.casadi_fun = & __MODEL_BASE_FNC__ ;\ + capsule->__CAPSULE_FNC__.casadi_n_in = & __MODEL_BASE_FNC__ ## _n_in; \ + capsule->__CAPSULE_FNC__.casadi_n_out = & __MODEL_BASE_FNC__ ## _n_out; \ + capsule->__CAPSULE_FNC__.casadi_sparsity_in = & __MODEL_BASE_FNC__ ## _sparsity_in; \ + capsule->__CAPSULE_FNC__.casadi_sparsity_out = & __MODEL_BASE_FNC__ ## _sparsity_out; \ + capsule->__CAPSULE_FNC__.casadi_work = & __MODEL_BASE_FNC__ ## _work; \ + external_function_external_param_casadi_create(&capsule->__CAPSULE_FNC__, &ext_fun_opts); \ + } while(false) + + external_function_opts ext_fun_opts; + external_function_opts_set_to_default(&ext_fun_opts); + + + ext_fun_opts.external_workspace = true; + if (N > 0) + { + + + + + // implicit dae + capsule->impl_dae_fun = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun[i], auv_model_impl_dae_fun); + } + + capsule->impl_dae_fun_jac_x_xdot_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_fun_jac_x_xdot_z[i], auv_model_impl_dae_fun_jac_x_xdot_z); + } + + capsule->impl_dae_jac_x_xdot_u_z = (external_function_external_param_casadi *) malloc(sizeof(external_function_external_param_casadi)*N); + for (int i = 0; i < N; i++) { + MAP_CASADI_FNC(impl_dae_jac_x_xdot_u_z[i], auv_model_impl_dae_jac_x_xdot_u_z); + } + + + + } // N > 0 + +#undef MAP_CASADI_FNC +} + + +/** + * Internal function for auv_model_acados_create: step 5 + */ +void auv_model_acados_create_set_default_parameters(auv_model_solver_capsule* capsule) +{ + + // no parameters defined + + + // no global parameters defined +} + + +/** + * Internal function for auv_model_acados_create: step 5 + */ +void auv_model_acados_setup_nlp_in(auv_model_solver_capsule* capsule, const int N, double* new_time_steps) +{ + assert(N == capsule->nlp_solver_plan->N); + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + + int tmp_int = 0; + + /************************************************ + * nlp_in + ************************************************/ + ocp_nlp_in * nlp_in = capsule->nlp_in; + /************************************************ + * nlp_out + ************************************************/ + ocp_nlp_out * nlp_out = capsule->nlp_out; + + // set up time_steps and cost_scaling + + if (new_time_steps) + { + // NOTE: this sets scaling and time_steps + auv_model_acados_update_time_steps(capsule, N, new_time_steps); + } + else + { + // set time_steps + + double time_step = 0.1; + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step); + } + // set cost scaling + double* cost_scaling = malloc((N+1)*sizeof(double)); + cost_scaling[0] = 0.1; + cost_scaling[1] = 0.1; + cost_scaling[2] = 0.1; + cost_scaling[3] = 0.1; + cost_scaling[4] = 0.1; + cost_scaling[5] = 0.1; + cost_scaling[6] = 0.1; + cost_scaling[7] = 0.1; + cost_scaling[8] = 0.1; + cost_scaling[9] = 0.1; + cost_scaling[10] = 0.1; + cost_scaling[11] = 0.1; + cost_scaling[12] = 0.1; + cost_scaling[13] = 0.1; + cost_scaling[14] = 0.1; + cost_scaling[15] = 0.1; + cost_scaling[16] = 0.1; + cost_scaling[17] = 0.1; + cost_scaling[18] = 0.1; + cost_scaling[19] = 0.1; + cost_scaling[20] = 1; + for (int i = 0; i <= N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &cost_scaling[i]); + } + free(cost_scaling); + } + + + + /**** Dynamics ****/ + for (int i = 0; i < N; i++) + { + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]); + ocp_nlp_dynamics_model_set_external_param_fun(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]); + + } + + /**** Cost ****/ + double* yref_0 = calloc(NY0, sizeof(double)); + // change only the non-zero elements: + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); + free(yref_0); + + double* W_0 = calloc(NY0*NY0, sizeof(double)); + // change only the non-zero elements: + W_0[0+(NY0) * 0] = 100; + W_0[4+(NY0) * 4] = 1; + W_0[5+(NY0) * 5] = 0.1; + W_0[6+(NY0) * 6] = 0.1; + W_0[7+(NY0) * 7] = 0.1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); + free(W_0); + double* Vx_0 = calloc(NY0*NX, sizeof(double)); + // change only the non-zero elements: + Vx_0[0+(NY0) * 0] = 1; + Vx_0[1+(NY0) * 4] = 1; + Vx_0[2+(NY0) * 5] = 1; + Vx_0[3+(NY0) * 7] = 1; + Vx_0[4+(NY0) * 8] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); + free(Vx_0); + double* Vu_0 = calloc(NY0*NU, sizeof(double)); + // change only the non-zero elements: + Vu_0[5+(NY0) * 0] = 1; + Vu_0[6+(NY0) * 1] = 1; + Vu_0[7+(NY0) * 2] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); + free(Vu_0); + double* yref = calloc(NY, sizeof(double)); + // change only the non-zero elements: + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); + } + free(yref); + double* W = calloc(NY*NY, sizeof(double)); + // change only the non-zero elements: + W[0+(NY) * 0] = 100; + W[4+(NY) * 4] = 1; + W[5+(NY) * 5] = 0.1; + W[6+(NY) * 6] = 0.1; + W[7+(NY) * 7] = 0.1; + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); + } + free(W); + double* Vx = calloc(NY*NX, sizeof(double)); + // change only the non-zero elements: + Vx[0+(NY) * 0] = 1; + Vx[1+(NY) * 4] = 1; + Vx[2+(NY) * 5] = 1; + Vx[3+(NY) * 7] = 1; + Vx[4+(NY) * 8] = 1; + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); + } + free(Vx); + + + double* Vu = calloc(NY*NU, sizeof(double)); + // change only the non-zero elements: + Vu[5+(NY) * 0] = 1; + Vu[6+(NY) * 1] = 1; + Vu[7+(NY) * 2] = 1; + + for (int i = 1; i < N; i++) + { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); + } + free(Vu); + double* yref_e = calloc(NYN, sizeof(double)); + // change only the non-zero elements: + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + free(yref_e); + + double* W_e = calloc(NYN*NYN, sizeof(double)); + // change only the non-zero elements: + W_e[0+(NYN) * 0] = 100; + W_e[4+(NYN) * 4] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + free(W_e); + double* Vx_e = calloc(NYN*NX, sizeof(double)); + // change only the non-zero elements: + Vx_e[0+(NYN) * 0] = 1; + Vx_e[1+(NYN) * 4] = 1; + Vx_e[2+(NYN) * 5] = 1; + Vx_e[3+(NYN) * 7] = 1; + Vx_e[4+(NYN) * 8] = 1; + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + free(Vx_e); + + + + + + + + /**** Constraints ****/ + + // bounds for initial stage + // x0 + int* idxbx0 = malloc(NBX0 * sizeof(int)); + idxbx0[0] = 0; + idxbx0[1] = 1; + idxbx0[2] = 2; + idxbx0[3] = 3; + idxbx0[4] = 4; + idxbx0[5] = 5; + idxbx0[6] = 6; + idxbx0[7] = 7; + idxbx0[8] = 8; + + double* lubx0 = calloc(2*NBX0, sizeof(double)); + double* lbx0 = lubx0; + double* ubx0 = lubx0 + NBX0; + // change only the non-zero elements: + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbx", idxbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); + free(idxbx0); + free(lubx0); + // idxbxe_0 + int* idxbxe_0 = malloc(9 * sizeof(int)); + idxbxe_0[0] = 0; + idxbxe_0[1] = 1; + idxbxe_0[2] = 2; + idxbxe_0[3] = 3; + idxbxe_0[4] = 4; + idxbxe_0[5] = 5; + idxbxe_0[6] = 6; + idxbxe_0[7] = 7; + idxbxe_0[8] = 8; + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "idxbxe", idxbxe_0); + free(idxbxe_0); + + + + + + + + + + + + + /* constraints that are the same for initial and intermediate */ + // u + int* idxbu = malloc(NBU * sizeof(int)); + idxbu[0] = 0; + idxbu[1] = 1; + idxbu[2] = 2; + double* lubu = calloc(2*NBU, sizeof(double)); + double* lbu = lubu; + double* ubu = lubu + NBU; + lbu[0] = -99.5; + ubu[0] = 99.5; + lbu[1] = -99.5; + ubu[1] = 99.5; + lbu[2] = -99.5; + ubu[2] = 99.5; + + for (int i = 0; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbu", idxbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbu", lbu); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubu", ubu); + } + free(idxbu); + free(lubu); + + + + + + + /* Path constraints */ + + // x + int* idxbx = malloc(NBX * sizeof(int)); + idxbx[0] = 7; + double* lubx = calloc(2*NBX, sizeof(double)); + double* lbx = lubx; + double* ubx = lubx + NBX; + lbx[0] = -1.4; + ubx[0] = 1.4; + + for (int i = 1; i < N; i++) + { + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "idxbx", idxbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "lbx", lbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, i, "ubx", ubx); + } + free(idxbx); + free(lubx); + + + + + + + + + + + + + + /* terminal constraints */ + + + + + + + + + + + + + + + + + + + + +} + + +static void auv_model_acados_create_set_opts(auv_model_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + void *nlp_opts = capsule->nlp_opts; + + /************************************************ + * opts + ************************************************/ + + + + int fixed_hess = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "fixed_hess", &fixed_hess); + + double globalization_fixed_step_length = 1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "globalization_fixed_step_length", &globalization_fixed_step_length); + + + + + int with_solution_sens_wrt_params = false; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_solution_sens_wrt_params", &with_solution_sens_wrt_params); + + int with_value_sens_wrt_params = false; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "with_value_sens_wrt_params", &with_value_sens_wrt_params); + + double solution_sens_qp_t_lam_min = 0.000000001; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "solution_sens_qp_t_lam_min", &solution_sens_qp_t_lam_min); + + int globalization_full_step_dual = 0; + ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization_full_step_dual", &globalization_full_step_dual); + + // set collocation type (relevant for implicit integrators) + sim_collocation_type collocation_type = GAUSS_LEGENDRE; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_collocation_type", &collocation_type); + + // set up sim_method_num_steps + // all sim_method_num_steps are identical + int sim_method_num_steps = 4; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); + + // set up sim_method_num_stages + // all sim_method_num_stages are identical + int sim_method_num_stages = 2; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); + + int newton_iter_val = 3; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); + + double newton_tol_val = 0; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_newton_tol", &newton_tol_val); + + // set up sim_method_jac_reuse + bool tmp_bool = (bool) 0; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); + + double levenberg_marquardt = 0.01; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "levenberg_marquardt", &levenberg_marquardt); + + /* options QP solver */ + + int nlp_solver_ext_qp_res = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "ext_qp_res", &nlp_solver_ext_qp_res); + + bool store_iterates = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "store_iterates", &store_iterates); + int log_primal_step_norm = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_primal_step_norm", &log_primal_step_norm); + + int log_dual_step_norm = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "log_dual_step_norm", &log_dual_step_norm); + + double nlp_solver_tol_min_step_norm = 0; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_min_step_norm", &nlp_solver_tol_min_step_norm); + // set HPIPM mode: should be done before setting other QP solver options + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_hpipm_mode", "BALANCE"); + + + + int qp_solver_t0_init = 2; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_t0_init", &qp_solver_t0_init); + + + + + // set SQP specific options + double nlp_solver_tol_stat = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_stat", &nlp_solver_tol_stat); + + double nlp_solver_tol_eq = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_eq", &nlp_solver_tol_eq); + + double nlp_solver_tol_ineq = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_ineq", &nlp_solver_tol_ineq); + + double nlp_solver_tol_comp = 0.000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "tol_comp", &nlp_solver_tol_comp); + + int nlp_solver_max_iter = 100; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "max_iter", &nlp_solver_max_iter); + + // set options for adaptive Levenberg-Marquardt Update + bool with_adaptive_levenberg_marquardt = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_adaptive_levenberg_marquardt", &with_adaptive_levenberg_marquardt); + + double adaptive_levenberg_marquardt_lam = 5; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_lam", &adaptive_levenberg_marquardt_lam); + + double adaptive_levenberg_marquardt_mu_min = 0.0000000000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu_min", &adaptive_levenberg_marquardt_mu_min); + + double adaptive_levenberg_marquardt_mu0 = 0.001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_mu0", &adaptive_levenberg_marquardt_mu0); + + double adaptive_levenberg_marquardt_obj_scalar = 2; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "adaptive_levenberg_marquardt_obj_scalar", &adaptive_levenberg_marquardt_obj_scalar); + + bool eval_residual_at_max_iter = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "eval_residual_at_max_iter", &eval_residual_at_max_iter); + + // QP scaling + double qpscaling_ub_max_abs_eig = 100000; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_ub_max_abs_eig", &qpscaling_ub_max_abs_eig); + + double qpscaling_lb_norm_inf_grad_obj = 0.0001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_lb_norm_inf_grad_obj", &qpscaling_lb_norm_inf_grad_obj); + + qpscaling_scale_objective_type qpscaling_scale_objective = NO_OBJECTIVE_SCALING; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_objective", &qpscaling_scale_objective); + + ocp_nlp_qpscaling_constraint_type qpscaling_scale_constraints = NO_CONSTRAINT_SCALING; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qpscaling_scale_constraints", &qpscaling_scale_constraints); + + // NLP QP tol strategy + ocp_nlp_qp_tol_strategy_t nlp_qp_tol_strategy = FIXED_QP_TOL; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_strategy", &nlp_qp_tol_strategy); + + double nlp_qp_tol_reduction_factor = 0.1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_reduction_factor", &nlp_qp_tol_reduction_factor); + + double nlp_qp_tol_safety_factor = 0.1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_safety_factor", &nlp_qp_tol_safety_factor); + + double nlp_qp_tol_min_stat = 0.000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_stat", &nlp_qp_tol_min_stat); + + double nlp_qp_tol_min_eq = 0.0000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_eq", &nlp_qp_tol_min_eq); + + double nlp_qp_tol_min_ineq = 0.0000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_ineq", &nlp_qp_tol_min_ineq); + + double nlp_qp_tol_min_comp = 0.00000000001; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "nlp_qp_tol_min_comp", &nlp_qp_tol_min_comp); + + bool with_anderson_acceleration = false; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "with_anderson_acceleration", &with_anderson_acceleration); + + double anderson_activation_threshold = 10; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "anderson_activation_threshold", &anderson_activation_threshold); + + int qp_solver_iter_max = 50; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_iter_max", &qp_solver_iter_max); + + + int qp_solver_warm_start = 1; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "qp_warm_start", &qp_solver_warm_start); + + int print_level = 2; + ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "print_level", &print_level); + + int ext_cost_num_hess = 0; +} + + +/** + * Internal function for auv_model_acados_create: step 7 + */ +void auv_model_acados_set_nlp_out(auv_model_solver_capsule* capsule) +{ + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; + + // initialize primal solution + double* xu0 = calloc(NX+NU, sizeof(double)); + double* x0 = xu0; + + // initialize with x0 + + + double* u0 = xu0 + NX; + + for (int i = 0; i < N; i++) + { + // x0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x0); + // u0 + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x0); + free(xu0); +} + + +/** + * Internal function for auv_model_acados_create: step 9 + */ +int auv_model_acados_create_precompute(auv_model_solver_capsule* capsule) { + int status = ocp_nlp_precompute(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + if (status != ACADOS_SUCCESS) { + printf("\nocp_nlp_precompute failed!\n\n"); + exit(1); + } + + return status; +} + + +int auv_model_acados_create_with_discretization(auv_model_solver_capsule* capsule, int N, double* new_time_steps) +{ + // If N does not match the number of shooting intervals used for code generation, new_time_steps must be given. + if (N != AUV_MODEL_N && !new_time_steps) { + fprintf(stderr, "auv_model_acados_create_with_discretization: new_time_steps is NULL " \ + "but the number of shooting intervals (= %d) differs from the number of " \ + "shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \ + N, AUV_MODEL_N); + return 1; + } + + // number of expected runtime parameters + capsule->nlp_np = NP; + + // 1) create and set nlp_solver_plan; create nlp_config + capsule->nlp_solver_plan = ocp_nlp_plan_create(N); + auv_model_acados_create_set_plan(capsule->nlp_solver_plan, N); + capsule->nlp_config = ocp_nlp_config_create(*capsule->nlp_solver_plan); + + // 2) create and set dimensions + capsule->nlp_dims = auv_model_acados_create_setup_dimensions(capsule); + + // 3) create and set nlp_opts + capsule->nlp_opts = ocp_nlp_solver_opts_create(capsule->nlp_config, capsule->nlp_dims); + auv_model_acados_create_set_opts(capsule); + + // 4) create and set nlp_out + // 4.1) nlp_out + capsule->nlp_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + // 4.2) sens_out + capsule->sens_out = ocp_nlp_out_create(capsule->nlp_config, capsule->nlp_dims); + auv_model_acados_set_nlp_out(capsule); + + // 5) create nlp_in + capsule->nlp_in = ocp_nlp_in_create(capsule->nlp_config, capsule->nlp_dims); + + // 6) setup functions, nlp_in and default parameters + auv_model_acados_create_setup_functions(capsule); + auv_model_acados_setup_nlp_in(capsule, N, new_time_steps); + auv_model_acados_create_set_default_parameters(capsule); + + // 7) create solver + capsule->nlp_solver = ocp_nlp_solver_create(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_opts, capsule->nlp_in); + + + // 8) do precomputations + int status = auv_model_acados_create_precompute(capsule); + + return status; +} + +/** + * This function is for updating an already initialized solver with a different number of qp_cond_N. It is useful for code reuse after code export. + */ +int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule* capsule, int qp_solver_cond_N) +{ + printf("\nacados_update_qp_solver_cond_N() not implemented, since no partial condensing solver is used!\n\n"); + exit(1); +} + + +int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem) +{ + + // set initialization to all zeros + + const int N = capsule->nlp_solver_plan->N; + ocp_nlp_config* nlp_config = capsule->nlp_config; + ocp_nlp_dims* nlp_dims = capsule->nlp_dims; + ocp_nlp_out* nlp_out = capsule->nlp_out; + ocp_nlp_in* nlp_in = capsule->nlp_in; + ocp_nlp_solver* nlp_solver = capsule->nlp_solver; + + double* buffer = calloc(NX+NU+NZ+2*NS+2*NSN+2*NS0+NBX+NBU+NG+NH+NPHI+NBX0+NBXN+NHN+NH0+NPHIN+NGN, sizeof(double)); + + for(int i=0; inlp_config, capsule->nlp_dims, capsule->nlp_in, stage, "parameter_values", p); + + return solver_status; +} + + +int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update) +{ + ocp_nlp_in_set_params_sparse(capsule->nlp_config, capsule->nlp_dims, capsule->nlp_in, stage, idx, p, n_update); + + return 0; +} + + +int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len) +{ + + // printf("No global_data, auv_model_acados_set_p_global_and_precompute_dependencies does nothing.\n"); + return 0; +} + + + + +int auv_model_acados_solve(auv_model_solver_capsule* capsule) +{ + // solve NLP + int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + return solver_status; +} + + + +int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule) +{ + int solver_status = ocp_nlp_setup_qp_matrices_and_factorize(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out); + + return solver_status; +} + + + + + + +int auv_model_acados_free(auv_model_solver_capsule* capsule) +{ + // before destroying, keep some info + const int N = capsule->nlp_solver_plan->N; + // free memory + ocp_nlp_solver_opts_destroy(capsule->nlp_opts); + ocp_nlp_in_destroy(capsule->nlp_in); + ocp_nlp_out_destroy(capsule->nlp_out); + ocp_nlp_out_destroy(capsule->sens_out); + ocp_nlp_solver_destroy(capsule->nlp_solver); + ocp_nlp_dims_destroy(capsule->nlp_dims); + ocp_nlp_config_destroy(capsule->nlp_config); + ocp_nlp_plan_destroy(capsule->nlp_solver_plan); + + /* free external function */ + // dynamics + for (int i = 0; i < N; i++) + { + external_function_external_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_external_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]); + external_function_external_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]); + + } + free(capsule->impl_dae_fun); + free(capsule->impl_dae_fun_jac_x_xdot_z); + free(capsule->impl_dae_jac_x_xdot_u_z); + + + // cost + + // constraints + + + + return 0; +} + + +void auv_model_acados_print_stats(auv_model_solver_capsule* capsule) +{ + int nlp_iter, stat_m, stat_n, tmp_int; + ocp_nlp_get(capsule->nlp_solver, "nlp_iter", &nlp_iter); + ocp_nlp_get(capsule->nlp_solver, "stat_n", &stat_n); + ocp_nlp_get(capsule->nlp_solver, "stat_m", &stat_m); + + + int stat_n_max = 16; + if (stat_n > stat_n_max) + { + printf("stat_n_max = %d is too small, increase it in the template!\n", stat_n_max); + exit(1); + } + double stat[1616]; + ocp_nlp_get(capsule->nlp_solver, "statistics", stat); + + int nrow = nlp_iter+1 < stat_m ? nlp_iter+1 : stat_m; + + + printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\talpha"); + if (stat_n > 8) + printf("\t\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp"); + printf("\n"); + for (int i = 0; i < nrow; i++) + { + for (int j = 0; j < stat_n + 1; j++) + { + if (j == 0 || j == 5 || j == 6) + { + tmp_int = (int) stat[i + j * nrow]; + printf("%d\t", tmp_int); + } + else + { + printf("%e\t", stat[i + j * nrow]); + } + } + printf("\n"); + } +} + +int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len) +{ + (void)capsule; + (void)data; + (void)data_len; + printf("\ndummy function that can be called in between solver calls to update parameters or numerical data efficiently in C.\n"); + printf("nothing set yet..\n"); + return 1; + +} + + + +ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule* capsule) { return capsule->nlp_in; } +ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule* capsule) { return capsule->nlp_out; } +ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule* capsule) { return capsule->sens_out; } +ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule* capsule) { return capsule->nlp_solver; } +ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule* capsule) { return capsule->nlp_config; } +void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule* capsule) { return capsule->nlp_opts; } +ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule* capsule) { return capsule->nlp_dims; } +ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule* capsule) { return capsule->nlp_solver_plan; } diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h new file mode 100644 index 000000000..8fd75b39a --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.h @@ -0,0 +1,174 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SOLVER_auv_model_H_ +#define ACADOS_SOLVER_auv_model_H_ + +#include "acados/utils/types.h" + +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +#define AUV_MODEL_NX 9 +#define AUV_MODEL_NZ 0 +#define AUV_MODEL_NU 3 +#define AUV_MODEL_NP 0 +#define AUV_MODEL_NP_GLOBAL 0 +#define AUV_MODEL_NBX 1 +#define AUV_MODEL_NBX0 9 +#define AUV_MODEL_NBU 3 +#define AUV_MODEL_NSBX 0 +#define AUV_MODEL_NSBU 0 +#define AUV_MODEL_NSH 0 +#define AUV_MODEL_NSH0 0 +#define AUV_MODEL_NSG 0 +#define AUV_MODEL_NSPHI 0 +#define AUV_MODEL_NSHN 0 +#define AUV_MODEL_NSGN 0 +#define AUV_MODEL_NSPHIN 0 +#define AUV_MODEL_NSPHI0 0 +#define AUV_MODEL_NSBXN 0 +#define AUV_MODEL_NS 0 +#define AUV_MODEL_NS0 0 +#define AUV_MODEL_NSN 0 +#define AUV_MODEL_NG 0 +#define AUV_MODEL_NBXN 0 +#define AUV_MODEL_NGN 0 +#define AUV_MODEL_NY0 8 +#define AUV_MODEL_NY 8 +#define AUV_MODEL_NYN 5 +#define AUV_MODEL_N 20 +#define AUV_MODEL_NH 0 +#define AUV_MODEL_NHN 0 +#define AUV_MODEL_NH0 0 +#define AUV_MODEL_NPHI0 0 +#define AUV_MODEL_NPHI 0 +#define AUV_MODEL_NPHIN 0 +#define AUV_MODEL_NR 0 + +#ifdef __cplusplus +extern "C" { +#endif + + +// ** capsule for solver data ** +typedef struct auv_model_solver_capsule +{ + // acados objects + ocp_nlp_in *nlp_in; + ocp_nlp_out *nlp_out; + ocp_nlp_out *sens_out; + ocp_nlp_solver *nlp_solver; + void *nlp_opts; + ocp_nlp_plan_t *nlp_solver_plan; + ocp_nlp_config *nlp_config; + ocp_nlp_dims *nlp_dims; + + // number of expected runtime parameters + unsigned int nlp_np; + + /* external functions */ + + // dynamics + + external_function_external_param_casadi *impl_dae_fun; + external_function_external_param_casadi *impl_dae_fun_jac_x_xdot_z; + external_function_external_param_casadi *impl_dae_jac_x_xdot_u_z; + external_function_external_param_casadi *impl_dae_jac_p; + + + + + // cost + + + + + + + // constraints + + + + + + + +} auv_model_solver_capsule; + +ACADOS_SYMBOL_EXPORT auv_model_solver_capsule * auv_model_acados_create_capsule(void); +ACADOS_SYMBOL_EXPORT int auv_model_acados_free_capsule(auv_model_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_create(auv_model_solver_capsule * capsule); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_reset(auv_model_solver_capsule* capsule, int reset_qp_solver_mem); + +/** + * Generic version of auv_model_acados_create which allows to use a different number of shooting intervals than + * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code + * generation, the time-steps from code generation is used. + */ +ACADOS_SYMBOL_EXPORT int auv_model_acados_create_with_discretization(auv_model_solver_capsule * capsule, int n_time_steps, double* new_time_steps); +/** + * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the + * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0. + */ +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_time_steps(auv_model_solver_capsule * capsule, int N, double* new_time_steps); +/** + * This function is used for updating an already initialized solver with a different number of qp_cond_N. + */ +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_qp_solver_cond_N(auv_model_solver_capsule * capsule, int qp_solver_cond_N); +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params(auv_model_solver_capsule * capsule, int stage, double *value, int np); +ACADOS_SYMBOL_EXPORT int auv_model_acados_update_params_sparse(auv_model_solver_capsule * capsule, int stage, int *idx, double *p, int n_update); +ACADOS_SYMBOL_EXPORT int auv_model_acados_set_p_global_and_precompute_dependencies(auv_model_solver_capsule* capsule, double* data, int data_len); + +ACADOS_SYMBOL_EXPORT int auv_model_acados_solve(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_setup_qp_matrices_and_factorize(auv_model_solver_capsule* capsule); + + + +ACADOS_SYMBOL_EXPORT int auv_model_acados_free(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void auv_model_acados_print_stats(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT int auv_model_acados_custom_update(auv_model_solver_capsule* capsule, double* data, int data_len); + +ACADOS_SYMBOL_EXPORT ocp_nlp_in *auv_model_acados_get_nlp_in(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_nlp_out(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *auv_model_acados_get_sens_out(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_solver *auv_model_acados_get_nlp_solver(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_config *auv_model_acados_get_nlp_config(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void *auv_model_acados_get_nlp_opts(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_dims *auv_model_acados_get_nlp_dims(auv_model_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *auv_model_acados_get_nlp_plan(auv_model_solver_capsule * capsule); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SOLVER_auv_model_H_ diff --git a/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o new file mode 100644 index 000000000..17dcd34d5 Binary files /dev/null and b/control/velocity_controller/scripts/build_auv_solver/acados_solver_auv_model.o differ diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c new file mode 100644 index 000000000..acd0b61e9 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.c @@ -0,0 +1,394 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; + +/* auv_model_impl_dae_fun:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a05=(a05*a06); + a07=30.; + a08=arg[0]? arg[0][4] : 0; + a09=(a07*a08); + a10=arg[0]? arg[0][2] : 0; + a09=(a09*a10); + a05=(a05+a09); + a02=(a02-a05); + a05=23.; + a09=arg[0]? arg[0][0] : 0; + a11=casadi_fabs(a09); + a05=(a05+a11); + a05=(a05*a09); + a02=(a02-a05); + a01=(a01*a02); + a05=6.0368975005218254e-05; + a11=(a07*a04); + a11=(a11*a09); + a12=arg[0]? arg[0][3] : 0; + a13=(a03*a12); + a13=(a13*a10); + a11=(a11+a13); + a13=46.; + a14=casadi_fabs(a06); + a14=(a13+a14); + a14=(a14*a06); + a11=(a11+a14); + a05=(a05*a11); + a01=(a01-a05); + a05=-1.1116270017027866e-07; + a03=(a03*a08); + a03=(a03*a09); + a07=(a07*a12); + a07=(a07*a06); + a03=(a03+a07); + a07=casadi_fabs(a10); + a07=(a13+a07); + a07=(a07*a10); + a03=(a03+a07); + a05=(a05*a03); + a01=(a01-a05); + a05=9.8084735444363873e-08; + a07=3.3399999999999999e+00; + a07=(a07*a04); + a07=(a07*a08); + a10=-3.3199999999999998e+00; + a10=(a10*a08); + a10=(a10*a04); + a07=(a07+a10); + a10=casadi_fabs(a12); + a10=(a13+a10); + a10=(a10*a12); + a07=(a07+a10); + a05=(a05*a07); + a01=(a01-a05); + a05=1.0920100546139184e-05; + a10=arg[2]? arg[2][1] : 0; + a06=-3.3399999999999999e+00; + a06=(a06*a04); + a06=(a06*a12); + a09=6.8000000000000005e-01; + a09=(a09*a12); + a09=(a09*a04); + a06=(a06+a09); + a10=(a10-a06); + a06=casadi_fabs(a08); + a06=(a13+a06); + a06=(a06*a08); + a10=(a10-a06); + a05=(a05*a10); + a01=(a01+a05); + a05=-6.0150572994295574e-03; + a06=arg[2]? arg[2][2] : 0; + a09=3.3199999999999998e+00; + a09=(a09*a08); + a09=(a09*a12); + a14=-6.8000000000000005e-01; + a14=(a14*a12); + a14=(a14*a08); + a09=(a09+a14); + a06=(a06-a09); + a09=casadi_fabs(a04); + a13=(a13+a09); + a13=(a13*a04); + a06=(a06-a13); + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a01=6.0368975005218423e-05; + a01=(a01*a02); + a05=3.3484658136227786e-02; + a05=(a05*a11); + a01=(a01-a05); + a05=-6.1658244361114702e-05; + a05=(a05*a03); + a01=(a01-a05); + a05=5.4404333259807184e-05; + a05=(a05*a07); + a01=(a01-a05); + a05=6.0570157695918683e-03; + a05=(a05*a10); + a01=(a01+a05); + a05=-3.0184487502609172e-03; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a01=-1.1116270017027936e-07; + a01=(a01*a02); + a05=-6.1658244361114783e-05; + a05=(a05*a11); + a01=(a01-a05); + a05=3.3963490377380855e-02; + a05=(a05*a03); + a01=(a01-a05); + a05=-2.9967785627100754e-02; + a05=(a05*a07); + a01=(a01-a05); + a05=-3.0801331505514837e-03; + a05=(a05*a10); + a01=(a01+a05); + a05=5.5581350085139543e-06; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a01=9.8084735444364535e-08; + a01=(a01*a02); + a05=5.4404333259807062e-05; + a05=(a05*a11); + a01=(a01-a05); + a05=-2.9967785627100469e-02; + a05=(a05*a03); + a01=(a01-a05); + a05=1.4970303990827358e+00; + a05=(a05*a07); + a01=(a01-a05); + a05=2.7177645446042520e-03; + a05=(a05*a10); + a01=(a01+a05); + a05=-4.9042367722181902e-06; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a01=1.0920100546139210e-05; + a01=(a01*a02); + a05=6.0570157695918657e-03; + a05=(a05*a11); + a01=(a01-a05); + a05=-3.0801331505514781e-03; + a05=(a05*a03); + a01=(a01-a05); + a05=2.7177645446042498e-03; + a05=(a05*a07); + a01=(a01-a05); + a05=3.0257778596593993e-01; + a05=(a05*a10); + a01=(a01+a05); + a05=-5.4600502730695923e-04; + a13=(a05*a06); + a01=(a01+a13); + a00=(a00-a01); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a01=-6.0150572994295635e-03; + a01=(a01*a02); + a02=-3.0184487502609133e-03; + a02=(a02*a11); + a01=(a01-a02); + a02=5.5581350085139340e-06; + a02=(a02*a03); + a01=(a01-a02); + a02=-4.9042367722181935e-06; + a02=(a02*a07); + a01=(a01-a02); + a05=(a05*a10); + a01=(a01+a05); + a05=3.0075286497147785e-01; + a05=(a05*a06); + a01=(a01+a05); + a00=(a00-a01); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a01=arg[0]? arg[0][6] : 0; + a05=sin(a01); + a06=arg[0]? arg[0][7] : 0; + a10=tan(a06); + a02=(a05*a10); + a02=(a02*a08); + a12=(a12+a02); + a01=cos(a01); + a10=(a01*a10); + a10=(a10*a04); + a12=(a12+a10); + a00=(a00-a12); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a12=(a01*a08); + a10=(a05*a04); + a12=(a12-a10); + a00=(a00-a12); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a06=cos(a06); + a05=(a05/a06); + a05=(a05*a08); + a01=(a01/a06); + a01=(a01*a04); + a05=(a05+a01); + a00=(a00-a05); + if (res[0]!=0) res[0][8]=a00; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_n_out(void) { return 1;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o new file mode 100644 index 000000000..0dc1612cd Binary files /dev/null and b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun.o differ diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c new file mode 100644 index 000000000..0321b6e18 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.c @@ -0,0 +1,847 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_u_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[24] = + {9, 3, 0, 6, 12, 18, 0, 1, + 2, 3, 4, 5, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5}; + +/* auv_model_impl_dae_fun_jac_x_xdot_u:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; + casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; + casadi_real a72, a73, a74, a75, a76, a77, a78; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a07=(a05*a06); + a08=30.; + a09=arg[0]? arg[0][4] : 0; + a10=(a08*a09); + a11=arg[0]? arg[0][2] : 0; + a12=(a10*a11); + a07=(a07+a12); + a02=(a02-a07); + a07=23.; + a12=arg[0]? arg[0][0] : 0; + a13=casadi_fabs(a12); + a07=(a07+a13); + a13=(a07*a12); + a02=(a02-a13); + a13=(a01*a02); + a14=6.0368975005218254e-05; + a15=(a08*a04); + a16=(a15*a12); + a17=arg[0]? arg[0][3] : 0; + a18=(a03*a17); + a19=(a18*a11); + a16=(a16+a19); + a19=46.; + a20=casadi_fabs(a06); + a20=(a19+a20); + a21=(a20*a06); + a16=(a16+a21); + a21=(a14*a16); + a13=(a13-a21); + a21=-1.1116270017027866e-07; + a22=(a03*a09); + a23=(a22*a12); + a24=(a08*a17); + a25=(a24*a06); + a23=(a23+a25); + a25=casadi_fabs(a11); + a25=(a19+a25); + a26=(a25*a11); + a23=(a23+a26); + a26=(a21*a23); + a13=(a13-a26); + a26=9.8084735444363873e-08; + a27=3.3399999999999999e+00; + a28=(a27*a04); + a29=(a28*a09); + a30=-3.3199999999999998e+00; + a31=(a30*a09); + a32=(a31*a04); + a29=(a29+a32); + a32=casadi_fabs(a17); + a32=(a19+a32); + a33=(a32*a17); + a29=(a29+a33); + a33=(a26*a29); + a13=(a13-a33); + a33=1.0920100546139184e-05; + a34=arg[2]? arg[2][1] : 0; + a35=-3.3399999999999999e+00; + a36=(a35*a04); + a37=(a36*a17); + a38=6.8000000000000005e-01; + a39=(a38*a17); + a40=(a39*a04); + a37=(a37+a40); + a34=(a34-a37); + a37=casadi_fabs(a09); + a37=(a19+a37); + a40=(a37*a09); + a34=(a34-a40); + a40=(a33*a34); + a13=(a13+a40); + a40=-6.0150572994295574e-03; + a41=arg[2]? arg[2][2] : 0; + a42=3.3199999999999998e+00; + a43=(a42*a09); + a44=(a43*a17); + a45=-6.8000000000000005e-01; + a46=(a45*a17); + a47=(a46*a09); + a44=(a44+a47); + a41=(a41-a44); + a44=casadi_fabs(a04); + a19=(a19+a44); + a44=(a19*a04); + a41=(a41-a44); + a44=(a40*a41); + a13=(a13+a44); + a00=(a00-a13); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a13=6.0368975005218423e-05; + a44=(a13*a02); + a47=3.3484658136227786e-02; + a48=(a47*a16); + a44=(a44-a48); + a48=-6.1658244361114702e-05; + a49=(a48*a23); + a44=(a44-a49); + a49=5.4404333259807184e-05; + a50=(a49*a29); + a44=(a44-a50); + a50=6.0570157695918683e-03; + a51=(a50*a34); + a44=(a44+a51); + a51=-3.0184487502609172e-03; + a52=(a51*a41); + a44=(a44+a52); + a00=(a00-a44); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a44=-1.1116270017027936e-07; + a52=(a44*a02); + a53=-6.1658244361114783e-05; + a54=(a53*a16); + a52=(a52-a54); + a54=3.3963490377380855e-02; + a55=(a54*a23); + a52=(a52-a55); + a55=-2.9967785627100754e-02; + a56=(a55*a29); + a52=(a52-a56); + a56=-3.0801331505514837e-03; + a57=(a56*a34); + a52=(a52+a57); + a57=5.5581350085139543e-06; + a58=(a57*a41); + a52=(a52+a58); + a00=(a00-a52); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a52=9.8084735444364535e-08; + a58=(a52*a02); + a59=5.4404333259807062e-05; + a60=(a59*a16); + a58=(a58-a60); + a60=-2.9967785627100469e-02; + a61=(a60*a23); + a58=(a58-a61); + a61=1.4970303990827358e+00; + a62=(a61*a29); + a58=(a58-a62); + a62=2.7177645446042520e-03; + a63=(a62*a34); + a58=(a58+a63); + a63=-4.9042367722181902e-06; + a64=(a63*a41); + a58=(a58+a64); + a00=(a00-a58); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a58=1.0920100546139210e-05; + a64=(a58*a02); + a65=6.0570157695918657e-03; + a66=(a65*a16); + a64=(a64-a66); + a66=-3.0801331505514781e-03; + a67=(a66*a23); + a64=(a64-a67); + a67=2.7177645446042498e-03; + a68=(a67*a29); + a64=(a64-a68); + a68=3.0257778596593993e-01; + a69=(a68*a34); + a64=(a64+a69); + a69=-5.4600502730695923e-04; + a70=(a69*a41); + a64=(a64+a70); + a00=(a00-a64); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a64=-6.0150572994295635e-03; + a02=(a64*a02); + a70=-3.0184487502609133e-03; + a16=(a70*a16); + a02=(a02-a16); + a16=5.5581350085139340e-06; + a23=(a16*a23); + a02=(a02-a23); + a23=-4.9042367722181935e-06; + a29=(a23*a29); + a02=(a02-a29); + a34=(a69*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a41=(a34*a41); + a02=(a02+a41); + a00=(a00-a02); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a02=arg[0]? arg[0][6] : 0; + a41=sin(a02); + a29=arg[0]? arg[0][7] : 0; + a71=tan(a29); + a72=(a41*a71); + a73=(a72*a09); + a73=(a17+a73); + a02=cos(a02); + a74=(a02*a71); + a75=(a74*a04); + a73=(a73+a75); + a00=(a00-a73); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a73=(a02*a09); + a75=(a41*a04); + a73=(a73-a75); + a00=(a00-a73); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a73=cos(a29); + a75=(a41/a73); + a76=(a75*a09); + a77=(a02/a73); + a78=(a77*a04); + a76=(a76+a78); + a00=(a00-a76); + if (res[0]!=0) res[0][8]=a00; + a00=casadi_sign(a12); + a00=(a12*a00); + a00=(a00+a07); + a07=(a01*a00); + a76=(a14*a15); + a07=(a07+a76); + a76=(a21*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][0]=a07; + a07=(a13*a00); + a76=(a47*a15); + a07=(a07+a76); + a76=(a48*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][1]=a07; + a07=(a44*a00); + a76=(a53*a15); + a07=(a07+a76); + a76=(a54*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][2]=a07; + a07=(a52*a00); + a76=(a59*a15); + a07=(a07+a76); + a76=(a60*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][3]=a07; + a07=(a58*a00); + a76=(a65*a15); + a07=(a07+a76); + a76=(a66*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][4]=a07; + a00=(a64*a00); + a15=(a70*a15); + a00=(a00+a15); + a22=(a16*a22); + a00=(a00+a22); + if (res[1]!=0) res[1][5]=a00; + a00=(a01*a05); + a22=casadi_sign(a06); + a22=(a06*a22); + a22=(a22+a20); + a20=(a14*a22); + a00=(a00+a20); + a20=(a21*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][6]=a00; + a00=(a13*a05); + a20=(a47*a22); + a00=(a00+a20); + a20=(a48*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][7]=a00; + a00=(a44*a05); + a20=(a53*a22); + a00=(a00+a20); + a20=(a54*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][8]=a00; + a00=(a52*a05); + a20=(a59*a22); + a00=(a00+a20); + a20=(a60*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][9]=a00; + a00=(a58*a05); + a20=(a65*a22); + a00=(a00+a20); + a20=(a66*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][10]=a00; + a05=(a64*a05); + a22=(a70*a22); + a05=(a05+a22); + a24=(a16*a24); + a05=(a05+a24); + if (res[1]!=0) res[1][11]=a05; + a05=(a01*a10); + a24=(a14*a18); + a05=(a05+a24); + a24=casadi_sign(a11); + a24=(a11*a24); + a24=(a24+a25); + a25=(a21*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][12]=a05; + a05=(a13*a10); + a25=(a47*a18); + a05=(a05+a25); + a25=(a48*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][13]=a05; + a05=(a44*a10); + a25=(a53*a18); + a05=(a05+a25); + a25=(a54*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][14]=a05; + a05=(a52*a10); + a25=(a59*a18); + a05=(a05+a25); + a25=(a60*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][15]=a05; + a05=(a58*a10); + a25=(a65*a18); + a05=(a05+a25); + a25=(a66*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][16]=a05; + a10=(a64*a10); + a18=(a70*a18); + a10=(a10+a18); + a24=(a16*a24); + a10=(a10+a24); + if (res[1]!=0) res[1][17]=a10; + a10=(a03*a11); + a24=(a14*a10); + a18=(a08*a06); + a05=(a21*a18); + a24=(a24+a05); + a05=casadi_sign(a17); + a05=(a17*a05); + a05=(a05+a32); + a32=(a26*a05); + a24=(a24+a32); + a38=(a38*a04); + a36=(a36+a38); + a38=(a33*a36); + a24=(a24+a38); + a45=(a45*a09); + a43=(a43+a45); + a45=(a40*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][18]=a24; + a24=(a47*a10); + a45=(a48*a18); + a24=(a24+a45); + a45=(a49*a05); + a24=(a24+a45); + a45=(a50*a36); + a24=(a24+a45); + a45=(a51*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][19]=a24; + a24=(a53*a10); + a45=(a54*a18); + a24=(a24+a45); + a45=(a55*a05); + a24=(a24+a45); + a45=(a56*a36); + a24=(a24+a45); + a45=(a57*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][20]=a24; + a24=(a59*a10); + a45=(a60*a18); + a24=(a24+a45); + a45=(a61*a05); + a24=(a24+a45); + a45=(a62*a36); + a24=(a24+a45); + a45=(a63*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][21]=a24; + a24=(a65*a10); + a45=(a66*a18); + a24=(a24+a45); + a45=(a67*a05); + a24=(a24+a45); + a45=(a68*a36); + a24=(a24+a45); + a45=(a69*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][22]=a24; + a10=(a70*a10); + a18=(a16*a18); + a10=(a10+a18); + a05=(a23*a05); + a10=(a10+a05); + a36=(a69*a36); + a10=(a10+a36); + a43=(a34*a43); + a10=(a10+a43); + if (res[1]!=0) res[1][23]=a10; + a10=-1.; + if (res[1]!=0) res[1][24]=a10; + a11=(a08*a11); + a10=(a01*a11); + a43=(a03*a12); + a21=(a21*a43); + a10=(a10+a21); + a30=(a30*a04); + a28=(a28+a30); + a30=(a26*a28); + a10=(a10+a30); + a30=casadi_sign(a09); + a30=(a09*a30); + a30=(a30+a37); + a37=(a33*a30); + a10=(a10+a37); + a42=(a42*a17); + a42=(a42+a46); + a46=(a40*a42); + a10=(a10+a46); + if (res[1]!=0) res[1][25]=a10; + a10=(a13*a11); + a48=(a48*a43); + a10=(a10+a48); + a48=(a49*a28); + a10=(a10+a48); + a48=(a50*a30); + a10=(a10+a48); + a48=(a51*a42); + a10=(a10+a48); + if (res[1]!=0) res[1][26]=a10; + a10=(a44*a11); + a54=(a54*a43); + a10=(a10+a54); + a54=(a55*a28); + a10=(a10+a54); + a54=(a56*a30); + a10=(a10+a54); + a54=(a57*a42); + a10=(a10+a54); + if (res[1]!=0) res[1][27]=a10; + a10=(a52*a11); + a60=(a60*a43); + a10=(a10+a60); + a60=(a61*a28); + a10=(a10+a60); + a60=(a62*a30); + a10=(a10+a60); + a60=(a63*a42); + a10=(a10+a60); + if (res[1]!=0) res[1][28]=a10; + a10=(a58*a11); + a66=(a66*a43); + a10=(a10+a66); + a66=(a67*a28); + a10=(a10+a66); + a66=(a68*a30); + a10=(a10+a66); + a66=(a69*a42); + a10=(a10+a66); + if (res[1]!=0) res[1][29]=a10; + a11=(a64*a11); + a16=(a16*a43); + a11=(a11+a16); + a28=(a23*a28); + a11=(a11+a28); + a30=(a69*a30); + a11=(a11+a30); + a42=(a34*a42); + a11=(a11+a42); + if (res[1]!=0) res[1][30]=a11; + a72=(-a72); + if (res[1]!=0) res[1][31]=a72; + a72=(-a02); + if (res[1]!=0) res[1][32]=a72; + a72=(-a75); + if (res[1]!=0) res[1][33]=a72; + a03=(a03*a06); + a01=(a01*a03); + a08=(a08*a12); + a14=(a14*a08); + a01=(a01+a14); + a27=(a27*a09); + a27=(a27+a31); + a26=(a26*a27); + a01=(a01+a26); + a35=(a35*a17); + a35=(a35+a39); + a33=(a33*a35); + a01=(a01+a33); + a33=casadi_sign(a04); + a33=(a04*a33); + a33=(a33+a19); + a40=(a40*a33); + a01=(a01+a40); + if (res[1]!=0) res[1][34]=a01; + a13=(a13*a03); + a47=(a47*a08); + a13=(a13+a47); + a49=(a49*a27); + a13=(a13+a49); + a50=(a50*a35); + a13=(a13+a50); + a51=(a51*a33); + a13=(a13+a51); + if (res[1]!=0) res[1][35]=a13; + a44=(a44*a03); + a53=(a53*a08); + a44=(a44+a53); + a55=(a55*a27); + a44=(a44+a55); + a56=(a56*a35); + a44=(a44+a56); + a57=(a57*a33); + a44=(a44+a57); + if (res[1]!=0) res[1][36]=a44; + a52=(a52*a03); + a59=(a59*a08); + a52=(a52+a59); + a61=(a61*a27); + a52=(a52+a61); + a62=(a62*a35); + a52=(a52+a62); + a63=(a63*a33); + a52=(a52+a63); + if (res[1]!=0) res[1][37]=a52; + a58=(a58*a03); + a65=(a65*a08); + a58=(a58+a65); + a67=(a67*a27); + a58=(a58+a67); + a68=(a68*a35); + a58=(a58+a68); + a68=(a69*a33); + a58=(a58+a68); + if (res[1]!=0) res[1][38]=a58; + a64=(a64*a03); + a70=(a70*a08); + a64=(a64+a70); + a23=(a23*a27); + a64=(a64+a23); + a69=(a69*a35); + a64=(a64+a69); + a34=(a34*a33); + a64=(a64+a34); + if (res[1]!=0) res[1][39]=a64; + a74=(-a74); + if (res[1]!=0) res[1][40]=a74; + if (res[1]!=0) res[1][41]=a41; + a74=(-a77); + if (res[1]!=0) res[1][42]=a74; + a74=(a71*a02); + a74=(a09*a74); + a71=(a71*a41); + a71=(a04*a71); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][43]=a74; + a74=(a09*a41); + a71=(a04*a02); + a74=(a74+a71); + if (res[1]!=0) res[1][44]=a74; + a74=(a09*a77); + a71=(a04*a75); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][45]=a74; + a74=casadi_sq(a73); + a41=(a41/a74); + a41=(a09*a41); + a02=(a02/a74); + a02=(a04*a02); + a41=(a41+a02); + a41=(-a41); + if (res[1]!=0) res[1][46]=a41; + a75=(a75/a73); + a29=sin(a29); + a75=(a75*a29); + a09=(a09*a75); + a77=(a77/a73); + a77=(a77*a29); + a04=(a04*a77); + a09=(a09+a04); + a09=(-a09); + if (res[1]!=0) res[1][47]=a09; + a09=1.; + if (res[2]!=0) res[2][0]=a09; + if (res[2]!=0) res[2][1]=a09; + if (res[2]!=0) res[2][2]=a09; + if (res[2]!=0) res[2][3]=a09; + if (res[2]!=0) res[2][4]=a09; + if (res[2]!=0) res[2][5]=a09; + if (res[2]!=0) res[2][6]=a09; + if (res[2]!=0) res[2][7]=a09; + if (res[2]!=0) res[2][8]=a09; + a09=-3.3453634479321918e-02; + if (res[3]!=0) res[3][0]=a09; + a09=-6.0368975005218423e-05; + if (res[3]!=0) res[3][1]=a09; + a09=1.1116270017027936e-07; + if (res[3]!=0) res[3][2]=a09; + a09=-9.8084735444364535e-08; + if (res[3]!=0) res[3][3]=a09; + a09=-1.0920100546139210e-05; + if (res[3]!=0) res[3][4]=a09; + a09=6.0150572994295635e-03; + if (res[3]!=0) res[3][5]=a09; + a09=-1.0920100546139184e-05; + if (res[3]!=0) res[3][6]=a09; + a09=-6.0570157695918683e-03; + if (res[3]!=0) res[3][7]=a09; + a09=3.0801331505514837e-03; + if (res[3]!=0) res[3][8]=a09; + a09=-2.7177645446042520e-03; + if (res[3]!=0) res[3][9]=a09; + a09=-3.0257778596593993e-01; + if (res[3]!=0) res[3][10]=a09; + a09=5.4600502730695923e-04; + if (res[3]!=0) res[3][11]=a09; + a04=6.0150572994295574e-03; + if (res[3]!=0) res[3][12]=a04; + a04=3.0184487502609172e-03; + if (res[3]!=0) res[3][13]=a04; + a04=-5.5581350085139543e-06; + if (res[3]!=0) res[3][14]=a04; + a04=4.9042367722181902e-06; + if (res[3]!=0) res[3][15]=a04; + if (res[3]!=0) res[3][16]=a09; + a09=-3.0075286497147785e-01; + if (res[3]!=0) res[3][17]=a09; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_n_out(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s4; + case 2: return casadi_s5; + case 3: return casadi_s6; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 4; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 4*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o new file mode 100644 index 000000000..4f9555c24 Binary files /dev/null and b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u.o differ diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c new file mode 100644 index 000000000..6bb52f036 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.c @@ -0,0 +1,851 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_u_z_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_s7 CASADI_PREFIX(s7) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[24] = + {9, 3, 0, 6, 12, 18, 0, 1, + 2, 3, 4, 5, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5}; +static const casadi_int casadi_s7[3] = {9, 0, 1}; + +/* auv_model_impl_dae_fun_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x3,18nz],o4[9x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; + casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; + casadi_real a72, a73, a74, a75, a76, a77, a78; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a07=(a05*a06); + a08=30.; + a09=arg[0]? arg[0][4] : 0; + a10=(a08*a09); + a11=arg[0]? arg[0][2] : 0; + a12=(a10*a11); + a07=(a07+a12); + a02=(a02-a07); + a07=23.; + a12=arg[0]? arg[0][0] : 0; + a13=casadi_fabs(a12); + a07=(a07+a13); + a13=(a07*a12); + a02=(a02-a13); + a13=(a01*a02); + a14=6.0368975005218254e-05; + a15=(a08*a04); + a16=(a15*a12); + a17=arg[0]? arg[0][3] : 0; + a18=(a03*a17); + a19=(a18*a11); + a16=(a16+a19); + a19=46.; + a20=casadi_fabs(a06); + a20=(a19+a20); + a21=(a20*a06); + a16=(a16+a21); + a21=(a14*a16); + a13=(a13-a21); + a21=-1.1116270017027866e-07; + a22=(a03*a09); + a23=(a22*a12); + a24=(a08*a17); + a25=(a24*a06); + a23=(a23+a25); + a25=casadi_fabs(a11); + a25=(a19+a25); + a26=(a25*a11); + a23=(a23+a26); + a26=(a21*a23); + a13=(a13-a26); + a26=9.8084735444363873e-08; + a27=3.3399999999999999e+00; + a28=(a27*a04); + a29=(a28*a09); + a30=-3.3199999999999998e+00; + a31=(a30*a09); + a32=(a31*a04); + a29=(a29+a32); + a32=casadi_fabs(a17); + a32=(a19+a32); + a33=(a32*a17); + a29=(a29+a33); + a33=(a26*a29); + a13=(a13-a33); + a33=1.0920100546139184e-05; + a34=arg[2]? arg[2][1] : 0; + a35=-3.3399999999999999e+00; + a36=(a35*a04); + a37=(a36*a17); + a38=6.8000000000000005e-01; + a39=(a38*a17); + a40=(a39*a04); + a37=(a37+a40); + a34=(a34-a37); + a37=casadi_fabs(a09); + a37=(a19+a37); + a40=(a37*a09); + a34=(a34-a40); + a40=(a33*a34); + a13=(a13+a40); + a40=-6.0150572994295574e-03; + a41=arg[2]? arg[2][2] : 0; + a42=3.3199999999999998e+00; + a43=(a42*a09); + a44=(a43*a17); + a45=-6.8000000000000005e-01; + a46=(a45*a17); + a47=(a46*a09); + a44=(a44+a47); + a41=(a41-a44); + a44=casadi_fabs(a04); + a19=(a19+a44); + a44=(a19*a04); + a41=(a41-a44); + a44=(a40*a41); + a13=(a13+a44); + a00=(a00-a13); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a13=6.0368975005218423e-05; + a44=(a13*a02); + a47=3.3484658136227786e-02; + a48=(a47*a16); + a44=(a44-a48); + a48=-6.1658244361114702e-05; + a49=(a48*a23); + a44=(a44-a49); + a49=5.4404333259807184e-05; + a50=(a49*a29); + a44=(a44-a50); + a50=6.0570157695918683e-03; + a51=(a50*a34); + a44=(a44+a51); + a51=-3.0184487502609172e-03; + a52=(a51*a41); + a44=(a44+a52); + a00=(a00-a44); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a44=-1.1116270017027936e-07; + a52=(a44*a02); + a53=-6.1658244361114783e-05; + a54=(a53*a16); + a52=(a52-a54); + a54=3.3963490377380855e-02; + a55=(a54*a23); + a52=(a52-a55); + a55=-2.9967785627100754e-02; + a56=(a55*a29); + a52=(a52-a56); + a56=-3.0801331505514837e-03; + a57=(a56*a34); + a52=(a52+a57); + a57=5.5581350085139543e-06; + a58=(a57*a41); + a52=(a52+a58); + a00=(a00-a52); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a52=9.8084735444364535e-08; + a58=(a52*a02); + a59=5.4404333259807062e-05; + a60=(a59*a16); + a58=(a58-a60); + a60=-2.9967785627100469e-02; + a61=(a60*a23); + a58=(a58-a61); + a61=1.4970303990827358e+00; + a62=(a61*a29); + a58=(a58-a62); + a62=2.7177645446042520e-03; + a63=(a62*a34); + a58=(a58+a63); + a63=-4.9042367722181902e-06; + a64=(a63*a41); + a58=(a58+a64); + a00=(a00-a58); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a58=1.0920100546139210e-05; + a64=(a58*a02); + a65=6.0570157695918657e-03; + a66=(a65*a16); + a64=(a64-a66); + a66=-3.0801331505514781e-03; + a67=(a66*a23); + a64=(a64-a67); + a67=2.7177645446042498e-03; + a68=(a67*a29); + a64=(a64-a68); + a68=3.0257778596593993e-01; + a69=(a68*a34); + a64=(a64+a69); + a69=-5.4600502730695923e-04; + a70=(a69*a41); + a64=(a64+a70); + a00=(a00-a64); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a64=-6.0150572994295635e-03; + a02=(a64*a02); + a70=-3.0184487502609133e-03; + a16=(a70*a16); + a02=(a02-a16); + a16=5.5581350085139340e-06; + a23=(a16*a23); + a02=(a02-a23); + a23=-4.9042367722181935e-06; + a29=(a23*a29); + a02=(a02-a29); + a34=(a69*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a41=(a34*a41); + a02=(a02+a41); + a00=(a00-a02); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a02=arg[0]? arg[0][6] : 0; + a41=sin(a02); + a29=arg[0]? arg[0][7] : 0; + a71=tan(a29); + a72=(a41*a71); + a73=(a72*a09); + a73=(a17+a73); + a02=cos(a02); + a74=(a02*a71); + a75=(a74*a04); + a73=(a73+a75); + a00=(a00-a73); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a73=(a02*a09); + a75=(a41*a04); + a73=(a73-a75); + a00=(a00-a73); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a73=cos(a29); + a75=(a41/a73); + a76=(a75*a09); + a77=(a02/a73); + a78=(a77*a04); + a76=(a76+a78); + a00=(a00-a76); + if (res[0]!=0) res[0][8]=a00; + a00=casadi_sign(a12); + a00=(a12*a00); + a00=(a00+a07); + a07=(a01*a00); + a76=(a14*a15); + a07=(a07+a76); + a76=(a21*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][0]=a07; + a07=(a13*a00); + a76=(a47*a15); + a07=(a07+a76); + a76=(a48*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][1]=a07; + a07=(a44*a00); + a76=(a53*a15); + a07=(a07+a76); + a76=(a54*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][2]=a07; + a07=(a52*a00); + a76=(a59*a15); + a07=(a07+a76); + a76=(a60*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][3]=a07; + a07=(a58*a00); + a76=(a65*a15); + a07=(a07+a76); + a76=(a66*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][4]=a07; + a00=(a64*a00); + a15=(a70*a15); + a00=(a00+a15); + a22=(a16*a22); + a00=(a00+a22); + if (res[1]!=0) res[1][5]=a00; + a00=(a01*a05); + a22=casadi_sign(a06); + a22=(a06*a22); + a22=(a22+a20); + a20=(a14*a22); + a00=(a00+a20); + a20=(a21*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][6]=a00; + a00=(a13*a05); + a20=(a47*a22); + a00=(a00+a20); + a20=(a48*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][7]=a00; + a00=(a44*a05); + a20=(a53*a22); + a00=(a00+a20); + a20=(a54*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][8]=a00; + a00=(a52*a05); + a20=(a59*a22); + a00=(a00+a20); + a20=(a60*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][9]=a00; + a00=(a58*a05); + a20=(a65*a22); + a00=(a00+a20); + a20=(a66*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][10]=a00; + a05=(a64*a05); + a22=(a70*a22); + a05=(a05+a22); + a24=(a16*a24); + a05=(a05+a24); + if (res[1]!=0) res[1][11]=a05; + a05=(a01*a10); + a24=(a14*a18); + a05=(a05+a24); + a24=casadi_sign(a11); + a24=(a11*a24); + a24=(a24+a25); + a25=(a21*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][12]=a05; + a05=(a13*a10); + a25=(a47*a18); + a05=(a05+a25); + a25=(a48*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][13]=a05; + a05=(a44*a10); + a25=(a53*a18); + a05=(a05+a25); + a25=(a54*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][14]=a05; + a05=(a52*a10); + a25=(a59*a18); + a05=(a05+a25); + a25=(a60*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][15]=a05; + a05=(a58*a10); + a25=(a65*a18); + a05=(a05+a25); + a25=(a66*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][16]=a05; + a10=(a64*a10); + a18=(a70*a18); + a10=(a10+a18); + a24=(a16*a24); + a10=(a10+a24); + if (res[1]!=0) res[1][17]=a10; + a10=(a03*a11); + a24=(a14*a10); + a18=(a08*a06); + a05=(a21*a18); + a24=(a24+a05); + a05=casadi_sign(a17); + a05=(a17*a05); + a05=(a05+a32); + a32=(a26*a05); + a24=(a24+a32); + a38=(a38*a04); + a36=(a36+a38); + a38=(a33*a36); + a24=(a24+a38); + a45=(a45*a09); + a43=(a43+a45); + a45=(a40*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][18]=a24; + a24=(a47*a10); + a45=(a48*a18); + a24=(a24+a45); + a45=(a49*a05); + a24=(a24+a45); + a45=(a50*a36); + a24=(a24+a45); + a45=(a51*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][19]=a24; + a24=(a53*a10); + a45=(a54*a18); + a24=(a24+a45); + a45=(a55*a05); + a24=(a24+a45); + a45=(a56*a36); + a24=(a24+a45); + a45=(a57*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][20]=a24; + a24=(a59*a10); + a45=(a60*a18); + a24=(a24+a45); + a45=(a61*a05); + a24=(a24+a45); + a45=(a62*a36); + a24=(a24+a45); + a45=(a63*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][21]=a24; + a24=(a65*a10); + a45=(a66*a18); + a24=(a24+a45); + a45=(a67*a05); + a24=(a24+a45); + a45=(a68*a36); + a24=(a24+a45); + a45=(a69*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][22]=a24; + a10=(a70*a10); + a18=(a16*a18); + a10=(a10+a18); + a05=(a23*a05); + a10=(a10+a05); + a36=(a69*a36); + a10=(a10+a36); + a43=(a34*a43); + a10=(a10+a43); + if (res[1]!=0) res[1][23]=a10; + a10=-1.; + if (res[1]!=0) res[1][24]=a10; + a11=(a08*a11); + a10=(a01*a11); + a43=(a03*a12); + a21=(a21*a43); + a10=(a10+a21); + a30=(a30*a04); + a28=(a28+a30); + a30=(a26*a28); + a10=(a10+a30); + a30=casadi_sign(a09); + a30=(a09*a30); + a30=(a30+a37); + a37=(a33*a30); + a10=(a10+a37); + a42=(a42*a17); + a42=(a42+a46); + a46=(a40*a42); + a10=(a10+a46); + if (res[1]!=0) res[1][25]=a10; + a10=(a13*a11); + a48=(a48*a43); + a10=(a10+a48); + a48=(a49*a28); + a10=(a10+a48); + a48=(a50*a30); + a10=(a10+a48); + a48=(a51*a42); + a10=(a10+a48); + if (res[1]!=0) res[1][26]=a10; + a10=(a44*a11); + a54=(a54*a43); + a10=(a10+a54); + a54=(a55*a28); + a10=(a10+a54); + a54=(a56*a30); + a10=(a10+a54); + a54=(a57*a42); + a10=(a10+a54); + if (res[1]!=0) res[1][27]=a10; + a10=(a52*a11); + a60=(a60*a43); + a10=(a10+a60); + a60=(a61*a28); + a10=(a10+a60); + a60=(a62*a30); + a10=(a10+a60); + a60=(a63*a42); + a10=(a10+a60); + if (res[1]!=0) res[1][28]=a10; + a10=(a58*a11); + a66=(a66*a43); + a10=(a10+a66); + a66=(a67*a28); + a10=(a10+a66); + a66=(a68*a30); + a10=(a10+a66); + a66=(a69*a42); + a10=(a10+a66); + if (res[1]!=0) res[1][29]=a10; + a11=(a64*a11); + a16=(a16*a43); + a11=(a11+a16); + a28=(a23*a28); + a11=(a11+a28); + a30=(a69*a30); + a11=(a11+a30); + a42=(a34*a42); + a11=(a11+a42); + if (res[1]!=0) res[1][30]=a11; + a72=(-a72); + if (res[1]!=0) res[1][31]=a72; + a72=(-a02); + if (res[1]!=0) res[1][32]=a72; + a72=(-a75); + if (res[1]!=0) res[1][33]=a72; + a03=(a03*a06); + a01=(a01*a03); + a08=(a08*a12); + a14=(a14*a08); + a01=(a01+a14); + a27=(a27*a09); + a27=(a27+a31); + a26=(a26*a27); + a01=(a01+a26); + a35=(a35*a17); + a35=(a35+a39); + a33=(a33*a35); + a01=(a01+a33); + a33=casadi_sign(a04); + a33=(a04*a33); + a33=(a33+a19); + a40=(a40*a33); + a01=(a01+a40); + if (res[1]!=0) res[1][34]=a01; + a13=(a13*a03); + a47=(a47*a08); + a13=(a13+a47); + a49=(a49*a27); + a13=(a13+a49); + a50=(a50*a35); + a13=(a13+a50); + a51=(a51*a33); + a13=(a13+a51); + if (res[1]!=0) res[1][35]=a13; + a44=(a44*a03); + a53=(a53*a08); + a44=(a44+a53); + a55=(a55*a27); + a44=(a44+a55); + a56=(a56*a35); + a44=(a44+a56); + a57=(a57*a33); + a44=(a44+a57); + if (res[1]!=0) res[1][36]=a44; + a52=(a52*a03); + a59=(a59*a08); + a52=(a52+a59); + a61=(a61*a27); + a52=(a52+a61); + a62=(a62*a35); + a52=(a52+a62); + a63=(a63*a33); + a52=(a52+a63); + if (res[1]!=0) res[1][37]=a52; + a58=(a58*a03); + a65=(a65*a08); + a58=(a58+a65); + a67=(a67*a27); + a58=(a58+a67); + a68=(a68*a35); + a58=(a58+a68); + a68=(a69*a33); + a58=(a58+a68); + if (res[1]!=0) res[1][38]=a58; + a64=(a64*a03); + a70=(a70*a08); + a64=(a64+a70); + a23=(a23*a27); + a64=(a64+a23); + a69=(a69*a35); + a64=(a64+a69); + a34=(a34*a33); + a64=(a64+a34); + if (res[1]!=0) res[1][39]=a64; + a74=(-a74); + if (res[1]!=0) res[1][40]=a74; + if (res[1]!=0) res[1][41]=a41; + a74=(-a77); + if (res[1]!=0) res[1][42]=a74; + a74=(a71*a02); + a74=(a09*a74); + a71=(a71*a41); + a71=(a04*a71); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][43]=a74; + a74=(a09*a41); + a71=(a04*a02); + a74=(a74+a71); + if (res[1]!=0) res[1][44]=a74; + a74=(a09*a77); + a71=(a04*a75); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][45]=a74; + a74=casadi_sq(a73); + a41=(a41/a74); + a41=(a09*a41); + a02=(a02/a74); + a02=(a04*a02); + a41=(a41+a02); + a41=(-a41); + if (res[1]!=0) res[1][46]=a41; + a75=(a75/a73); + a29=sin(a29); + a75=(a75*a29); + a09=(a09*a75); + a77=(a77/a73); + a77=(a77*a29); + a04=(a04*a77); + a09=(a09+a04); + a09=(-a09); + if (res[1]!=0) res[1][47]=a09; + a09=1.; + if (res[2]!=0) res[2][0]=a09; + if (res[2]!=0) res[2][1]=a09; + if (res[2]!=0) res[2][2]=a09; + if (res[2]!=0) res[2][3]=a09; + if (res[2]!=0) res[2][4]=a09; + if (res[2]!=0) res[2][5]=a09; + if (res[2]!=0) res[2][6]=a09; + if (res[2]!=0) res[2][7]=a09; + if (res[2]!=0) res[2][8]=a09; + a09=-3.3453634479321918e-02; + if (res[3]!=0) res[3][0]=a09; + a09=-6.0368975005218423e-05; + if (res[3]!=0) res[3][1]=a09; + a09=1.1116270017027936e-07; + if (res[3]!=0) res[3][2]=a09; + a09=-9.8084735444364535e-08; + if (res[3]!=0) res[3][3]=a09; + a09=-1.0920100546139210e-05; + if (res[3]!=0) res[3][4]=a09; + a09=6.0150572994295635e-03; + if (res[3]!=0) res[3][5]=a09; + a09=-1.0920100546139184e-05; + if (res[3]!=0) res[3][6]=a09; + a09=-6.0570157695918683e-03; + if (res[3]!=0) res[3][7]=a09; + a09=3.0801331505514837e-03; + if (res[3]!=0) res[3][8]=a09; + a09=-2.7177645446042520e-03; + if (res[3]!=0) res[3][9]=a09; + a09=-3.0257778596593993e-01; + if (res[3]!=0) res[3][10]=a09; + a09=5.4600502730695923e-04; + if (res[3]!=0) res[3][11]=a09; + a04=6.0150572994295574e-03; + if (res[3]!=0) res[3][12]=a04; + a04=3.0184487502609172e-03; + if (res[3]!=0) res[3][13]=a04; + a04=-5.5581350085139543e-06; + if (res[3]!=0) res[3][14]=a04; + a04=4.9042367722181902e-06; + if (res[3]!=0) res[3][15]=a04; + if (res[3]!=0) res[3][16]=a09; + a09=-3.0075286497147785e-01; + if (res[3]!=0) res[3][17]=a09; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_u_z_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_u_z_n_out(void) { return 5;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_u_z_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_u_z_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + case 4: return "o4"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_u_z_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s4; + case 2: return casadi_s5; + case 3: return casadi_s6; + case 4: return casadi_s7; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 5; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 5*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o new file mode 100644 index 000000000..dc3e918bb Binary files /dev/null and b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_u_z.o differ diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c new file mode 100644 index 000000000..7cbf6b868 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.c @@ -0,0 +1,809 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_fun_jac_x_xdot_z_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[3] = {9, 0, 1}; + +/* auv_model_impl_dae_fun_jac_x_xdot_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9],o1[9x9,48nz],o2[9x9,9nz],o3[9x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59; + casadi_real a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a70, a71; + casadi_real a72, a73, a74, a75, a76, a77, a78; + a00=arg[1]? arg[1][0] : 0; + a01=3.3453634479321918e-02; + a02=arg[2]? arg[2][0] : 0; + a03=-30.; + a04=arg[0]? arg[0][5] : 0; + a05=(a03*a04); + a06=arg[0]? arg[0][1] : 0; + a07=(a05*a06); + a08=30.; + a09=arg[0]? arg[0][4] : 0; + a10=(a08*a09); + a11=arg[0]? arg[0][2] : 0; + a12=(a10*a11); + a07=(a07+a12); + a02=(a02-a07); + a07=23.; + a12=arg[0]? arg[0][0] : 0; + a13=casadi_fabs(a12); + a07=(a07+a13); + a13=(a07*a12); + a02=(a02-a13); + a13=(a01*a02); + a14=6.0368975005218254e-05; + a15=(a08*a04); + a16=(a15*a12); + a17=arg[0]? arg[0][3] : 0; + a18=(a03*a17); + a19=(a18*a11); + a16=(a16+a19); + a19=46.; + a20=casadi_fabs(a06); + a20=(a19+a20); + a21=(a20*a06); + a16=(a16+a21); + a21=(a14*a16); + a13=(a13-a21); + a21=-1.1116270017027866e-07; + a22=(a03*a09); + a23=(a22*a12); + a24=(a08*a17); + a25=(a24*a06); + a23=(a23+a25); + a25=casadi_fabs(a11); + a25=(a19+a25); + a26=(a25*a11); + a23=(a23+a26); + a26=(a21*a23); + a13=(a13-a26); + a26=9.8084735444363873e-08; + a27=3.3399999999999999e+00; + a28=(a27*a04); + a29=(a28*a09); + a30=-3.3199999999999998e+00; + a31=(a30*a09); + a32=(a31*a04); + a29=(a29+a32); + a32=casadi_fabs(a17); + a32=(a19+a32); + a33=(a32*a17); + a29=(a29+a33); + a33=(a26*a29); + a13=(a13-a33); + a33=1.0920100546139184e-05; + a34=arg[2]? arg[2][1] : 0; + a35=-3.3399999999999999e+00; + a36=(a35*a04); + a37=(a36*a17); + a38=6.8000000000000005e-01; + a39=(a38*a17); + a40=(a39*a04); + a37=(a37+a40); + a34=(a34-a37); + a37=casadi_fabs(a09); + a37=(a19+a37); + a40=(a37*a09); + a34=(a34-a40); + a40=(a33*a34); + a13=(a13+a40); + a40=-6.0150572994295574e-03; + a41=arg[2]? arg[2][2] : 0; + a42=3.3199999999999998e+00; + a43=(a42*a09); + a44=(a43*a17); + a45=-6.8000000000000005e-01; + a46=(a45*a17); + a47=(a46*a09); + a44=(a44+a47); + a41=(a41-a44); + a44=casadi_fabs(a04); + a19=(a19+a44); + a44=(a19*a04); + a41=(a41-a44); + a44=(a40*a41); + a13=(a13+a44); + a00=(a00-a13); + if (res[0]!=0) res[0][0]=a00; + a00=arg[1]? arg[1][1] : 0; + a13=6.0368975005218423e-05; + a44=(a13*a02); + a47=3.3484658136227786e-02; + a48=(a47*a16); + a44=(a44-a48); + a48=-6.1658244361114702e-05; + a49=(a48*a23); + a44=(a44-a49); + a49=5.4404333259807184e-05; + a50=(a49*a29); + a44=(a44-a50); + a50=6.0570157695918683e-03; + a51=(a50*a34); + a44=(a44+a51); + a51=-3.0184487502609172e-03; + a52=(a51*a41); + a44=(a44+a52); + a00=(a00-a44); + if (res[0]!=0) res[0][1]=a00; + a00=arg[1]? arg[1][2] : 0; + a44=-1.1116270017027936e-07; + a52=(a44*a02); + a53=-6.1658244361114783e-05; + a54=(a53*a16); + a52=(a52-a54); + a54=3.3963490377380855e-02; + a55=(a54*a23); + a52=(a52-a55); + a55=-2.9967785627100754e-02; + a56=(a55*a29); + a52=(a52-a56); + a56=-3.0801331505514837e-03; + a57=(a56*a34); + a52=(a52+a57); + a57=5.5581350085139543e-06; + a58=(a57*a41); + a52=(a52+a58); + a00=(a00-a52); + if (res[0]!=0) res[0][2]=a00; + a00=arg[1]? arg[1][3] : 0; + a52=9.8084735444364535e-08; + a58=(a52*a02); + a59=5.4404333259807062e-05; + a60=(a59*a16); + a58=(a58-a60); + a60=-2.9967785627100469e-02; + a61=(a60*a23); + a58=(a58-a61); + a61=1.4970303990827358e+00; + a62=(a61*a29); + a58=(a58-a62); + a62=2.7177645446042520e-03; + a63=(a62*a34); + a58=(a58+a63); + a63=-4.9042367722181902e-06; + a64=(a63*a41); + a58=(a58+a64); + a00=(a00-a58); + if (res[0]!=0) res[0][3]=a00; + a00=arg[1]? arg[1][4] : 0; + a58=1.0920100546139210e-05; + a64=(a58*a02); + a65=6.0570157695918657e-03; + a66=(a65*a16); + a64=(a64-a66); + a66=-3.0801331505514781e-03; + a67=(a66*a23); + a64=(a64-a67); + a67=2.7177645446042498e-03; + a68=(a67*a29); + a64=(a64-a68); + a68=3.0257778596593993e-01; + a69=(a68*a34); + a64=(a64+a69); + a69=-5.4600502730695923e-04; + a70=(a69*a41); + a64=(a64+a70); + a00=(a00-a64); + if (res[0]!=0) res[0][4]=a00; + a00=arg[1]? arg[1][5] : 0; + a64=-6.0150572994295635e-03; + a02=(a64*a02); + a70=-3.0184487502609133e-03; + a16=(a70*a16); + a02=(a02-a16); + a16=5.5581350085139340e-06; + a23=(a16*a23); + a02=(a02-a23); + a23=-4.9042367722181935e-06; + a29=(a23*a29); + a02=(a02-a29); + a34=(a69*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a41=(a34*a41); + a02=(a02+a41); + a00=(a00-a02); + if (res[0]!=0) res[0][5]=a00; + a00=arg[1]? arg[1][6] : 0; + a02=arg[0]? arg[0][6] : 0; + a41=sin(a02); + a29=arg[0]? arg[0][7] : 0; + a71=tan(a29); + a72=(a41*a71); + a73=(a72*a09); + a73=(a17+a73); + a02=cos(a02); + a74=(a02*a71); + a75=(a74*a04); + a73=(a73+a75); + a00=(a00-a73); + if (res[0]!=0) res[0][6]=a00; + a00=arg[1]? arg[1][7] : 0; + a73=(a02*a09); + a75=(a41*a04); + a73=(a73-a75); + a00=(a00-a73); + if (res[0]!=0) res[0][7]=a00; + a00=arg[1]? arg[1][8] : 0; + a73=cos(a29); + a75=(a41/a73); + a76=(a75*a09); + a77=(a02/a73); + a78=(a77*a04); + a76=(a76+a78); + a00=(a00-a76); + if (res[0]!=0) res[0][8]=a00; + a00=casadi_sign(a12); + a00=(a12*a00); + a00=(a00+a07); + a07=(a01*a00); + a76=(a14*a15); + a07=(a07+a76); + a76=(a21*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][0]=a07; + a07=(a13*a00); + a76=(a47*a15); + a07=(a07+a76); + a76=(a48*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][1]=a07; + a07=(a44*a00); + a76=(a53*a15); + a07=(a07+a76); + a76=(a54*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][2]=a07; + a07=(a52*a00); + a76=(a59*a15); + a07=(a07+a76); + a76=(a60*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][3]=a07; + a07=(a58*a00); + a76=(a65*a15); + a07=(a07+a76); + a76=(a66*a22); + a07=(a07+a76); + if (res[1]!=0) res[1][4]=a07; + a00=(a64*a00); + a15=(a70*a15); + a00=(a00+a15); + a22=(a16*a22); + a00=(a00+a22); + if (res[1]!=0) res[1][5]=a00; + a00=(a01*a05); + a22=casadi_sign(a06); + a22=(a06*a22); + a22=(a22+a20); + a20=(a14*a22); + a00=(a00+a20); + a20=(a21*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][6]=a00; + a00=(a13*a05); + a20=(a47*a22); + a00=(a00+a20); + a20=(a48*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][7]=a00; + a00=(a44*a05); + a20=(a53*a22); + a00=(a00+a20); + a20=(a54*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][8]=a00; + a00=(a52*a05); + a20=(a59*a22); + a00=(a00+a20); + a20=(a60*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][9]=a00; + a00=(a58*a05); + a20=(a65*a22); + a00=(a00+a20); + a20=(a66*a24); + a00=(a00+a20); + if (res[1]!=0) res[1][10]=a00; + a05=(a64*a05); + a22=(a70*a22); + a05=(a05+a22); + a24=(a16*a24); + a05=(a05+a24); + if (res[1]!=0) res[1][11]=a05; + a05=(a01*a10); + a24=(a14*a18); + a05=(a05+a24); + a24=casadi_sign(a11); + a24=(a11*a24); + a24=(a24+a25); + a25=(a21*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][12]=a05; + a05=(a13*a10); + a25=(a47*a18); + a05=(a05+a25); + a25=(a48*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][13]=a05; + a05=(a44*a10); + a25=(a53*a18); + a05=(a05+a25); + a25=(a54*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][14]=a05; + a05=(a52*a10); + a25=(a59*a18); + a05=(a05+a25); + a25=(a60*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][15]=a05; + a05=(a58*a10); + a25=(a65*a18); + a05=(a05+a25); + a25=(a66*a24); + a05=(a05+a25); + if (res[1]!=0) res[1][16]=a05; + a10=(a64*a10); + a18=(a70*a18); + a10=(a10+a18); + a24=(a16*a24); + a10=(a10+a24); + if (res[1]!=0) res[1][17]=a10; + a10=(a03*a11); + a24=(a14*a10); + a18=(a08*a06); + a05=(a21*a18); + a24=(a24+a05); + a05=casadi_sign(a17); + a05=(a17*a05); + a05=(a05+a32); + a32=(a26*a05); + a24=(a24+a32); + a38=(a38*a04); + a36=(a36+a38); + a38=(a33*a36); + a24=(a24+a38); + a45=(a45*a09); + a43=(a43+a45); + a45=(a40*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][18]=a24; + a24=(a47*a10); + a45=(a48*a18); + a24=(a24+a45); + a45=(a49*a05); + a24=(a24+a45); + a45=(a50*a36); + a24=(a24+a45); + a45=(a51*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][19]=a24; + a24=(a53*a10); + a45=(a54*a18); + a24=(a24+a45); + a45=(a55*a05); + a24=(a24+a45); + a45=(a56*a36); + a24=(a24+a45); + a45=(a57*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][20]=a24; + a24=(a59*a10); + a45=(a60*a18); + a24=(a24+a45); + a45=(a61*a05); + a24=(a24+a45); + a45=(a62*a36); + a24=(a24+a45); + a45=(a63*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][21]=a24; + a24=(a65*a10); + a45=(a66*a18); + a24=(a24+a45); + a45=(a67*a05); + a24=(a24+a45); + a45=(a68*a36); + a24=(a24+a45); + a45=(a69*a43); + a24=(a24+a45); + if (res[1]!=0) res[1][22]=a24; + a10=(a70*a10); + a18=(a16*a18); + a10=(a10+a18); + a05=(a23*a05); + a10=(a10+a05); + a36=(a69*a36); + a10=(a10+a36); + a43=(a34*a43); + a10=(a10+a43); + if (res[1]!=0) res[1][23]=a10; + a10=-1.; + if (res[1]!=0) res[1][24]=a10; + a11=(a08*a11); + a10=(a01*a11); + a43=(a03*a12); + a21=(a21*a43); + a10=(a10+a21); + a30=(a30*a04); + a28=(a28+a30); + a30=(a26*a28); + a10=(a10+a30); + a30=casadi_sign(a09); + a30=(a09*a30); + a30=(a30+a37); + a37=(a33*a30); + a10=(a10+a37); + a42=(a42*a17); + a42=(a42+a46); + a46=(a40*a42); + a10=(a10+a46); + if (res[1]!=0) res[1][25]=a10; + a10=(a13*a11); + a48=(a48*a43); + a10=(a10+a48); + a48=(a49*a28); + a10=(a10+a48); + a48=(a50*a30); + a10=(a10+a48); + a48=(a51*a42); + a10=(a10+a48); + if (res[1]!=0) res[1][26]=a10; + a10=(a44*a11); + a54=(a54*a43); + a10=(a10+a54); + a54=(a55*a28); + a10=(a10+a54); + a54=(a56*a30); + a10=(a10+a54); + a54=(a57*a42); + a10=(a10+a54); + if (res[1]!=0) res[1][27]=a10; + a10=(a52*a11); + a60=(a60*a43); + a10=(a10+a60); + a60=(a61*a28); + a10=(a10+a60); + a60=(a62*a30); + a10=(a10+a60); + a60=(a63*a42); + a10=(a10+a60); + if (res[1]!=0) res[1][28]=a10; + a10=(a58*a11); + a66=(a66*a43); + a10=(a10+a66); + a66=(a67*a28); + a10=(a10+a66); + a66=(a68*a30); + a10=(a10+a66); + a66=(a69*a42); + a10=(a10+a66); + if (res[1]!=0) res[1][29]=a10; + a11=(a64*a11); + a16=(a16*a43); + a11=(a11+a16); + a28=(a23*a28); + a11=(a11+a28); + a30=(a69*a30); + a11=(a11+a30); + a42=(a34*a42); + a11=(a11+a42); + if (res[1]!=0) res[1][30]=a11; + a72=(-a72); + if (res[1]!=0) res[1][31]=a72; + a72=(-a02); + if (res[1]!=0) res[1][32]=a72; + a72=(-a75); + if (res[1]!=0) res[1][33]=a72; + a03=(a03*a06); + a01=(a01*a03); + a08=(a08*a12); + a14=(a14*a08); + a01=(a01+a14); + a27=(a27*a09); + a27=(a27+a31); + a26=(a26*a27); + a01=(a01+a26); + a35=(a35*a17); + a35=(a35+a39); + a33=(a33*a35); + a01=(a01+a33); + a33=casadi_sign(a04); + a33=(a04*a33); + a33=(a33+a19); + a40=(a40*a33); + a01=(a01+a40); + if (res[1]!=0) res[1][34]=a01; + a13=(a13*a03); + a47=(a47*a08); + a13=(a13+a47); + a49=(a49*a27); + a13=(a13+a49); + a50=(a50*a35); + a13=(a13+a50); + a51=(a51*a33); + a13=(a13+a51); + if (res[1]!=0) res[1][35]=a13; + a44=(a44*a03); + a53=(a53*a08); + a44=(a44+a53); + a55=(a55*a27); + a44=(a44+a55); + a56=(a56*a35); + a44=(a44+a56); + a57=(a57*a33); + a44=(a44+a57); + if (res[1]!=0) res[1][36]=a44; + a52=(a52*a03); + a59=(a59*a08); + a52=(a52+a59); + a61=(a61*a27); + a52=(a52+a61); + a62=(a62*a35); + a52=(a52+a62); + a63=(a63*a33); + a52=(a52+a63); + if (res[1]!=0) res[1][37]=a52; + a58=(a58*a03); + a65=(a65*a08); + a58=(a58+a65); + a67=(a67*a27); + a58=(a58+a67); + a68=(a68*a35); + a58=(a58+a68); + a68=(a69*a33); + a58=(a58+a68); + if (res[1]!=0) res[1][38]=a58; + a64=(a64*a03); + a70=(a70*a08); + a64=(a64+a70); + a23=(a23*a27); + a64=(a64+a23); + a69=(a69*a35); + a64=(a64+a69); + a34=(a34*a33); + a64=(a64+a34); + if (res[1]!=0) res[1][39]=a64; + a74=(-a74); + if (res[1]!=0) res[1][40]=a74; + if (res[1]!=0) res[1][41]=a41; + a74=(-a77); + if (res[1]!=0) res[1][42]=a74; + a74=(a71*a02); + a74=(a09*a74); + a71=(a71*a41); + a71=(a04*a71); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][43]=a74; + a74=(a09*a41); + a71=(a04*a02); + a74=(a74+a71); + if (res[1]!=0) res[1][44]=a74; + a74=(a09*a77); + a71=(a04*a75); + a74=(a74-a71); + a74=(-a74); + if (res[1]!=0) res[1][45]=a74; + a74=casadi_sq(a73); + a41=(a41/a74); + a41=(a09*a41); + a02=(a02/a74); + a02=(a04*a02); + a41=(a41+a02); + a41=(-a41); + if (res[1]!=0) res[1][46]=a41; + a75=(a75/a73); + a29=sin(a29); + a75=(a75*a29); + a09=(a09*a75); + a77=(a77/a73); + a77=(a77*a29); + a04=(a04*a77); + a09=(a09+a04); + a09=(-a09); + if (res[1]!=0) res[1][47]=a09; + a09=1.; + if (res[2]!=0) res[2][0]=a09; + if (res[2]!=0) res[2][1]=a09; + if (res[2]!=0) res[2][2]=a09; + if (res[2]!=0) res[2][3]=a09; + if (res[2]!=0) res[2][4]=a09; + if (res[2]!=0) res[2][5]=a09; + if (res[2]!=0) res[2][6]=a09; + if (res[2]!=0) res[2][7]=a09; + if (res[2]!=0) res[2][8]=a09; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_fun_jac_x_xdot_z_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_fun_jac_x_xdot_z_n_out(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_fun_jac_x_xdot_z_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_fun_jac_x_xdot_z_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s4; + case 2: return casadi_s5; + case 3: return casadi_s6; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 4; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_fun_jac_x_xdot_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 4*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o new file mode 100644 index 000000000..1ffd6fd57 Binary files /dev/null and b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_fun_jac_x_xdot_z.o differ diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c new file mode 100644 index 000000000..6c3f66037 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.c @@ -0,0 +1,708 @@ +/* This file was automatically generated by CasADi 3.7.2. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifdef __cplusplus +extern "C" { +#endif + +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) auv_model_impl_dae_jac_x_xdot_u_z_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_fabs CASADI_PREFIX(fabs) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_s6 CASADI_PREFIX(s6) +#define casadi_s7 CASADI_PREFIX(s7) +#define casadi_sign CASADI_PREFIX(sign) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_sign(casadi_real x) { return x<0 ? -1 : x>0 ? 1 : x;} + +casadi_real casadi_fabs(casadi_real x) { +/* Pre-c99 compatibility */ +#if __STDC_VERSION__ < 199901L + return x>0 ? x : -x; +#else + return fabs(x); +#endif +} + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[3] = {9, 1, 1}; +static const casadi_int casadi_s1[3] = {3, 1, 1}; +static const casadi_int casadi_s2[3] = {0, 1, 1}; +static const casadi_int casadi_s3[3] = {0, 0, 1}; +static const casadi_int casadi_s4[60] = + {9, 9, 0, 6, 12, 18, 25, 34, + 43, 46, 48, 48, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5, + 0, 1, 2, 3, 4, 5, 0, 1, + 2, 3, 4, 5, 6, 0, 1, 2, + 3, 4, 5, 6, 7, 8, 0, 1, + 2, 3, 4, 5, 6, 7, 8, 6, + 7, 8, 6, 8}; +static const casadi_int casadi_s5[21] = + {9, 9, 0, 1, 2, 3, 4, 5, + 6, 7, 8, 9, 0, 1, 2, 3, + 4, 5, 6, 7, 8}; +static const casadi_int casadi_s6[24] = + {9, 3, 0, 6, 12, 18, 0, 1, + 2, 3, 4, 5, 0, 1, 2, 3, + 4, 5, 0, 1, 2, 3, 4, 5}; +static const casadi_int casadi_s7[3] = {9, 0, 1}; + +/* auv_model_impl_dae_jac_x_xdot_u_z:(i0[9],i1[9],i2[3],i3[0],i4[],i5[0])->(o0[9x9,48nz],o1[9x9,9nz],o2[9x3,18nz],o3[9x0]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a00, a01, a02, a03, a04, a05, a06, a07, a08, a09, a10, a11; + casadi_real a12, a13, a14, a15, a16, a17, a18, a19, a20, a21, a22, a23; + casadi_real a24, a25, a26, a27, a28, a29, a30, a31, a32, a33, a34, a35; + casadi_real a36, a37, a38, a39, a40, a41, a42, a43, a44, a45, a46, a47; + casadi_real a48, a49, a50, a51, a52, a53; + a00=3.3453634479321918e-02; + a01=arg[0]? arg[0][0] : 0; + a02=casadi_sign(a01); + a02=(a01*a02); + a03=23.; + a04=casadi_fabs(a01); + a03=(a03+a04); + a02=(a02+a03); + a03=(a00*a02); + a04=6.0368975005218254e-05; + a05=30.; + a06=arg[0]? arg[0][5] : 0; + a07=(a05*a06); + a08=(a04*a07); + a03=(a03+a08); + a08=-1.1116270017027866e-07; + a09=-30.; + a10=arg[0]? arg[0][4] : 0; + a11=(a09*a10); + a12=(a08*a11); + a03=(a03+a12); + if (res[0]!=0) res[0][0]=a03; + a03=6.0368975005218423e-05; + a12=(a03*a02); + a13=3.3484658136227786e-02; + a14=(a13*a07); + a12=(a12+a14); + a14=-6.1658244361114702e-05; + a15=(a14*a11); + a12=(a12+a15); + if (res[0]!=0) res[0][1]=a12; + a12=-1.1116270017027936e-07; + a15=(a12*a02); + a16=-6.1658244361114783e-05; + a17=(a16*a07); + a15=(a15+a17); + a17=3.3963490377380855e-02; + a18=(a17*a11); + a15=(a15+a18); + if (res[0]!=0) res[0][2]=a15; + a15=9.8084735444364535e-08; + a18=(a15*a02); + a19=5.4404333259807062e-05; + a20=(a19*a07); + a18=(a18+a20); + a20=-2.9967785627100469e-02; + a21=(a20*a11); + a18=(a18+a21); + if (res[0]!=0) res[0][3]=a18; + a18=1.0920100546139210e-05; + a21=(a18*a02); + a22=6.0570157695918657e-03; + a23=(a22*a07); + a21=(a21+a23); + a23=-3.0801331505514781e-03; + a24=(a23*a11); + a21=(a21+a24); + if (res[0]!=0) res[0][4]=a21; + a21=-6.0150572994295635e-03; + a02=(a21*a02); + a24=-3.0184487502609133e-03; + a07=(a24*a07); + a02=(a02+a07); + a07=5.5581350085139340e-06; + a11=(a07*a11); + a02=(a02+a11); + if (res[0]!=0) res[0][5]=a02; + a02=(a09*a06); + a11=(a00*a02); + a25=arg[0]? arg[0][1] : 0; + a26=casadi_sign(a25); + a26=(a25*a26); + a27=46.; + a28=casadi_fabs(a25); + a28=(a27+a28); + a26=(a26+a28); + a28=(a04*a26); + a11=(a11+a28); + a28=arg[0]? arg[0][3] : 0; + a29=(a05*a28); + a30=(a08*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][6]=a11; + a11=(a03*a02); + a30=(a13*a26); + a11=(a11+a30); + a30=(a14*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][7]=a11; + a11=(a12*a02); + a30=(a16*a26); + a11=(a11+a30); + a30=(a17*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][8]=a11; + a11=(a15*a02); + a30=(a19*a26); + a11=(a11+a30); + a30=(a20*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][9]=a11; + a11=(a18*a02); + a30=(a22*a26); + a11=(a11+a30); + a30=(a23*a29); + a11=(a11+a30); + if (res[0]!=0) res[0][10]=a11; + a02=(a21*a02); + a26=(a24*a26); + a02=(a02+a26); + a29=(a07*a29); + a02=(a02+a29); + if (res[0]!=0) res[0][11]=a02; + a02=(a05*a10); + a29=(a00*a02); + a26=(a09*a28); + a11=(a04*a26); + a29=(a29+a11); + a11=arg[0]? arg[0][2] : 0; + a30=casadi_sign(a11); + a30=(a11*a30); + a31=casadi_fabs(a11); + a31=(a27+a31); + a30=(a30+a31); + a31=(a08*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][12]=a29; + a29=(a03*a02); + a31=(a13*a26); + a29=(a29+a31); + a31=(a14*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][13]=a29; + a29=(a12*a02); + a31=(a16*a26); + a29=(a29+a31); + a31=(a17*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][14]=a29; + a29=(a15*a02); + a31=(a19*a26); + a29=(a29+a31); + a31=(a20*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][15]=a29; + a29=(a18*a02); + a31=(a22*a26); + a29=(a29+a31); + a31=(a23*a30); + a29=(a29+a31); + if (res[0]!=0) res[0][16]=a29; + a02=(a21*a02); + a26=(a24*a26); + a02=(a02+a26); + a30=(a07*a30); + a02=(a02+a30); + if (res[0]!=0) res[0][17]=a02; + a02=(a09*a11); + a30=(a04*a02); + a26=(a05*a25); + a29=(a08*a26); + a30=(a30+a29); + a29=9.8084735444363873e-08; + a31=casadi_sign(a28); + a31=(a28*a31); + a32=casadi_fabs(a28); + a32=(a27+a32); + a31=(a31+a32); + a32=(a29*a31); + a30=(a30+a32); + a32=1.0920100546139184e-05; + a33=-3.3399999999999999e+00; + a34=(a33*a06); + a35=6.8000000000000005e-01; + a36=(a35*a06); + a34=(a34+a36); + a36=(a32*a34); + a30=(a30+a36); + a36=-6.0150572994295574e-03; + a37=3.3199999999999998e+00; + a38=(a37*a10); + a39=-6.8000000000000005e-01; + a40=(a39*a10); + a38=(a38+a40); + a40=(a36*a38); + a30=(a30+a40); + if (res[0]!=0) res[0][18]=a30; + a30=(a13*a02); + a40=(a14*a26); + a30=(a30+a40); + a40=5.4404333259807184e-05; + a41=(a40*a31); + a30=(a30+a41); + a41=6.0570157695918683e-03; + a42=(a41*a34); + a30=(a30+a42); + a42=-3.0184487502609172e-03; + a43=(a42*a38); + a30=(a30+a43); + if (res[0]!=0) res[0][19]=a30; + a30=(a16*a02); + a43=(a17*a26); + a30=(a30+a43); + a43=-2.9967785627100754e-02; + a44=(a43*a31); + a30=(a30+a44); + a44=-3.0801331505514837e-03; + a45=(a44*a34); + a30=(a30+a45); + a45=5.5581350085139543e-06; + a46=(a45*a38); + a30=(a30+a46); + if (res[0]!=0) res[0][20]=a30; + a30=(a19*a02); + a46=(a20*a26); + a30=(a30+a46); + a46=1.4970303990827358e+00; + a47=(a46*a31); + a30=(a30+a47); + a47=2.7177645446042520e-03; + a48=(a47*a34); + a30=(a30+a48); + a48=-4.9042367722181902e-06; + a49=(a48*a38); + a30=(a30+a49); + if (res[0]!=0) res[0][21]=a30; + a30=(a22*a02); + a49=(a23*a26); + a30=(a30+a49); + a49=2.7177645446042498e-03; + a50=(a49*a31); + a30=(a30+a50); + a50=3.0257778596593993e-01; + a51=(a50*a34); + a30=(a30+a51); + a51=-5.4600502730695923e-04; + a52=(a51*a38); + a30=(a30+a52); + if (res[0]!=0) res[0][22]=a30; + a02=(a24*a02); + a26=(a07*a26); + a02=(a02+a26); + a26=-4.9042367722181935e-06; + a31=(a26*a31); + a02=(a02+a31); + a34=(a51*a34); + a02=(a02+a34); + a34=3.0075286497147785e-01; + a38=(a34*a38); + a02=(a02+a38); + if (res[0]!=0) res[0][23]=a02; + a02=-1.; + if (res[0]!=0) res[0][24]=a02; + a11=(a05*a11); + a02=(a00*a11); + a38=(a09*a01); + a08=(a08*a38); + a02=(a02+a08); + a08=3.3399999999999999e+00; + a31=(a08*a06); + a30=-3.3199999999999998e+00; + a52=(a30*a06); + a31=(a31+a52); + a52=(a29*a31); + a02=(a02+a52); + a52=casadi_sign(a10); + a52=(a10*a52); + a53=casadi_fabs(a10); + a53=(a27+a53); + a52=(a52+a53); + a53=(a32*a52); + a02=(a02+a53); + a37=(a37*a28); + a39=(a39*a28); + a37=(a37+a39); + a39=(a36*a37); + a02=(a02+a39); + if (res[0]!=0) res[0][25]=a02; + a02=(a03*a11); + a14=(a14*a38); + a02=(a02+a14); + a14=(a40*a31); + a02=(a02+a14); + a14=(a41*a52); + a02=(a02+a14); + a14=(a42*a37); + a02=(a02+a14); + if (res[0]!=0) res[0][26]=a02; + a02=(a12*a11); + a17=(a17*a38); + a02=(a02+a17); + a17=(a43*a31); + a02=(a02+a17); + a17=(a44*a52); + a02=(a02+a17); + a17=(a45*a37); + a02=(a02+a17); + if (res[0]!=0) res[0][27]=a02; + a02=(a15*a11); + a20=(a20*a38); + a02=(a02+a20); + a20=(a46*a31); + a02=(a02+a20); + a20=(a47*a52); + a02=(a02+a20); + a20=(a48*a37); + a02=(a02+a20); + if (res[0]!=0) res[0][28]=a02; + a02=(a18*a11); + a23=(a23*a38); + a02=(a02+a23); + a23=(a49*a31); + a02=(a02+a23); + a23=(a50*a52); + a02=(a02+a23); + a23=(a51*a37); + a02=(a02+a23); + if (res[0]!=0) res[0][29]=a02; + a11=(a21*a11); + a07=(a07*a38); + a11=(a11+a07); + a31=(a26*a31); + a11=(a11+a31); + a52=(a51*a52); + a11=(a11+a52); + a37=(a34*a37); + a11=(a11+a37); + if (res[0]!=0) res[0][30]=a11; + a11=arg[0]? arg[0][6] : 0; + a37=sin(a11); + a52=arg[0]? arg[0][7] : 0; + a31=tan(a52); + a07=(a37*a31); + a07=(-a07); + if (res[0]!=0) res[0][31]=a07; + a11=cos(a11); + a07=(-a11); + if (res[0]!=0) res[0][32]=a07; + a07=cos(a52); + a38=(a37/a07); + a02=(-a38); + if (res[0]!=0) res[0][33]=a02; + a09=(a09*a25); + a00=(a00*a09); + a05=(a05*a01); + a04=(a04*a05); + a00=(a00+a04); + a08=(a08*a10); + a30=(a30*a10); + a08=(a08+a30); + a29=(a29*a08); + a00=(a00+a29); + a33=(a33*a28); + a35=(a35*a28); + a33=(a33+a35); + a32=(a32*a33); + a00=(a00+a32); + a32=casadi_sign(a06); + a32=(a06*a32); + a35=casadi_fabs(a06); + a27=(a27+a35); + a32=(a32+a27); + a36=(a36*a32); + a00=(a00+a36); + if (res[0]!=0) res[0][34]=a00; + a03=(a03*a09); + a13=(a13*a05); + a03=(a03+a13); + a40=(a40*a08); + a03=(a03+a40); + a41=(a41*a33); + a03=(a03+a41); + a42=(a42*a32); + a03=(a03+a42); + if (res[0]!=0) res[0][35]=a03; + a12=(a12*a09); + a16=(a16*a05); + a12=(a12+a16); + a43=(a43*a08); + a12=(a12+a43); + a44=(a44*a33); + a12=(a12+a44); + a45=(a45*a32); + a12=(a12+a45); + if (res[0]!=0) res[0][36]=a12; + a15=(a15*a09); + a19=(a19*a05); + a15=(a15+a19); + a46=(a46*a08); + a15=(a15+a46); + a47=(a47*a33); + a15=(a15+a47); + a48=(a48*a32); + a15=(a15+a48); + if (res[0]!=0) res[0][37]=a15; + a18=(a18*a09); + a22=(a22*a05); + a18=(a18+a22); + a49=(a49*a08); + a18=(a18+a49); + a50=(a50*a33); + a18=(a18+a50); + a50=(a51*a32); + a18=(a18+a50); + if (res[0]!=0) res[0][38]=a18; + a21=(a21*a09); + a24=(a24*a05); + a21=(a21+a24); + a26=(a26*a08); + a21=(a21+a26); + a51=(a51*a33); + a21=(a21+a51); + a34=(a34*a32); + a21=(a21+a34); + if (res[0]!=0) res[0][39]=a21; + a21=(a11*a31); + a21=(-a21); + if (res[0]!=0) res[0][40]=a21; + if (res[0]!=0) res[0][41]=a37; + a21=(a11/a07); + a34=(-a21); + if (res[0]!=0) res[0][42]=a34; + a34=(a31*a11); + a34=(a10*a34); + a31=(a31*a37); + a31=(a06*a31); + a34=(a34-a31); + a34=(-a34); + if (res[0]!=0) res[0][43]=a34; + a34=(a10*a37); + a31=(a06*a11); + a34=(a34+a31); + if (res[0]!=0) res[0][44]=a34; + a34=(a10*a21); + a31=(a06*a38); + a34=(a34-a31); + a34=(-a34); + if (res[0]!=0) res[0][45]=a34; + a34=casadi_sq(a07); + a37=(a37/a34); + a37=(a10*a37); + a11=(a11/a34); + a11=(a06*a11); + a37=(a37+a11); + a37=(-a37); + if (res[0]!=0) res[0][46]=a37; + a38=(a38/a07); + a52=sin(a52); + a38=(a38*a52); + a10=(a10*a38); + a21=(a21/a07); + a21=(a21*a52); + a06=(a06*a21); + a10=(a10+a06); + a10=(-a10); + if (res[0]!=0) res[0][47]=a10; + a10=1.; + if (res[1]!=0) res[1][0]=a10; + if (res[1]!=0) res[1][1]=a10; + if (res[1]!=0) res[1][2]=a10; + if (res[1]!=0) res[1][3]=a10; + if (res[1]!=0) res[1][4]=a10; + if (res[1]!=0) res[1][5]=a10; + if (res[1]!=0) res[1][6]=a10; + if (res[1]!=0) res[1][7]=a10; + if (res[1]!=0) res[1][8]=a10; + a10=-3.3453634479321918e-02; + if (res[2]!=0) res[2][0]=a10; + a10=-6.0368975005218423e-05; + if (res[2]!=0) res[2][1]=a10; + a10=1.1116270017027936e-07; + if (res[2]!=0) res[2][2]=a10; + a10=-9.8084735444364535e-08; + if (res[2]!=0) res[2][3]=a10; + a10=-1.0920100546139210e-05; + if (res[2]!=0) res[2][4]=a10; + a10=6.0150572994295635e-03; + if (res[2]!=0) res[2][5]=a10; + a10=-1.0920100546139184e-05; + if (res[2]!=0) res[2][6]=a10; + a10=-6.0570157695918683e-03; + if (res[2]!=0) res[2][7]=a10; + a10=3.0801331505514837e-03; + if (res[2]!=0) res[2][8]=a10; + a10=-2.7177645446042520e-03; + if (res[2]!=0) res[2][9]=a10; + a10=-3.0257778596593993e-01; + if (res[2]!=0) res[2][10]=a10; + a10=5.4600502730695923e-04; + if (res[2]!=0) res[2][11]=a10; + a06=6.0150572994295574e-03; + if (res[2]!=0) res[2][12]=a06; + a06=3.0184487502609172e-03; + if (res[2]!=0) res[2][13]=a06; + a06=-5.5581350085139543e-06; + if (res[2]!=0) res[2][14]=a06; + a06=4.9042367722181902e-06; + if (res[2]!=0) res[2][15]=a06; + if (res[2]!=0) res[2][16]=a10; + a10=-3.0075286497147785e-01; + if (res[2]!=0) res[2][17]=a10; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_alloc_mem(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_init_mem(int mem) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_free_mem(int mem) { +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_checkout(void) { + return 0; +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_release(int mem) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_incref(void) { +} + +CASADI_SYMBOL_EXPORT void auv_model_impl_dae_jac_x_xdot_u_z_decref(void) { +} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_in(void) { return 6;} + +CASADI_SYMBOL_EXPORT casadi_int auv_model_impl_dae_jac_x_xdot_u_z_n_out(void) { return 4;} + +CASADI_SYMBOL_EXPORT casadi_real auv_model_impl_dae_jac_x_xdot_u_z_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_in(casadi_int i) { + switch (i) { + case 0: return "i0"; + case 1: return "i1"; + case 2: return "i2"; + case 3: return "i3"; + case 4: return "i4"; + case 5: return "i5"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const char* auv_model_impl_dae_jac_x_xdot_u_z_name_out(casadi_int i) { + switch (i) { + case 0: return "o0"; + case 1: return "o1"; + case 2: return "o2"; + case 3: return "o3"; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s0; + case 2: return casadi_s1; + case 3: return casadi_s2; + case 4: return casadi_s3; + case 5: return casadi_s2; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT const casadi_int* auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s4; + case 1: return casadi_s5; + case 2: return casadi_s6; + case 3: return casadi_s7; + default: return 0; + } +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6; + if (sz_res) *sz_res = 4; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +CASADI_SYMBOL_EXPORT int auv_model_impl_dae_jac_x_xdot_u_z_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 6*sizeof(const casadi_real*); + if (sz_res) *sz_res = 4*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o new file mode 100644 index 000000000..1fde7b8e4 Binary files /dev/null and b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_impl_dae_jac_x_xdot_u_z.o differ diff --git a/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_model.h b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_model.h new file mode 100644 index 000000000..0e6d62dda --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/auv_model_model/auv_model_model.h @@ -0,0 +1,81 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef auv_model_MODEL +#define auv_model_MODEL + +#ifdef __cplusplus +extern "C" { +#endif + + + +// implicit ODE: function +int auv_model_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int auv_model_impl_dae_fun_work(int *, int *, int *, int *); +const int *auv_model_impl_dae_fun_sparsity_in(int); +const int *auv_model_impl_dae_fun_sparsity_out(int); +int auv_model_impl_dae_fun_n_in(void); +int auv_model_impl_dae_fun_n_out(void); + +// implicit ODE: function + jacobians +int auv_model_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int auv_model_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *); +const int *auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_in(int); +const int *auv_model_impl_dae_fun_jac_x_xdot_z_sparsity_out(int); +int auv_model_impl_dae_fun_jac_x_xdot_z_n_in(void); +int auv_model_impl_dae_fun_jac_x_xdot_z_n_out(void); + +// implicit ODE: jacobians only +int auv_model_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int auv_model_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *); +const int *auv_model_impl_dae_jac_x_xdot_u_z_sparsity_in(int); +const int *auv_model_impl_dae_jac_x_xdot_u_z_sparsity_out(int); +int auv_model_impl_dae_jac_x_xdot_u_z_n_in(void); +int auv_model_impl_dae_jac_x_xdot_u_z_n_out(void); + +// implicit ODE - for lifted_irk +int auv_model_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int auv_model_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *); +const int *auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_in(int); +const int *auv_model_impl_dae_fun_jac_x_xdot_u_sparsity_out(int); +int auv_model_impl_dae_fun_jac_x_xdot_u_n_in(void); +int auv_model_impl_dae_fun_jac_x_xdot_u_n_out(void); + + + + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // auv_model_MODEL diff --git a/control/velocity_controller/scripts/build_auv_solver/libacados_ocp_solver_auv_model.so b/control/velocity_controller/scripts/build_auv_solver/libacados_ocp_solver_auv_model.so new file mode 100755 index 000000000..c00ae82aa Binary files /dev/null and b/control/velocity_controller/scripts/build_auv_solver/libacados_ocp_solver_auv_model.so differ diff --git a/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c new file mode 100644 index 000000000..800de47b4 --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/main_auv_model.c @@ -0,0 +1,189 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" +#include "acados_solver_auv_model.h" + +// blasfeo +#include "blasfeo_d_aux_ext_dep.h" + +#define NX AUV_MODEL_NX +#define NP AUV_MODEL_NP +#define NU AUV_MODEL_NU +#define NBX0 AUV_MODEL_NBX0 +#define NP_GLOBAL AUV_MODEL_NP_GLOBAL + + +int main() +{ + + auv_model_solver_capsule *acados_ocp_capsule = auv_model_acados_create_capsule(); + // there is an opportunity to change the number of shooting intervals in C without new code generation + int N = AUV_MODEL_N; + // allocate the array and fill it accordingly + double* new_time_steps = NULL; + int status = auv_model_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps); + + if (status) + { + printf("auv_model_acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + ocp_nlp_config *nlp_config = auv_model_acados_get_nlp_config(acados_ocp_capsule); + ocp_nlp_dims *nlp_dims = auv_model_acados_get_nlp_dims(acados_ocp_capsule); + ocp_nlp_in *nlp_in = auv_model_acados_get_nlp_in(acados_ocp_capsule); + ocp_nlp_out *nlp_out = auv_model_acados_get_nlp_out(acados_ocp_capsule); + ocp_nlp_solver *nlp_solver = auv_model_acados_get_nlp_solver(acados_ocp_capsule); + void *nlp_opts = auv_model_acados_get_nlp_opts(acados_ocp_capsule); + // initial condition + double lbx0[NBX0]; + double ubx0[NBX0]; + lbx0[0] = 0; + ubx0[0] = 0; + lbx0[1] = 0; + ubx0[1] = 0; + lbx0[2] = 0; + ubx0[2] = 0; + lbx0[3] = 0; + ubx0[3] = 0; + lbx0[4] = 0; + ubx0[4] = 0; + lbx0[5] = 0; + ubx0[5] = 0; + lbx0[6] = 0; + ubx0[6] = 0; + lbx0[7] = 0; + ubx0[7] = 0; + lbx0[8] = 0; + ubx0[8] = 0; + + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "lbx", lbx0); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, nlp_out, 0, "ubx", ubx0); + + // initialization for state values + double x_init[NX]; + x_init[0] = 0.0; + x_init[1] = 0.0; + x_init[2] = 0.0; + x_init[3] = 0.0; + x_init[4] = 0.0; + x_init[5] = 0.0; + x_init[6] = 0.0; + x_init[7] = 0.0; + x_init[8] = 0.0; + + // initial value for control input + double u0[NU]; + u0[0] = 0.0; + u0[1] = 0.0; + u0[2] = 0.0; + + // prepare evaluation + int NTIMINGS = 1; + double min_time = 1e12; + double kkt_norm_inf; + double elapsed_time; + int sqp_iter; + + double xtraj[NX * (N+1)]; + double utraj[NU * N]; + + // solve ocp in loop + for (int ii = 0; ii < NTIMINGS; ii++) + { + // initialize solution + for (int i = 0; i < N; i++) + { + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "x", x_init); + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, i, "u", u0); + } + ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, nlp_in, N, "x", x_init); + status = auv_model_acados_solve(acados_ocp_capsule); + ocp_nlp_get(nlp_solver, "time_tot", &elapsed_time); + min_time = MIN(elapsed_time, min_time); + } + + /* print solution and statistics */ + for (int ii = 0; ii <= nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]); + for (int ii = 0; ii < nlp_dims->N; ii++) + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]); + + printf("\n--- xtraj ---\n"); + d_print_exp_tran_mat( NX, N+1, xtraj, NX); + printf("\n--- utraj ---\n"); + d_print_exp_tran_mat( NU, N, utraj, NU ); + // ocp_nlp_out_print(nlp_solver->dims, nlp_out); + + printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS); + + if (status == ACADOS_SUCCESS) + { + printf("auv_model_acados_solve(): SUCCESS!\n"); + } + else + { + printf("auv_model_acados_solve() failed with status %d.\n", status); + } + + // get solution + ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf); + ocp_nlp_get(nlp_solver, "sqp_iter", &sqp_iter); + + auv_model_acados_print_stats(acados_ocp_capsule); + + printf("\nSolver info:\n"); + printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n", + sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf); + + + + // free solver + status = auv_model_acados_free(acados_ocp_capsule); + if (status) { + printf("auv_model_acados_free() returned status %d. \n", status); + } + // free solver capsule + status = auv_model_acados_free_capsule(acados_ocp_capsule); + if (status) { + printf("auv_model_acados_free_capsule() returned status %d. \n", status); + } + + return status; +} diff --git a/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c b/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c new file mode 100644 index 000000000..3b036370b --- /dev/null +++ b/control/velocity_controller/scripts/build_auv_solver/main_sim_auv_model.c @@ -0,0 +1,141 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +// standard +#include +#include +// acados +#include "acados/utils/print.h" +#include "acados/utils/math.h" +#include "acados_c/sim_interface.h" +#include "acados_sim_solver_auv_model.h" + +#define NX AUV_MODEL_NX +#define NZ AUV_MODEL_NZ +#define NU AUV_MODEL_NU +#define NP AUV_MODEL_NP + + +int main() +{ + int status = 0; + auv_model_sim_solver_capsule *capsule = auv_model_acados_sim_solver_create_capsule(); + status = auv_model_acados_sim_create(capsule); + + if (status) + { + printf("acados_create() returned status %d. Exiting.\n", status); + exit(1); + } + + sim_config *acados_sim_config = auv_model_acados_get_sim_config(capsule); + sim_in *acados_sim_in = auv_model_acados_get_sim_in(capsule); + sim_out *acados_sim_out = auv_model_acados_get_sim_out(capsule); + void *acados_sim_dims = auv_model_acados_get_sim_dims(capsule); + + // initial condition + double x_current[NX]; + x_current[0] = 0.0; + x_current[1] = 0.0; + x_current[2] = 0.0; + x_current[3] = 0.0; + x_current[4] = 0.0; + x_current[5] = 0.0; + x_current[6] = 0.0; + x_current[7] = 0.0; + x_current[8] = 0.0; + + + x_current[0] = 0; + x_current[1] = 0; + x_current[2] = 0; + x_current[3] = 0; + x_current[4] = 0; + x_current[5] = 0; + x_current[6] = 0; + x_current[7] = 0; + x_current[8] = 0; + + + + + // initial value for control input + double u0[NU]; + u0[0] = 0.0; + u0[1] = 0.0; + u0[2] = 0.0; + + + + + int n_sim_steps = 3; + // solve ocp in loop + for (int ii = 0; ii < n_sim_steps; ii++) + { + // set inputs + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "x", x_current); + sim_in_set(acados_sim_config, acados_sim_dims, + acados_sim_in, "u", u0); + + // solve + status = auv_model_acados_sim_solve(capsule); + if (status != ACADOS_SUCCESS) + { + printf("acados_solve() failed with status %d.\n", status); + } + + // get outputs + sim_out_get(acados_sim_config, acados_sim_dims, + acados_sim_out, "x", x_current); + + + + // print solution + printf("\nx_current, %d\n", ii); + for (int jj = 0; jj < NX; jj++) + { + printf("%e\n", x_current[jj]); + } + } + + printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps); + + // free solver + status = auv_model_acados_sim_free(capsule); + if (status) { + printf("auv_model_acados_sim_free() returned status %d. \n", status); + } + + auv_model_acados_sim_solver_free_capsule(capsule); + + return status; +} diff --git a/control/velocity_controller/scripts/dynamics.py b/control/velocity_controller/scripts/dynamics.py new file mode 100644 index 000000000..cfd1e220b --- /dev/null +++ b/control/velocity_controller/scripts/dynamics.py @@ -0,0 +1,163 @@ + +# auv_model.py +from casadi import SX, vertcat, diag, cos, sin, tan, fabs, inv +import numpy as np +import yaml + +def euler_kinematics_T(phi, theta): + """ + Euler ZYX rate mapping: [phi_dot, theta_dot, psi_dot] = T(phi,theta) * [p q r] + Avoid singularity at cos(theta)=0 with a tiny epsilon if needed (handled later in OCP). + """ + T = SX.zeros(3, 3) + T[0, 0] = 1.0 + T[0, 1] = sin(phi) * tan(theta) + T[0, 2] = cos(phi) * tan(theta) + T[1, 0] = 0.0 + T[1, 1] = cos(phi) + T[1, 2] = -sin(phi) + T[2, 0] = 0.0 + T[2, 1] = sin(phi) / cos(theta) + T[2, 2] = cos(phi) / cos(theta) + return T + +def coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r): + """ + Rigid-body Coriolis/centripetal matrix C_RB for diagonal inertia matrices. + Fossen 2011-style, for 6DOF body velocities [u v w p q r]. + """ + C = SX.zeros(6, 6) + + # Angular velocity block + C[0, 1] = -m * r + C[0, 2] = m * q + C[1, 0] = m * r + C[1, 2] = -m * p + C[2, 0] = -m * q + C[2, 1] = m * p + + # Angular-Angular block (J*omega x omega) + C[3, 4] = Iz * r + C[3, 5] = -Iy * q + C[4, 3] = -Iz * r + C[4, 5] = Ix * p + C[5, 3] = Iy * q + C[5, 4] = -Ix * p + return C + +def dampening(linear_diag, quad_diag, u, v, w, p, q, r): + """ + Linear + quadratic diagonal damping D(v)*v = (Dl + Dq*|v|) v. + Inputs Dl, Dq are 6-vectors (or lists) for [u v w p q r]. + """ + Dl = SX(linear_diag) + Dq_vec = SX(6, 1) + vel = vertcat(u, v, w, p, q, r) + abs_vel = SX(6, 1) + for i in range(6): + abs_vel[i] = fabs(vel[i]) + Dq = SX(quad_diag) # elementwise times abs_vel when applied + # Effective damping operator when multiplying right by vel: + # (Dl + Dq*|vel|) * vel + return Dl, Dq, abs_vel + +def make_auv_model(mass_inertia_matrix,D_lin,D_quad): + """ + Build symbolic CasADi model for a 9x3 AUV suitable for acados codegen. + + params (dict) expected keys: + - m, Ix, Iy, Iz : scalars + - D_lin: length-6 list/tuple : linear damping diag for [u v w p q r] + - D_quad: length-6 list/tuple : quadratic damping diag + - g_vec: length-6 list/tuple : restoring forces/torques in body (optional; use zeros if unknown) + Returns: + dict with keys: x, u, f_expl_expr, name + """ + # Unpack parameters + M=SX(mass_inertia_matrix) + Ix = mass_inertia_matrix[3][3] + Iy = mass_inertia_matrix[4][4] + Iz = mass_inertia_matrix[5][5] + m=M[0][0] + #D_lin = params.get('D_lin') + #D_quad = params.get('D_quad') + + # States: body velocities and Euler angles + u = SX.sym('u') # surge + v = SX.sym('v') # sway + w = SX.sym('w') # heave + p = SX.sym('p') # roll rate NED + q = SX.sym('q') # pitch rate NED + r = SX.sym('r') # yaw rate NED + phi = SX.sym('phi') #Body + theta = SX.sym('theta') #Body + psi = SX.sym('psi') #Body + + x = vertcat(u, v, w, p, q, r, phi, theta, psi) + + # Inputs: surge force, pitch & yaw moments + Fx = SX.sym('Fx') + My = SX.sym('My') + Mz = SX.sym('Mz') + u_in = vertcat(Fx, My, Mz) + + # Inertia (diagonal for now) + #M = diag(SX([m, m, m, Ix, Iy, Iz])) + + # Coriolis matrix for diagonal inertia + C = coriolis_rb_diag(m, Ix, Iy, Iz, u, v, w, p, q, r) + + # Damping (linear + quadratic diagonal) + Dl, Dq, abs_vel = dampening(D_lin, D_quad, u, v, w, p, q, r) + #TODO: blend the dampening functions + # Generalized input vector tau: map [Fx, My, Mz] to 6x1 wrench + # tau = [Fx, 0, 0, 0, My, Mz]^T + tau = vertcat(Fx, 0.0, 0.0, 0.0, My, Mz) + + + # 6DOF body dynamics: nu_dot = M^{-1} (tau - C*nu - (Dl + Dq*|nu|) nu - g) + nu = vertcat(u, v, w, p, q, r) + D_eff_times_nu = (Dl + diag(Dq @ abs_vel)) @ nu # elementwise: (Dq*|nu|) * nu + rhs_nu = SX(6, 1) + rhs_nu = inv(M) @ (tau - C @ nu - D_eff_times_nu) # + print(D_eff_times_nu) + # Kinematics: eta_dot = T(eta) * omega + T = euler_kinematics_T(phi, theta) + eta_dot = T @ vertcat(p, q, r) + + xdot = vertcat(rhs_nu, eta_dot) + xdot_sym = SX.sym("xdot", x.size()[0]) + model = { + "name": "auv_model", + "x": x, + "xdot": xdot_sym, + "u": u_in, + "f_expl_expr": xdot, # explicit ODE f(x,u) + # implicit form (optional in acados): f_impl = xdot - f(x,u) + "f_impl_expr": xdot_sym - xdot + } + return model + +if __name__ == "__main__": + # Quick smoke test + m=[[6,0,0,0,0,0,], + [0,5,0,0,0,0,], + [0,0,4,0,0,0,], + [0,0,0,3,0,0,], + [0,0,0,0,5,0,], + [0,0,0,0,0,9]] + D_lin=[[6,0,0,0,0,0,], + [0,5,0,0,0,0,], + [0,0,4,0,0,0,], + [0,0,0,3,0,0,], + [0,0,0,0,5,0,], + [0,0,0,0,0,9]] + D_quad=[[6,0,0,0,0,0,], + [0,5,0,0,0,0,], + [0,0,4,0,0,0,], + [0,0,0,3,0,0,], + [0,0,0,0,5,0,], + [0,0,0,0,0,9]] + mdl = make_auv_model(m,D_lin,D_quad) + print("Model:", mdl["name"]) + print("x dim:", mdl["x"].numel(), "u dim:", mdl["u"].numel()) diff --git a/control/velocity_controller/scripts/generator.py b/control/velocity_controller/scripts/generator.py new file mode 100644 index 000000000..cbd2ee512 --- /dev/null +++ b/control/velocity_controller/scripts/generator.py @@ -0,0 +1,223 @@ + +#!/usr/bin/env python3 +""" +AUV NMPC OCP generator for acados +N = 20, Tf = 0.05 (2.5 ms steps) +Nonlinear thrust magnitude constraint. +Diagonal Q/R/Qe loaded from weights.yaml or defaults. + +Generates a solver in ./build_auv_solver/ +""" + +import numpy as np +import yaml +from pathlib import Path +import scipy.linalg + +from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel +from casadi import SX, vertcat + +# Import the underwater vehicle model from Step 1 +from dynamics import make_auv_model + + +# ----------------------------- +# Load weighting matrices +# ----------------------------- +def load_matrices(path="../config/parameters.yaml"): + if Path(path).exists(): + with open(path, "r") as f: + data = yaml.safe_load(f) + node_key = next(iter(data.keys())) + print("Top-level keys:", list(data.keys())) + print("[INFO] Loading weights from", path) + Q = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) + R = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["R"]) + Qe = np.diag(data[node_key]["ros__parameters"]["NMPC_params"]["Q"]) + inertia_M=np.reshape(data[node_key]["ros__parameters"]["inertia_matrix"],(6,6)) + D_lin = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_low"],(6,6)) + D_quad = np.reshape(data[node_key]["ros__parameters"]["dampening_matrix_high"],(6,6)) + N=data[node_key]["ros__parameters"]["NMPC_params"]["N"] + delta_t=data[node_key]["ros__parameters"]["publish_rate"] + max_force=data[node_key]["ros__parameters"]["max_force"] + else: + print("[ERROR], yaml file not found") + + return Q, R, Qe, inertia_M, D_lin, D_quad,N,delta_t,max_force + + +# ----------------------------- +# Build the OCP +# ----------------------------- +def create_auv_ocp(): + # Load weights + Q, R, Qe, inertia_Matrix, D_lin, D_quad, N, delta_t, max_force = load_matrices() + + # Load dynamical model + mdl = make_auv_model(inertia_Matrix, D_lin, D_quad) + + # Wrap into acados model format + acados_model = AcadosModel() + acados_model.name = mdl["name"] + acados_model.x = mdl["x"] + acados_model.xdot=mdl["xdot"] + acados_model.u = mdl["u"] + acados_model.f_expl_expr = mdl["f_expl_expr"] + acados_model.f_impl_expr = mdl["f_impl_expr"] + + # Create OCP + ocp = AcadosOcp() + ocp.model = acados_model + + # Horizon settings + Tf = (delta_t*N)/1000 # total horizon [seconds] + ocp.dims.N = N + ocp.solver_options.tf = Tf + + ocp.solver_options.integrator_type="IRK" + ocp.solver_options.sim_method_num_stages=2 + ocp.solver_options.sim_method_num_steps=4 + + nx = acados_model.x.size()[0] + nu = acados_model.u.size()[0] + + # ---------------------------------- + # Cost: LINEAR_LS (yref-based) + # ---------------------------------- + # ---------------------------------- + ocp.cost.cost_type = "LINEAR_LS" + ocp.cost.cost_type_e = "LINEAR_LS" + + # States you care about: u=0, q=4, r=5 + idx_states = [0, 4, 5, 7, 8] + idx_controls = [0, 1, 2] + + n_y = len(idx_states) + len(idx_controls) # 8 + n_ye = len(idx_states) # 5 + + # Vx: (8, nx) — selects only u, q, r from state vector + Vx = np.zeros((n_y, nx)) + for i, idx in enumerate(idx_states): + Vx[i, idx] = 1.0 + + # Vu: (8, nu) — selects all 3 controls, placed in lower block + Vu = np.zeros((n_y, nu)) + for i, idx in enumerate(idx_controls): + Vu[len(idx_states) + i, idx] = 1.0 + + # W: (8, 8) — only track what you care about, well conditioned + Q_tracked = np.diag([ + Q[0, 0], # u + Q[1, 1], # q + Q[2, 2], # r + Q[3, 3], # theta + Q[4, 4], # psi +]) + R_tracked = np.diag([R[0,0], R[1,1], R[2,2]]) # weights for controls + + W = scipy.linalg.block_diag(Q_tracked, R_tracked) + + ocp.cost.Vx = Vx # (8, nx) + ocp.cost.Vu = Vu # (8, nu) + ocp.cost.W = W # (8, 8) + + # Terminal cost — same state selection, no controls + Vx_e = np.zeros((n_ye, nx)) + for i, idx in enumerate(idx_states): + Vx_e[i, idx] = 1.0 + + Q_e_tracked = np.diag([Qe[0,0], Qe[1,1], Qe[2,2], Qe[3,3],Qe[4,4]]) + + ocp.cost.Vx_e = Vx_e # (5, nx) + ocp.cost.W_e = Q_e_tracked # (5, 5) + + # References must match ny=6 and ny_e=3 + ocp.cost.yref = np.zeros(n_y) # [u, q, r, theta, psi, u1, u2, u3] + ocp.cost.yref_e = np.zeros(n_ye) # [u, q, r, theta, psi] + + # ---------------------------------- + # Nonlinear input constraint: + # Fx^2 + My^2 + Mz^2 <= 10000 + # ---------------------------------- + #Fx, My, Mz = acados_model.u[0], acados_model.u[1], acados_model.u[2] + #h_expr = Fx**2 + My**2 + Mz**2 + + #ocp.model.con_h_expr = vertcat(h_expr) # 1 constraint + #ocp.dims.nh=1 + #ocp.constraints.lh = np.array([0.0]) # lower bound: h >= 0 (redundant) + #ocp.constraints.uh = np.array([max_force**2]) # upper bound: magnitude <= 100 + + # No bounds on slack variables (we don't use slacks) + #ocp.constraints.idxsh = np.array([], dtype=int) + #ocp.constraints.lsh = np.array([]) + #ocp.constraints.ush = np.array([]) + + # No box-constraints on h (handled via lh/uh) + #ocp.constraints.idxbh = np.array([], dtype=int) + #ocp.constraints.lbh = np.array([]) + #ocp.constraints.ubh = np.array([]) + + u_max = max_force # from YAML + + ocp.constraints.lbu = -u_max * np.ones(nu) + ocp.constraints.ubu = u_max * np.ones(nu) + ocp.constraints.idxbu = np.arange(nu, dtype=int) + ocp.constraints.idxbx = np.array([7]) + ocp.constraints.lbx = -1.4 + ocp.constraints.ubx = 1.4 + ocp.dims.nbx = 1 + + # ---------------------------------- + # Initial state constraint (must be updated before solve) + # ---------------------------------- + ocp.constraints.x0 = np.zeros(nx) + + # ---------------------------------- + # Solver options + # ---------------------------------- + print("W shape:", ocp.cost.W.shape) + print("W diagonal:", np.diag(ocp.cost.W)) + print("W_e shape:", ocp.cost.W_e.shape) + print("W_e diagonal:", np.diag(ocp.cost.W_e)) + print("Vx shape:", ocp.cost.Vx.shape) + print("Vx_e shape:", ocp.cost.Vx_e.shape) + print("yref:", ocp.cost.yref) + print("yref_e:", ocp.cost.yref_e) + print("ny:", ocp.dims.ny) + print("ny_e:", ocp.dims.ny_e) + print("VX",ocp.cost.Vx) + print("VU", ocp.cost.Vu) + ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM" + ocp.solver_options.qp_solver_warm_start=1 + ocp.solver_options.hessian_approx = "GAUSS_NEWTON" + #ocp.solver_options.integrator_type = "ERK" + ocp.solver_options.nlp_solver_type = "SQP" # fast real-time iteration + #ocp.solver_options.nlp_solver_max_iter = 100 + + #ocp.solver_options.globalization = 'MERIT_BACKTRACKING' + ocp.solver_options.levenberg_marquardt = 1e-2 + ocp.solver_options.print_level = 2 + + # ---------------------------------- + # Output directory + # ---------------------------------- + ocp.code_gen_opts.code_export_directory = "build_auv_solver" + + print("nh:", ocp.dims.nh) + print("lh:", ocp.constraints.lh) + print("uh:", ocp.constraints.uh) + + #print("idxbh:", ocp.constraints.idxbh) + print("idxsh:", ocp.constraints.idxsh) + + return ocp + + +# ----------------------------- +# Main entry: generate solver +# ----------------------------- +if __name__ == "__main__": + ocp = create_auv_ocp() + print("[INFO] Generating AUV NMPC solver...") + AcadosOcpSolver(ocp, json_file="auv_ocp.json") + print("[INFO] Done. Solver code is in ./build_auv_solver/") diff --git a/control/velocity_controller/src/LQR_setup.cpp b/control/velocity_controller/src/LQR_setup.cpp new file mode 100644 index 000000000..0de30a5ff --- /dev/null +++ b/control/velocity_controller/src/LQR_setup.cpp @@ -0,0 +1,264 @@ + +#include "velocity_controller/LQR_setup.hpp" +#include "rclcpp/rclcpp.hpp" +#include +#include +#include +#include +#include +#include +//#include +//#include +//#include +//#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/utilities.hpp" +#include +//#include +#include "vortex/utils/math.hpp" +#include "ct/optcon/lqr/LQR.hpp" +#include "velocity_controller/NMPC_setup.hpp" + + + +//Eigen::IOFormat fmt(Eigen::StreamPrecision, 0, ", ", "\n", "[", "]"); +LQRController::LQRController() +{ + Q.setZero(); + R.setZero(); + B.setZero(); + D_low.setZero(); + D_high.setZero(); + inertia_matrix_inv.setZero(); +}; +bool LQRController::set_matrices(std::vector Q_,std::vector R_,std::vector inertia_matrix_,double max_force_, std::vector water_r_low,std::vector water_r_high){ + //Possible error handling here to check for size and allowed values. + if (Q_.size()!=8){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); + return 0; + } + if(R_.size()!=3){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); + return 0; + } + if(inertia_matrix_.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); + return 0; + } + if(water_r_low.size()!=36||water_r_high.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); + return 0; + } + if (max_force_<0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); + return 0; + } + max_force=max_force_; + // Ensure full matrices are zeroed before assigning diagonals + Q.diagonal() = Eigen::Map(Q_.data(), Q_.size()); + R.diagonal() = Eigen::Map(R_.data(), R_.size()); + Ixx=inertia_matrix_.at(6*3+3); Iyy=inertia_matrix_.at(4*6+4); Izz=inertia_matrix_.at(5*6+5); mass=inertia_matrix_.at(0); + + Eigen::Matrix inertia_matrix = Eigen::Map>(inertia_matrix_.data(),6,6); + D_low=Eigen::Map>(water_r_low.data(),6,6); + D_high=Eigen::Map>(water_r_high.data(),6,6); + inertia_matrix_inv=inertia_matrix.inverse(); + + Eigen::MatrixB_t=inertia_matrix_inv*(Eigen::Matrix()<<1,0,0, 0,0,0, 0,0,0, 0,0,0, 0,1,0, 0,0,1).finished(); + B.setZero(); + Eigen::Matrix B_m; + B_m.setZero(); + B_m.block<6,3>(0,0)=B_t; + std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; + for (long unsigned int i=0;i(0,0)=B_m.block<5,3>(0,0); + reset_controller(); + return 1; +} + +//Can be optimized +std::tuple LQRController::saturate (double value, bool windup, double limit){ + if (abs(value) > limit){ + windup=true; + value = limit * (value/abs(value)); + } + else { + windup=false; + } + return {windup,value}; +} + + + +double LQRController::anti_windup(double error, double integral_sum, bool windup){ + if (!windup){ + integral_sum += error*interval_; + } + return integral_sum; +} + + + +Eigen::Matrix LQRController::linearize(State s){ + //Eigen::Matrix A; + Eigen::Matrix D=Eigen::Matrix::Zero(); + + if (s.surge<100){ //Threshold tbd + D=-inertia_matrix_inv*D_low; + } + else { + D=-inertia_matrix_inv*D_high; + } + Eigen::Matrix C=coriolis(s); + /* //Need to decide if i want to learize around 0? + C(1,5)=-mass*s.surge; + C(2,4)=mass*s.surge; + */ + D-=inertia_matrix_inv*C; //To avoid unneccessary allocation + + Eigen::Matrix T=Eigen::Matrix::Identity(); + /*T<<1,sin(s.yaw)*tan(s.pitch),cos(s.yaw)*tan(s.pitch), + 0,cos(s.yaw),-sin(s.yaw), + 0,sin(s.yaw)/cos(s.pitch),cos(s.yaw)/cos(s.pitch);*/ + Eigen::Matrix A; + A.block<6,6>(0,0)=D; + A.block<3,3>(0,6)=A.block<3,3>(6,0)=A.block<3,3>(6,6)=Eigen::Matrix3d::Zero(); + A.block<3,3>(6,3)=T; + std::vector> swaplines{{1,7},{2,8},{3,4},{4,5}}; + for (long unsigned int i=0;i ret; + ret.setZero(); + ret.block<5,5>(0,0)=A.block<5,5>(0,0); + ret.block<3,3>(5,0)=Eigen::Matrix3d::Identity(); + + return ret; +}; +Eigen::Vector LQRController::update_error(const Guidance_data& error, const State& state){ + double surge_error = error.surge; + double pitch_error = error.pitch; + double yaw_error = error.yaw; + + integral_error_surge = anti_windup(surge_error, integral_error_surge, surge_windup); + integral_error_pitch = anti_windup(pitch_error, integral_error_pitch, pitch_windup); + integral_error_yaw = anti_windup(yaw_error, integral_error_yaw, yaw_windup); + + Eigen::Vector state_error= {surge_error, pitch_error, yaw_error, -state.pitch_rate, -state.yaw_rate, integral_error_surge, integral_error_pitch, integral_error_yaw}; + return state_error; +} +Eigen::Vector LQRController::saturate_input(Eigen::Vector u){ + double force_x, torque_y, torque_z; + std::tie(surge_windup, force_x) = saturate(u[0], surge_windup, max_force); + std::tie(pitch_windup, torque_y) = saturate(u[1], pitch_windup, max_force); + std::tie(yaw_windup, torque_z) = saturate(u[2], yaw_windup, max_force); + return {force_x, torque_y, torque_z}; +} +bool LQRController::calculate_thrust(State state, Guidance_data error){ + ct::optcon::LQR<8,3> lqr; + Eigen::Matrix K_l; + + //TODO: consider making my own solver using eigen so that it does not need controll_toolbox library + bool INFO= lqr.compute(Q,R,linearize(state),B,K_l,true,false); + if(INFO==0){ + return false; + } + + Eigen::Matrix state_error = update_error(error, state); + u=saturate_input( (K_l*state_error)); + return true; +} +void LQRController::reset_controller(int nr){ + if(nr==0||nr==1){ + integral_error_surge=0.0; + surge_windup=false; + } + if(nr==0||nr==2){ + integral_error_pitch=0.0; + pitch_windup=false; + } + if(nr==0||nr==3){ + integral_error_yaw=0.0; + yaw_windup=false; + } + + return; +} +bool LQRController::set_interval(double interval){ + if(interval<=0)return false; + interval_=interval; + return true; +} + +Eigen::Vector LQRController::get_thrust(){ + return u; +} +//Hjelpefunksjoner for å konvertere mellom std::vector og Eigen::Matrix3d +/*/ +Eigen::Matrix3d vector_to_matrix3d(const std::vector &other_matrix){ + Eigen::Matrix3d mat; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + mat(i, j) = other_matrix[i * 3 + j]; + return mat; +} +std::vector matrix3d_to_vector(const Eigen::Matrix3d &mat){ + std::vector other_matrix(9); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + other_matrix[i * 3 + j] = mat(i, j); + return other_matrix; +} + +std::vector> matrix3d_to_vector2d(const Eigen::Matrix3d &mat){ + std::vector> other_matrix(3, std::vector(3)); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + other_matrix[i][j] = mat(i, j); + return other_matrix; +} + +Eigen::Matrix3d vector2d_to_matrix3d(const std::vector> &other_matrix){ + Eigen::Matrix3d mat; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + mat(i, j) = other_matrix[i][j]; + return mat; +} +*/ +//TODO: double check the matrices here +Eigen::Matrix LQRController::coriolis(const State& s) +{ + // Body velocities + double u = s.surge; + double v = s.sway; + double w = s.heave; + double p = s.roll_rate; + double q = s.pitch_rate; + double r = s.yaw_rate; + + + Eigen::Matrix C = Eigen::Matrix::Zero(); + + + // Top-right block (translational-rotational coupling) + C(0,4) = mass * w; C(0,5) = -mass * v; + C(1,3) = -mass * w; C(1,5) = mass * u; + C(2,3) = mass * v; C(2,4) = -mass * u; + + // Bottom-left block (rotational-translational coupling) + C(3,1) = mass * w; C(3,2) = -mass * v; + C(4,0) = -mass* w; C(4,2) = mass * u; + C(5,0) = mass * v; C(5,1) = -mass * u; + + // Bottom-right block (rotational-rotational coupling) + C(3,4) = Izz * r; C(3,5) = -Iyy * q; + C(4,3) = -Izz * r; C(4,5) = Ixx * p; + C(5,3) = Iyy * q; C(5,4) = -Ixx * p; + + return C; +} \ No newline at end of file diff --git a/control/velocity_controller/src/NMPC_acados.cpp b/control/velocity_controller/src/NMPC_acados.cpp new file mode 100644 index 000000000..9f7e99a11 --- /dev/null +++ b/control/velocity_controller/src/NMPC_acados.cpp @@ -0,0 +1,183 @@ +//#include "rclcpp/rclcpp.hpp" +#include "velocity_controller/NMPC_acados.hpp" +#include // memcpy +#include +//#include "acados_solver_auv_model.h" +#include + +void AuvNMPC::set_diag(double* M, int n, const std::vector& diag) { + for (int i = 0; i < n; ++i) { + std::memset(M + i*n, 0, n * sizeof(double)); + if (i < (int)diag.size()) M[i*n + i] = diag[i]; + } +} + +AuvNMPC::~AuvNMPC() { + if (capsule_) { + auv_model_acados_free(capsule_); + auv_model_acados_free_capsule(capsule_); + capsule_ = nullptr; + std::cout << "Destroying AuvNMPC..." << std::endl; + } +} + +bool AuvNMPC::init() { + capsule_ = auv_model_acados_create_capsule(); + if (!capsule_) return false; + + + int status = auv_model_acados_create(capsule_); + if (status) { + std::cerr << "[AuvNMPC] create failed, status: " << status << std::endl; + return false; + } + + solver_ = auv_model_acados_get_nlp_solver(capsule_); + config_ = auv_model_acados_get_nlp_config(capsule_); + dims_ = auv_model_acados_get_nlp_dims(capsule_); + nlp_in_ = auv_model_acados_get_nlp_in(capsule_); + nlp_out_= auv_model_acados_get_nlp_out(capsule_); + N_ = (N_override_ > 0) ? N_override_ : AUV_MODEL_N; // fallback + return true; +} +bool AuvNMPC::initialize_guess(std::array x,std::array u_init){ + for (int i=0;i& Wd, const std::vector& We) { + std::vector W_diag_(NY, 0.0); + std::vector W_e_diag_(NY_E, 0.0); + if ((int)Wd.size() == NY) { + W_diag_ = Wd; + } else { + std::cerr << "[AuvNMPC] set_weights: W size mismatch, got " + << Wd.size() << " expected " << NY << std::endl; + } + if ((int)We.size() == NY_E) { + W_e_diag_ = We; + } else { + std::cerr << "[AuvNMPC] set_weights: W_e size mismatch, got " + << We.size() << " expected " << NY_E << std::endl; + } + // Build W and W_e from current diagonals (could cache) + + set_diag(W_.data(), NY, W_diag_); + + set_diag(W_e_.data(), NY_E, W_e_diag_); +} + +void AuvNMPC::set_max_force(double max_force) { + max_force2_ = max_force; + if (std::isnan(max_force2_)) std::cout<<"Max force is Nan"<(x0.data())); + ocp_nlp_constraints_model_set(config_, dims_, nlp_in_, nlp_out_, 0, "ubx", const_cast(x0.data())); + + + + // Update stages + // Stage yref: [u, q, r, theta, psi, tau1, tau2, tau3] + // Matches Vx selecting states [0,4,5,7,8] and Vu selecting all 3 inputs + double yref[NY] = { + xr[0], // u (surge velocity) + xr[1], // q (pitch rate) + xr[2], // r (yaw rate) + xr[3], // theta (pitch) + xr[4], // psi (yaw) + ur[0], // tau_surge + ur[1], // tau_pitch + ur[2] // tau_yaw + }; + for (int k = 0; k < N_; ++k) { + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "yref", yref); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, k, "W", W_.data()); + } + // Terminal yref_e: [u, q, r, theta, psi] + double yref_e[NY_E] = { + xr[0], // u + xr[1], // q + xr[2], // r + xr[3], // theta + xr[4] // psi + }; + + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "yref_e", yref_e); + ocp_nlp_cost_model_set(config_, dims_, nlp_in_, N_, "W_e", W_e_.data()); + + // Solve (blocking) + /* + for (int k = 0; k <= N_; ++k) { + ocp_nlp_out_set(config_, dims_, nlp_out_, nlp_in_,k, "x", const_cast(x0.data())); + } + double u_init[NU] = {0.0, 0.0, 0.0}; + for (int k = 0; k < N_; ++k) { + ocp_nlp_out_set(config_, dims_, nlp_out_,nlp_in_, k, "u", u_init); + }*/ + + int status = auv_model_acados_solve(capsule_); + + if (status == 0) { + std::cout << "--- Predicted Trajectory ---" << std::endl; + for (int k = 0; k <= N_; ++k) { + double x_k[NX]; + ocp_nlp_out_get(config_, dims_, nlp_out_, k, "x", x_k); + std::cout << "Step " << k << ": " < AuvNMPC::getU0(){ + return u0_out; +} + +void AuvNMPC::setState(const std::array& x){ + x0=x; + for (int i=0;i& x_ref){ //surge, pitch_rate, yaw_rate, pitch, yaw + xr=x_ref; + std::cout<<"xr: "; + for (int i=0;i +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "velocity_controller/utilities.hpp" +#include "velocity_controller/NMPC_setup.hpp" + + +bool NMPC_controller::set_matrices(std::vector Q,std::vector R,std::vector inertia_matrix, double max_force,std::vector water_r_low,std::vector water_r_high){ + if (Q.size()!=9){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The Q matrix has the wrong amount of elements"); + return 0; + } + if(R.size()!=3){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The R matrix has the wrong amount of elements"); + return 0; + } + if(inertia_matrix.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The M matrix has the wrong amount of elements"); + return 0; + } + if(water_r_low.size()!=36||water_r_high.size()!=36){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The D matrix has the wrong amount of elements"); + return 0; + } + if (max_force<0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"The max_force need to be >0"); + return 0; + } + if (inertia_matrix[0]<0){ + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),"Negative mass?"); + return 0; + } + Q_.setZero(); + R_.setZero(); + Q_.diagonal() = Eigen::Map(Q.data(), Q.size()); + R_.diagonal() = Eigen::Map(R.data(), R.size()); + Eigen::Matrix inertia_matrix_ = Eigen::Map>(inertia_matrix.data(),6,6); + D_low=Eigen::Map>(water_r_low.data(),6,6); + D_high=Eigen::Map>(water_r_high.data(),6,6); + M_inv=inertia_matrix_.inverse(); + mass=inertia_matrix[0]; + Ix=inertia_matrix_(3,3);std::vector Q2; + std::vector R2; + Iy=inertia_matrix_(4,4); + Iz=inertia_matrix_(5,5); + + //B_=M_inv*(Eigen::Matrix() << 1,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0).finished(); + + return true; +}; +void NMPC_controller::reset_controller(){ + return; +} +bool NMPC_controller::set_interval(double interval){ + interval_=interval; + return true; +} +bool NMPC_controller::initialize_MPC(){ + using SYM=casadi::MX; + SYM X=SYM::sym("X",n,1); //u,v,w,p,q,r,phi,theta,psi + //SYM A=SYM::zeros(n,n); + casadi::DM M_i=casadi::DM::zeros(6,6); + SYM U=SYM::sym("U",3,1); + casadi::DM Q=casadi::DM::zeros(n,n); + casadi::DM R=casadi::DM::zeros(m,m); + /*U(0,0)=SYM::sym("u_surge"); + U(1,0)=SYM::sym("u_pitch"); + U(2,0)=SYM::sym("u_yaw");*/ + + //Creating M_i matrixstd::vector Q2; + std::vector R2; + for (int i=0;i X_v(N+1), U_v(N); + for (int i=0;i<=N;i++) X_v[i] = SYM::sym("X_"+std::to_string(i),n); + for (int i=0;i z_parts; + z_parts.insert(z_parts.end(), X_v.begin(), X_v.end()); + z_parts.insert(z_parts.end(), U_v.begin(), U_v.end()); + SYM Z = vertcat(z_parts); + + //Initial state + SYM x0=SYM::sym("x0",n); + SYM xr=SYM::sym("xr",n); + SYM ur=SYM::sym("ur",m); + + SYM P=SYM::vertcat({x0,xr,ur}); + + + auto p_x0 = P(casadi::Slice(0, n)); // x0 + auto p_xr = P(casadi::Slice(n, 2*n)); // xr + auto p_ur = P(casadi::Slice(2*n, 2*n + m)); // ur + + //Dynamic constraints + + std::vector g_list; + g_list.push_back(X_v[0]-p_x0); + for (int i=0; i lbx_parts, ubx_parts; + x_min(7)=-1.4; + x_max(7)=1.4; + // X0..XN + for (int k = 0; k <= N; ++k) { + lbx_parts.push_back(x_min); + ubx_parts.push_back(x_max); + } + // U0..U_{N-1} + for (int k = 0; k < N; ++k) { + lbx_parts.push_back(u_min); + ubx_parts.push_back(u_max); + } + lbx = vertcat(lbx_parts); + ubx = vertcat(ubx_parts); + + // Equality constraints: G == 0 + lbg = casadi::DM::zeros(n*(N+1)); + ubg = casadi::DM::zeros(n*(N+1)); + + + //building NLP + casadi::MXDict nlp; + nlp["x"]=Z; + nlp["f"]=J; + nlp["g"]=G; + nlp["p"]=P; + casadi::Dict opts1; + opts1["ipopt.print_level"]=2; + opts1["print_time"]=false; + opts1["ipopt.sb"]="yes"; + opts1["expand"]=true; + opts1["jit"]=false; + opts1["ipopt.tol"]=1e-4; + opts1["ipopt.max_iter"]=100; + opts1["ipopt.linear_solver"]="mumps"; //robust default + + solver=casadi::nlpsol("solver","ipopt",nlp,opts1); + + // -------------------------------------------------------- + // Prepare parameter vector Pval and initial guess Z0 + // -------------------------------------------------------- + // Example numeric values: + casadi::DM x0_val = casadi::DM::zeros(n,1); + std::vector Pval_parts; + Pval_parts.push_back(x0_val); + Pval_parts.push_back(casadi::DM::zeros(n,1)); + Pval_parts.push_back(casadi::DM::zeros(m,1)); + Pval = vertcat(Pval_parts); + + // Initial guess: Z0 (X guesses then U guesses) + std::vector Z0_parts; + for (int k = 0; k <= N; ++k) Z0_parts.push_back(x0_val); // start with x0 everywhere + for (int k = 0; k < N; ++k) Z0_parts.push_back(casadi::DM::zeros(m,1)); + Z0_next = vertcat(Z0_parts); + + + return true; +} + +bool NMPC_controller::calculate_thrust(Guidance_data guidance_values, State state){ + + casadi::DM x0_val={state.surge,state.sway,state.heave,state.roll_rate,state.pitch_rate,state.yaw_rate,state.roll,state.pitch,state.yaw}; + casadi::DM xr_val={guidance_values.surge,0,0,0,0,0,0,guidance_values.pitch,guidance_values.yaw}; + casadi::DM ur_val={0,0,0}; + Pval=casadi::DM::vertcat({x0_val,xr_val,ur_val}); + // Solve + + casadi::DMDict solver_in; + solver_in["x0"] = Z0_next; + solver_in["lbx"] = lbx; + solver_in["ubx"] = ubx; + solver_in["lbg"] = lbg; + solver_in["ubg"] = ubg; + solver_in["p"] = Pval; + auto sol = solver(solver_in); + if (sol.count("x") == 0) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "NLP solver failed"); + return 1; + } + casadi::DM Zstar = sol.at("x"); // optimal stacked decision vector + //TODO: check to see NAN or INF values in solution + + // index of U0 start: + int offset_u0 = n*(N+1); + // Extract u0* (first control block after all states) + // Z = [X0; X1; ...; XN; U0; U1; ...; U_{N-1}] + casadi::DM u0_star = Zstar(casadi::Slice(offset_u0, offset_u0 + m)); + + std::cout << "u0* = " << u0_star << std::endl; + + + // Warm-start shift for next iteration + // Build new Z0_next = [X1*; X2*; ...; XN*; XN*; U1*; ...; U_{N-1}*; U_{N-1}*] + // (X0 will be re-anchored by the new measured x0 in constraints) + std::vector Xstar(N+1), Ustar(N); + // Unstack from Zstar: + for (int k = 0; k <= N; ++k) { + int i0 = k*n; + Xstar[k] = Zstar(casadi::Slice(i0, i0+n)); + } + for (int k = 0; k < N; ++k) { + int i0 = n*(N+1) + k*m; + Ustar[k] = Zstar(casadi::Slice(i0, i0+m)); + } + + std::vector Z0_next_parts; + // shifted states: X1..XN, repeat XN at end + for (int k = 1; k <= N; ++k) Z0_next_parts.push_back(Xstar[k]); + Z0_next_parts.push_back(Xstar[N]); + // shifted inputs: U1..U_{N-1}, repeat last + for (int k = 1; k < N; ++k) Z0_next_parts.push_back(Ustar[k]); + Z0_next_parts.push_back(Ustar[N-1]); + + Z0_next = vertcat(Z0_next_parts); + + + thrust(0) = double(u0_star(0)); + thrust(1) = double(u0_star(1)); + thrust(2) = double(u0_star(2)); + return 0; +} + +Eigen::Matrix NMPC_controller::get_thrust(){ + return thrust; +} \ No newline at end of file diff --git a/control/velocity_controller/src/PID_setup.cpp b/control/velocity_controller/src/PID_setup.cpp new file mode 100644 index 000000000..3b8102214 --- /dev/null +++ b/control/velocity_controller/src/PID_setup.cpp @@ -0,0 +1,86 @@ +#include "velocity_controller/PID_setup.hpp" + +/* +PID_controller::PID_controller( double Kp, double Ki, double Kd, double max_output, double min_output, double dt):Kp_(Kp), Ki_(Ki), Kd_(Kd), max_output_(max_output), min_output_(min_output), dt_(dt) { + integral = 0.0; + previous_error = 0.0; +};*/ +//TODO: kanskje forbedre integrasjon og derivasjons beregningene +//TODO: check for more errors, f.example Nan or very high intergral +bool PID_controller::calculate_thrust(double error){ + if(!init)return false; + //Saturation + if (output>max_output_){ + output = max_output_; + } + else if (output < min_output_){ + output = min_output_; + } + else{ + integral+=error*dt_; //anti-wind up + } + //P + I + D + output=Kp_*error+Ki_*integral + Kd_ * (error - previous_error) / dt_; + previous_error = error; + + return true; +}; +bool PID_controller::calculate_thrust(double error, double error_d){ + if(!init)return false; + //Saturation + if (output>max_output_){ + output = max_output_; + } + else if (output < min_output_){ + output = min_output_; + } + else{ + integral+=error*dt_; //anti-wind up + } + //P + I + D + output=Kp_*error+Ki_*integral + Kd_ * error_d; + previous_error = error; + + return true; +} +void PID_controller::reset_controller(){ + integral = 0.0; + previous_error = 0.0; + output=0.0; +} + +double PID_controller::get_output(){ + return output; +}; + +bool PID_controller::set_output_limits(double min_output, double max_output){ + if (max_output& params,double dt){ + return set_parameters(params.at(0),params.at(1),params.at(2), dt); + +}; + diff --git a/control/velocity_controller/src/ct_instantiations.cpp b/control/velocity_controller/src/ct_instantiations.cpp new file mode 100644 index 000000000..124a25e99 --- /dev/null +++ b/control/velocity_controller/src/ct_instantiations.cpp @@ -0,0 +1,16 @@ +// src/ct_instantiations.cpp +// This file exists ONLY to emit Control Toolbox symbols + +//#include +#include +//#define CT_OPTCON_ENABLE_LQR +//#define CT_CORE_ENABLE_CARE + +//#include +//#include + +//#include +//#include + +template class ct::optcon::LQR<8, 3>; +template class ct::optcon::CARE<8, 3>; \ No newline at end of file diff --git a/control/velocity_controller/src/utilities.cpp b/control/velocity_controller/src/utilities.cpp new file mode 100644 index 000000000..69242e7e0 --- /dev/null +++ b/control/velocity_controller/src/utilities.cpp @@ -0,0 +1,143 @@ +#include "velocity_controller/utilities.hpp" +#include "Eigen/Dense" +#include +#include +#include +#include + +angle quaternion_to_euler_angle(double w, double x, double y, double z){ + double ysqr = y * y; + + double t0 = +2.0 * (w * x + y * z); + double t1 = +1.0 - 2.0 * (x * x + ysqr); + double phi = std::atan2(t0, t1); + + double t2 = +2.0 * (w * y - z * x); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + double theta = std::asin(t2); + + double t3 = +2.0 * (w * z + x * y); + double t4 = +1.0 - 2.0 * (ysqr + z * z); + double psi = std::atan2(t3, t4); + + return {phi, theta, psi}; +}; +/* +angle NED_to_BODY(const angle &a,const State &s){ + //TODO tests for illegal angles maybe + Eigen::Vector3d q; + q<pose.pose.orientation.w; + x=rhs->pose.pose.orientation.x; + y=rhs->pose.pose.orientation.y; + z=rhs->pose.pose.orientation.z; + + auto [r,p,y_]=quaternion_to_euler_angle(w, x, y, z); + roll=r; + pitch=p; + yaw=y_; + + //angular velocity + roll_rate=rhs->twist.twist.angular.x; + pitch_rate=rhs->twist.twist.angular.y; + yaw_rate=rhs->twist.twist.angular.z; + //velocity + surge = rhs->twist.twist.linear.x; + sway = rhs->twist.twist.linear.y; + heave = rhs->twist.twist.linear.z; + + return (*this); + +} + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw){ + double cy = cos(yaw * 0.5); + double sy = sin(yaw * 0.5); + double cp = cos(pitch * 0.5); + double sp = sin(pitch * 0.5); + double cr = cos(roll * 0.5); + double sr = sin(roll * 0.5); + + geometry_msgs::msg::Quaternion q; + q.w = cr * cp * cy + sr * sp * sy; + q.x = sr * cp * cy - cr * sp * sy; + q.y = cr * sp * cy + sr * cp * sy; + q.z = cr * cp * sy - sr * sp * cy; + + return q; +} + +angle angle_NED_to_body( double roll_des, double pitch_des, double yaw_des,double roll, double pitch, double yaw) +{ + double cr = std::cos(roll), sr = std::sin(roll); + double cp = std::cos(pitch), sp = std::sin(pitch); + double cy = std::cos(yaw), sy = std::sin(yaw); + + // R_current: NED to body for current attitude + Eigen::Matrix3d R_current; + R_current << cp*cy, cp*sy, -sp, + sr*sp*cy - cr*sy, sr*sp*sy + cr*cy, sr*cp, + cr*sp*cy + sr*sy, cr*sp*sy - sr*cy, cr*cp; + + double cr_d = std::cos(roll_des), sr_d = std::sin(roll_des); + double cp_d = std::cos(pitch_des), sp_d = std::sin(pitch_des); + double cy_d = std::cos(yaw_des), sy_d = std::sin(yaw_des); + + // R_desired: NED to body for desired attitude + Eigen::Matrix3d R_desired; + R_desired << cp_d*cy_d, cp_d*sy_d, -sp_d, + sr_d*sp_d*cy_d - cr_d*sy_d, sr_d*sp_d*sy_d + cr_d*cy_d, sr_d*cp_d, + cr_d*sp_d*cy_d + sr_d*sy_d, cr_d*sp_d*sy_d - sr_d*cy_d, cr_d*cp_d; + + // Error rotation matrix: how much to rotate in body to reach desired + // R_error = R_desired * R_current^T + Eigen::Matrix3d R_error = R_desired * R_current.transpose(); + + // Extract euler angles from R_error — this gives the error in body frame + double pitch_err = std::asin(-R_error(2, 0)); + double roll_err = std::atan2(R_error(2, 1), R_error(2, 2)); + double yaw_err = std::atan2(R_error(1, 0), R_error(0, 0)); + + return {roll_err, pitch_err, yaw_err}; +} + +angle State::get_angle(){ + return {roll,pitch,yaw}; +} +Guidance_data& Guidance_data::operator=(const vortex_msgs::msg::LOSGuidance::SharedPtr& msg){ + surge=msg->surge; + pitch=msg->pitch; + yaw=msg->yaw; + return *this; +} diff --git a/control/velocity_controller/src/velocity_controller.cpp b/control/velocity_controller/src/velocity_controller.cpp new file mode 100644 index 000000000..037730231 --- /dev/null +++ b/control/velocity_controller/src/velocity_controller.cpp @@ -0,0 +1,391 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "velocity_controller/velocity_controller.hpp" +#include +#include "geometry_msgs/msg/wrench_stamped.hpp" +#include "std_msgs/msg/float64_multi_array.hpp" +#include "std_msgs/msg/bool.hpp" +#include "velocity_controller/NMPC_setup.hpp" +#include "velocity_controller/PID_setup.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" +#include "vortex/utils/math.hpp" +#include "velocity_controller/utilities.hpp" +#include + + +//Konstruktør +Velocity_node::Velocity_node() : rclcpp_lifecycle::LifecycleNode("velocity_controller_lifecycle"), lqr_controller(), pub_QoS(10), sub_QoS(10) +{ + get_new_parameters(); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + //TODO: dont need to save the Q3 R3 and the other matries, just use #define + //NMPC controller + NMPC.set_matrices(Q3,R3, inertia_matrix, max_force, dampening_matrix_low, dampening_matrix_high); + NMPC.set_interval(publish_rate/1000.0); + //NMPC.initialize_MPC(); + //NMPC acados controller + NMPC_acados.init(); + NMPC_acados.set_max_force(max_force); + std::vector W=Q2; + W.insert(W.end(),R2.begin(),R2.end()); + std::vector We=Q2; + NMPC_acados.set_weights(W, We); + + + //Controllers + PID_surge.set_output_limits(-max_force, max_force); + PID_pitch.set_output_limits(-max_force, max_force); + PID_yaw.set_output_limits(-max_force, max_force); + PID_surge.set_parameters(surge_params,publish_rate/1000.0); + PID_pitch.set_parameters(pitch_params,publish_rate/1000.0); + PID_yaw.set_parameters(yaw_params,publish_rate/1000.0); + + if(!lqr_controller.set_matrices(Q,R,inertia_matrix,max_force,dampening_matrix_low,dampening_matrix_high)||!lqr_controller.set_interval(static_cast(publish_rate)/1000)){ + controller_type=1; + RCLCPP_INFO(this->get_logger(),"Switching to PID"); + }; + if(auto_start){ + startup_timer_=create_wall_timer(std::chrono::milliseconds(0), [this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);}); + } + RCLCPP_INFO(this->get_logger(), "Velocity control node has been started."); + + return; +} + + +void Velocity_node::calc_thrust() +{ + if (odometry_dropout_guard){ + publish_counter++; + if(publish_counter>=100){ + reset_controllers(); + RCLCPP_WARN(this->get_logger(),"Odometry dropout, no thrust"); + return; + } + } + //TODO: Do I need ssa here? + angle ref_in_body=angle_NED_to_body({0,vortex::utils::math::ssa(guidance_values.pitch),vortex::utils::math::ssa(guidance_values.yaw)},current_state.get_angle()); + Guidance_data error={guidance_values.surge-current_state.surge,-ref_in_body.thetat,-ref_in_body.psit}; + + if(anti_overshoot){ + if (abs(error.yaw)get_logger(),"Switching to PID"); + } + else{ + Eigen::Vector3d u=lqr_controller.get_thrust(); + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + } + break; + } + case 3:{ + //RCLCPP_INFO(this->get_logger(),"Guidance: %f, %f, %f",guidance_values.surge,guidance_values.pitch,guidance_values.yaw); + Eigen::Matrix u; + if (NMPC.calculate_thrust(guidance_values, current_state)){ + controller_type=1; + RCLCPP_ERROR(this->get_logger(),"Switching to PID"); + rclcpp::shutdown(); + } + else{ + u=NMPC.get_thrust(); + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + RCLCPP_INFO(this->get_logger(),"NMPC: surge: %f, pitch %f, yaw %f",u(0),u(1),u(2)); + } + + break; + } + case 4:{ + std::vectoru; + std::array x_ref={guidance_values.surge,0.0,0.0,guidance_values.pitch,guidance_values.yaw}; //surge, pitch_rate, yaw_rate, pitch, yaw + + NMPC_acados.setReference(x_ref); + std::array state_array={current_state.surge,current_state.sway,current_state.heave,current_state.roll_rate,current_state.pitch_rate,current_state.yaw_rate,current_state.roll,current_state.pitch,current_state.yaw}; + NMPC_acados.setState(state_array); + int status=NMPC_acados.solve_once(); + RCLCPP_INFO(this->get_logger(),"Status %i",status); + if(status){ + + rclcpp::shutdown(); + }; + u=NMPC_acados.getU0(); + thrust_out.wrench.force.x=u[0]; + thrust_out.wrench.torque.y=u[1]; + thrust_out.wrench.torque.z=u[2]; + break; + } + default:{ + //Some crash handling here + RCLCPP_ERROR(this->get_logger(),"Unknown controller set"); + break; + } + } + publisher_thrust->publish(thrust_out); + return; +} + + + +//Callback functions +void Velocity_node::guidance_callback(const vortex_msgs::msg::LOSGuidance::SharedPtr msg_ptr){ + if(reset_on_new_ref){ //On big step changes, reset the controllers to avoid big overshoots + if(abs(msg_ptr->surge-guidance_values.surge)>=0.1) reset_controllers(1); + if (abs(msg_ptr->pitch-guidance_values.pitch)>std::numbers::pi/4)reset_controllers(2); + if (abs(msg_ptr->yaw-guidance_values.yaw)declare_parameter("topics.wrench_input"); + this->topic_thrust = this->get_parameter("topics.wrench_input").as_string(); + this->declare_parameter("topics.guidance.los"); + this->topic_guidance = this->get_parameter("topics.guidance.los").as_string(); + this->declare_parameter("topics.odom"); + this->topic_odometry = this->get_parameter("topics.odom").as_string(); + + //variables + + this->declare_parameter("max_force"); + this->max_force = this->get_parameter("max_force").as_double(); + this->declare_parameter("publish_rate"); + this->publish_rate = this->get_parameter("publish_rate").as_int(); + this->declare_parameter("controller_type"); + this->controller_type=this->get_parameter("controller_type").as_int(); + + //PID Params + this->declare_parameter>("PID_params.surge"); + surge_params=this->get_parameter("PID_params.surge").as_double_array(); + this->declare_parameter>("PID_params.pitch"); + pitch_params=this->get_parameter("PID_params.pitch").as_double_array(); + this->declare_parameter>("PID_params.yaw"); + yaw_params=this->get_parameter("PID_params.yaw").as_double_array(); + + //LQR Parameters + + this->declare_parameter>("LQR_params.Q"); + Q=this->get_parameter("LQR_params.Q").as_double_array(); + this->declare_parameter>("LQR_params.R"); + R=this->get_parameter("LQR_params.R").as_double_array(); + this->declare_parameter>("physical.mass_matrix"); + inertia_matrix=this->get_parameter("physical.mass_matrix").as_double_array(); + + //D + this->declare_parameter>("dampening_matrix_low"); + this->declare_parameter>("dampening_matrix_high"); + this->dampening_matrix_low=this->get_parameter("dampening_matrix_low").as_double_array(); + this->dampening_matrix_high=this->get_parameter("dampening_matrix_high").as_double_array(); + + //NMPC acados Parameters + this->declare_parameter>("NMPCA_params.Q"); + this->declare_parameter>("NMPCA_params.R"); + Q2=this->get_parameter("NMPCA_params.Q").as_double_array(); + R2=this->get_parameter("NMPCA_params.R").as_double_array(); + //NMPC + + this->declare_parameter>("NMPC_params.Q"); + this->declare_parameter>("NMPC_params.R"); + Q3=this->get_parameter("NMPC_params.Q").as_double_array(); + R3=this->get_parameter("NMPC_params.R").as_double_array(); + + +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Velocity_node::on_configure(const rclcpp_lifecycle::State &){ + RCLCPP_INFO(get_logger(), "Configure VC"); + + // Publishers + publisher_thrust = create_publisher(topic_thrust, pub_QoS); + + //Subscribers + subscriber_Odometry = this->create_subscription( topic_odometry,sub_QoS, std::bind(&Velocity_node::odometry_callback,this,std::placeholders::_1)); + subscriber_guidance = this->create_subscription( topic_guidance,sub_QoS,std::bind(&Velocity_node::guidance_callback,this, std::placeholders::_1)); + //Timer + if(first_start&&auto_start){ + startup_timer_=create_wall_timer(std::chrono::milliseconds(0),[this](){startup_timer_->cancel(); trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);}); + } + first_start=false; + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_activate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Activating..."); + timer_calculation = this->create_wall_timer(std::chrono::milliseconds(publish_rate), std::bind(&Velocity_node::calc_thrust, this)); + auto ret = LifecycleNode::on_activate(state); + + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_deactivate(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Deactivating..."); + auto ret = LifecycleNode::on_deactivate(state); + timer_calculation.reset(); + reset_controllers(); + return ret; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_cleanup(const rclcpp_lifecycle::State &) +{ + RCLCPP_INFO(get_logger(), "Cleaning up..."); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + return CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +Velocity_node::on_shutdown(const rclcpp_lifecycle::State & state) +{ + RCLCPP_INFO(get_logger(), "Shutting down from state %s", state.label().c_str()); + if(timer_calculation) timer_calculation->cancel(); + timer_calculation.reset(); + publisher_thrust.reset(); + subscriber_guidance.reset(); + subscriber_Odometry.reset(); + should_exit_=true; + return CallbackReturn::SUCCESS; +} + +void Velocity_node::reset_controllers(int nr){ + switch (nr) { + case 0: + PID_pitch.reset_controller(); + PID_surge.reset_controller(); + PID_yaw.reset_controller(); + lqr_controller.reset_controller(); + break; + case 1: + PID_surge.reset_controller(); + lqr_controller.reset_controller(1); + + break; + + case 2: + PID_pitch.reset_controller(); + lqr_controller.reset_controller(2); + + break; + + case 3: + PID_yaw.reset_controller(); + lqr_controller.reset_controller(3); + break; + + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto lc_node = std::make_shared(); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(lc_node->get_node_base_interface()); + + while (rclcpp::ok()&&!lc_node->should_exit_){ + exec.spin_some(); + } + //rclcpp::shutdown(); + return 0; +} + +//---------------------------------------------------------------------------------------------------------------- +//Operator overloading for geometry_msgs::msg::WrenchStamped +/* +geometry_msgs::msg::WrenchStamped operator-(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) +{ + geometry_msgs::msg::WrenchStamped result; + result.wrench.force.x = a.wrench.force.x - b.wrench.force.x; + result.wrench.force.y = a.wrench.force.y - b.wrench.force.y; + result.wrench.force.z = a.wrench.force.z - b.wrench.force.z; + result.wrench.torque.x = a.wrench.torque.x - b.wrench.torque.x; + result.wrench.torque.y = a.wrench.torque.y - b.wrench.torque.y; + result.wrench.torque.z = a.wrench.torque.z - b.wrench.torque.z; + return result; +} +geometry_msgs::msg::WrenchStamped operator+(const geometry_msgs::msg::WrenchStamped & a, const geometry_msgs::msg::WrenchStamped & b) +{ + geometry_msgs::msg::WrenchStamped result; + result.wrench.force.x = a.wrench.force.x + b.wrench.force.x; + result.wrench.force.y = a.wrench.force.y + b.wrench.force.y; + result.wrench.force.z = a.wrench.force.z + b.wrench.force.z; + result.wrench.torque.x = a.wrench.torque.x + b.wrench.torque.x; + result.wrench.torque.y = a.wrench.torque.y + b.wrench.torque.y; + result.wrench.torque.z = a.wrench.torque.z + b.wrench.torque.z; + return result; +} +//operator overloading for guidance_data +Guidance_data Guidance_data::operator-(const Guidance_data & b) const +{ + Guidance_data result; + result.surge = this->surge - b.surge; + result.pitch = this->pitch - b.pitch; + result.yaw = this->yaw - b.yaw; + return result; +} + +Guidance_data& Guidance_data::operator=(const std_msgs::msg::Float64MultiArray& msg) +{ + if (msg.data.size()>=3){ + surge=msg.data[0]; + pitch=msg.data[1]; + yaw=msg.data[2]; + } + else{ + //throw std::runtime_error("Guidance message too short, needs at least 3 values"); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Guidance message too short, needs at least 3 values"); + } + return *this; +} +*/ + diff --git a/control/velocity_controller/tests/CMakeLists.txt b/control/velocity_controller/tests/CMakeLists.txt new file mode 100644 index 000000000..0d579c4f3 --- /dev/null +++ b/control/velocity_controller/tests/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.8) +include(GoogleTest) +find_package(ament_cmake_gtest REQUIRED) +#find_package(GTest REQUIRED) +find_package(yaml-cpp REQUIRED) + #find_package(tf2 REQUIRED) + +#Unit test +set(TEST_BINARY_NAME PID_test) + +ament_add_gtest(${TEST_BINARY_NAME} +test_PID.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/PID_setup.cpp +) + +ament_add_gtest(LQR_test +test_LQR.cpp +../src/utilities.cpp +../src/ct_instantiations.cpp +../src/LQR_setup.cpp +) + +#To DO need to clean up the include directories/dependencies and such +target_include_directories(${TEST_BINARY_NAME} PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) +target_include_directories(LQR_test PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) #Need to fix +ament_target_dependencies(${TEST_BINARY_NAME} +rclcpp +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) +ament_target_dependencies(LQR_test + rclcpp + vortex_utils + std_msgs + geometry_msgs + vortex_msgs + nav_msgs +) +target_compile_definitions(LQR_test PRIVATE +YAML_PATH="${PROJECT_SOURCE_DIR}/config/parameters.yaml") + +target_link_libraries(${TEST_BINARY_NAME} +Eigen3::Eigen +ct_optcon +ct_core +) +target_link_libraries(LQR_test + Eigen3::Eigen + ct_optcon + ct_core + yaml-cpp +) + + +#System tests + + +target_link_libraries( + ${TEST_BINARY_NAME} + ${LIB_NAME} + #yaml-cpp + #GTest::GTest + #spdlog::spdlog + Eigen3::Eigen + ct_optcon + ct_core +) +add_executable(test_VC_node +test_VC.cpp +../src/utilities.cpp +) + +target_include_directories(test_VC_node PUBLIC +$ +$ +${EIGEN3_INCLUDE_DIR} +) + + +target_link_libraries(test_VC_node Eigen3::Eigen ct_optcon ct_core) + +ament_target_dependencies(test_VC_node +rclcpp +rclcpp_lifecycle +std_msgs +vortex_msgs +geometry_msgs +nav_msgs +vortex_utils +) + +install(TARGETS test_VC_node +DESTINATION lib/${PROJECT_NAME} +) \ No newline at end of file diff --git a/control/velocity_controller/tests/LQR_test.cpp b/control/velocity_controller/tests/LQR_test.cpp new file mode 100644 index 000000000..392103cff --- /dev/null +++ b/control/velocity_controller/tests/LQR_test.cpp @@ -0,0 +1,55 @@ +#include "rclcpp/rclcpp.hpp" +#include +#include "velocity_controller/LQR_setup.hpp" +#include "ct/optcon/lqr/LQR.hpp" + +class test_LQR_node : public rclcpp::Node{ + public: + double q_surge =75.0, q_pitch= 175.0, q_yaw= 175.0, r_surge= 0.3, r_pitch= 0.4, r_yaw= 0.4, i_surge= 0.3, + i_pitch= 0.4, i_yaw= 0.3, i_weight= 0.5, dt= 0.1; + LQRparameters param={q_surge, q_pitch, q_yaw, r_surge, r_pitch, r_yaw, i_surge, i_pitch, i_yaw, i_weight}; + LQRController controller; + test_LQR_node():Node("test_LQR_node"), controller(){ + RCLCPP_INFO(this->get_logger(),"LQR test node started"); + Eigen::Matrix Q; + Eigen::VectorXd qdiag(8); + qdiag << 75, 175, 175, 1, 0.3, 0.4, 0.3, 1; + Q.diagonal() = qdiag; + Eigen::Matrix R; + Eigen::VectorXd rdiag(3); + rdiag << 0.3, 0.4, 0.4; + R.diagonal() = rdiag; + Eigen::Matrix A=(Eigen::Matrix()<<5,7,23,0,0,0,1,1,0,45,21,4,3,4,3,2,0,23,1,7,6,5,5,7,5,7,6,3,5,7,0,9,2,2,3,2,1,0,3,7,0,0,8,7,6,5,8,5,4,7,1,5,6,2,4,7,6,2,4,5,2,12,5,6).finished(); + Eigen::Matrix B=(Eigen::Matrix()<<2,0,0,3,0,2,0,3,0,1,2,0,3,4,0,3,5,0,6,3,4,1,2,3).finished(); + /*Eigen::Matrix3d inertia_matrix=(Eigen::Matrix3d()<<30.0, 0.6, 0.0, 0.6, 1.629, 0.0, 0.0, 0.0, 1.729).finished(); + Eigen::Matrix3d inertia_matrix_inv=inertia_matrix.inverse(); + Eigen::Matrix3d coriolis_matrix=(Eigen::Matrix3d()<<0.2,-30*2*0.01,-30*2*0.0,30 * 2*0.01,0,1.629 * 2,30 * 2*0.01,1.769 * 2,0).finished(); + Eigen::Matrix3d system_matrix=inertia_matrix.inverse()*coriolis_matrix; + Eigen::Matrix augmented_system_matrix =(Eigen::Matrix()< augmented_input_matrix=(Eigen::Matrix()<< inertia_matrix_inv(0,0),inertia_matrix_inv(0,1),inertia_matrix_inv(0,2),0,0,0, + inertia_matrix_inv(1,0),inertia_matrix_inv(1,1),inertia_matrix_inv(1,2),0,0,0, + inertia_matrix_inv(2,0),inertia_matrix_inv(2,1),inertia_matrix_inv(2,2),0,0,0).finished();*/ + ct::optcon::LQR<8,3> lqr_solver; + Eigen::Matrix K; + lqr_solver.compute(Q,R,A,B,K,true,false); + RCLCPP_INFO(this->get_logger(),"LQR Gain K matrix:"); + RCLCPP_INFO(this->get_logger(),"\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f\n%f %f %f %f %f %f %f %f", + K(0,0),K(0,1),K(0,2),K(0,3),K(0,4),K(0,5),K(0,6),K(0,7), + K(1,0),K(1,1),K(1,2),K(1,3),K(1,4),K(1,5),K(1,6),K(1,7), + K(2,0),K(2,1),K(2,2),K(2,3),K(2,4),K(2,5),K(2,6),K(2,7)); + + } +}; + +int main(int argc, char const *argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/control/velocity_controller/tests/test_LQR.cpp b/control/velocity_controller/tests/test_LQR.cpp new file mode 100644 index 000000000..9317da845 --- /dev/null +++ b/control/velocity_controller/tests/test_LQR.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include +#include "velocity_controller/utilities.hpp" +#include + +class LQR_test : public ::testing::Test{ + protected: + double delta=0.0001; + static inline YAML::Node cfg; + LQRController controller; + static void SetUpTestSuite() { + try { + cfg = YAML::LoadFile(YAML_PATH); + } + catch (const YAML::Exception& e) { + FAIL() << "Failed to load YAML from '" << YAML_PATH << "': " << e.what(); + } + + }; + void SetUp() override{ + controller.set_matrices(cfg["/**"]["ros__parameters"]["LQR_params"]["Q"].as>(),cfg["/**"]["ros__parameters"]["LQR_params"]["R"].as>(),cfg["/**"]["ros__parameters"]["inertia_matrix"].as>(),cfg["/**"]["ros__parameters"]["max_force"].as(),cfg["/**"]["ros__parameters"]["dampening_matrix_low"].as>(),cfg["/**"]["ros__parameters"]["dampening_matrix_high"].as>()); + controller.reset_controller(); + controller.set_interval(0.01); + } + void TearDown() override{ + + } +}; +/* +TEST(LQR,setup){ + LQRController controller; + controller.set_interval(1); + YAML::Node cfg; + ASSERT_NO_THROW(cfg=YAML::LoadFile(YAML_PATH)); + controller.set_matrices(cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],cfg["LQR"]["Q"],100); +}; +*/ +TEST_F(LQR_test,wrong_setup){ + //LQRController controller; + std::vector eight={1,2,3,4,5,6,7,8}; + std::vector six={1,2,3,4,5,6}; + std::vector thirty_six={1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31,32,33,34,35,36}; + std::vector three={1,2,3}; + EXPECT_TRUE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,eight,thirty_six,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(three,three,thirty_six,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,eight,100,thirty_six,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,eight,thirty_six)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,100,thirty_six,eight)); + EXPECT_FALSE(controller.set_matrices(eight,three,thirty_six,-100,thirty_six,thirty_six)); +}; +/* +TEST_F(LQR_test,solve){ + State states{1,1,1,2,2,2,1,2,1}; + Guidance_data value{1,3,2}; + Eigen::Vector result=controller.calculate_thrust(states,value); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); +};*/ +TEST_F(LQR_test,Direction){ + Guidance_data value; + State state{}; + value.surge=0.2; + controller.calculate_thrust(state,value); + Eigen::Vector result=controller.get_thrust(); + EXPECT_TRUE(result(0)>0); + +} +TEST_F(LQR_test,zero_input){ + State states{}; + states.surge=1; + states.yaw=0.2; + states.pitch=0.3; + Guidance_data value; + value.surge=1.0; + value.yaw=0.2; + value.pitch=0.3; + controller.calculate_thrust(states,value); + Eigen::Vector result=controller.get_thrust(); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); + controller.reset_controller(); + states.surge=0; + states.pitch=0; + states.yaw=0; + value.surge=0; + value.pitch=0; + value.yaw=0; + controller.calculate_thrust(states, value); + result=controller.get_thrust(); + EXPECT_NEAR(result(0),0,delta); + EXPECT_NEAR(result(1),0,delta); + EXPECT_NEAR(result(2),0,delta); +} + +int main(int argc,char** argv){ + testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +} diff --git a/control/velocity_controller/tests/test_PID.cpp b/control/velocity_controller/tests/test_PID.cpp new file mode 100644 index 000000000..0d449c15f --- /dev/null +++ b/control/velocity_controller/tests/test_PID.cpp @@ -0,0 +1,111 @@ +#include +//#include +//#include +#include +//#include + +class PID_test : public ::testing::Test{ + protected: + double delta=0.0001; + PID_controller PID; + void SetUp() override{ + PID.set_parameters(0,0,0,1); + PID.reset_controller(); + } + void TearDown() override{ + + } + +}; +/* +class Node_test : public ::testing:Test{ + protected: + static void SetUpTestSuite(){ + int argc=0; + char ** argv==nullptr; + rclcpp::init(); + } + static void TearDownTestSuite(){ + rclcpp::shutdown(); + } + +}; + + +*/ +TEST_F(PID_test,reset_controller){ + PID.set_parameters(0,1,0,100); + PID.calculate_thrust(100); + PID.set_parameters(0,1,0,1); + PID.calculate_thrust(0); + PID.reset_controller(); + SCOPED_TRACE("Scenario: reset"); + EXPECT_NEAR(PID.get_output(),0,delta); + PID.calculate_thrust(0); + SCOPED_TRACE("Scenario: reset2"); + EXPECT_NEAR(PID.get_output(),0,delta); +} +TEST_F(PID_test,P){ + PID.set_parameters(1,0,0,1); + EXPECT_NEAR(PID.calculate_thrust(1),1,delta); + EXPECT_NEAR(PID.calculate_thrust(2),2,delta); + PID.set_parameters(1.2,0,0,1); + EXPECT_NEAR(PID.calculate_thrust(-2.2),-2.64,delta); + EXPECT_NEAR(PID.calculate_thrust(-1.5),-1.8,delta); +} +TEST_F(PID_test,I){ + PID.set_parameters(0,1.1,0,1); + PID.calculate_thrust(1); + EXPECT_NEAR(PID.get_output(),0,delta); + PID.calculate_thrust(1); + EXPECT_NEAR(PID.get_output(),1.1,delta); + PID.calculate_thrust(-1); + EXPECT_NEAR(PID.get_output(),2.2,delta); + EXPECT_NEAR(PID.calculate_thrust(0),1.1,delta); + PID.set_output_limits(-101,101); + PID.reset_controller(); + PID.set_parameters(1,1,0,10); + EXPECT_NEAR(PID.calculate_thrust(1000),101,delta); + PID.set_parameters(1,1,0,1); + EXPECT_NEAR(PID.calculate_thrust(0),0,delta); + PID.reset_controller(); + EXPECT_NEAR(PID.calculate_thrust(-10000),-101,delta); + PID.calculate_thrust(-50); + EXPECT_NEAR(PID.calculate_thrust(1),-49,delta); +} +TEST_F(PID_test,D){ + +} +TEST_F(PID_test,illegal_inputs){ + double temp=PID.get_output(); + PID.set_parameters(1,1,0,0); + + EXPECT_FALSE(PID.calculate_thrust(1)); + EXPECT_NEAR(PID.get_output(),temp,delta); + EXPECT_FALSE(PID.set_output_limits(1,-1)); + PID.set_parameters(1,1,0,-1); + + EXPECT_FALSE(PID.calculate_thrust(1)); +} +/* +TEST(PID,BASIC){ + PID_controller foo (1,1,0); + ASSERT_NO_THROW(foo.set_output_limits(-10,100)); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_TRUE(foo.get_output()>0); + EXPECT_NEAR(foo.get_output(),1,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1,1)); + EXPECT_NEAR(foo.get_output(),2,0.01); + ASSERT_NO_THROW(foo.calculate_thrust(1000000,1)); + EXPECT_EQ(foo.get_output(),100); + ASSERT_NO_THROW(foo.calculate_thrust(-100000,1)); + EXPECT_EQ(foo.get_output(),-10); +} +*/ + + + +int main(int argc,char** argv){ + testing::InitGoogleTest(&argc,argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/control/velocity_controller/tests/test_VC.cpp b/control/velocity_controller/tests/test_VC.cpp new file mode 100644 index 000000000..93dced1f4 --- /dev/null +++ b/control/velocity_controller/tests/test_VC.cpp @@ -0,0 +1,72 @@ +#include +#include +#include +#include +#include +#include +///#include "velocity_controller/PID_setup.hpp" +#include "test_VC.hpp" +#include +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" +#include "velocity_controller/utilities.hpp" +//#include "velocity_controller/velocity_controller.hpp" +//#include "LQR_setup.hpp" +//Denne noden er kun for å teste velocity_controller noden + +test_VC::test_VC() : Node("test_VC_node") +{ + this->declare_parameter("topics.guidance.los"); + this->declare_parameter("topics.odom"); + this->topic_guidance=this->get_parameter("topics.guidance.los").as_string(); + this->topic_odometry=this->get_parameter("topics.odom").as_string(); + rclcpp::QoS pub_QoS(10); + pub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + + publisher_guidance = this->create_publisher(topic_guidance, pub_QoS); + publisher_state = this->create_publisher("/state", pub_QoS); + + rclcpp::QoS sub_QoS(10); + sub_QoS.keep_last(10).reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT).durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + subscription_state = this->create_subscription( + topic_odometry,sub_QoS, + std::bind(&test_VC::odometry_callback,this,std::placeholders::_1)); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(200), + std::bind(&test_VC::send_guidance, this)); + clock_ = this->get_clock(); + RCLCPP_INFO(this->get_logger(), "Test_VC node has been started"); + + +} + +void test_VC::send_guidance() +{ + time1+=0.2; + reference_msg.yaw=std::numbers::pi*sin(time1*std::numbers::pi/9); + //reference_msg.pitch=0.3*sin(time1*std::numbers::pi/9); + reference_msg.surge=0.20;reference_msg.pitch=-0.4;//reference_msg.yaw=0.0; //Surge, pitch, yaw + publisher_guidance->publish(reference_msg); +} + +void test_VC::odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr){ + vortex_msgs::msg::LOSGuidance msg; + angle temp=quaternion_to_euler_angle(msg_ptr->pose.pose.orientation.w, msg_ptr->pose.pose.orientation.x, msg_ptr->pose.pose.orientation.y, msg_ptr->pose.pose.orientation.z); + msg.set__pitch(temp.thetat); + msg.set__yaw(temp.psit); + msg.set__surge(msg_ptr->twist.twist.linear.x); + publisher_state->publish(msg); + +} +int main(int argc, char const *argv[]) +{ + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + + + \ No newline at end of file diff --git a/control/velocity_controller/tests/test_VC.hpp b/control/velocity_controller/tests/test_VC.hpp new file mode 100644 index 000000000..ff82b8cd5 --- /dev/null +++ b/control/velocity_controller/tests/test_VC.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include +#include +#include +#include "velocity_controller/PID_setup.hpp" +#include "velocity_controller/velocity_controller.hpp" +#include +#include +#include "vortex_msgs/msg/los_guidance.hpp" + +class test_VC : public rclcpp::Node{ + public: + test_VC(); + //Callback functions + void send_guidance(); + void odometry_callback(const nav_msgs::msg::Odometry::SharedPtr msg_ptr); + + //Variables + + //Subscribers and publishers + rclcpp::Publisher::SharedPtr publisher_guidance; + rclcpp::Publisher::SharedPtr publisher_state; + rclcpp::Subscription::SharedPtr subscription_state; + //Timers + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Clock::SharedPtr clock_; + //Messages + vortex_msgs::msg::LOSGuidance reference_msg; + + //Topics + std::string topic_guidance; + std::string topic_state="/state"; + std::string topic_odometry; + + + //MSGS + double time1=0; +}; + +geometry_msgs::msg::Quaternion euler_angle_to_quaternion(double roll, double pitch, double yaw); \ No newline at end of file diff --git a/dependencies.repos b/dependencies.repos index a62354be6..b925a14d8 100644 --- a/dependencies.repos +++ b/dependencies.repos @@ -5,6 +5,9 @@ repositories: vortex-vkf: type: git url: https://github.com/vortexntnu/vortex-vkf.git + control-toolbox: + type: git + url: https://github.com/ethz-adrl/control-toolbox.git vortex-utils: type: git url: https://github.com/vortexntnu/vortex-utils.git diff --git a/guidance/los_guidance/CMakeLists.txt b/guidance/los_guidance/CMakeLists.txt index 90b51de3a..00c00a391 100644 --- a/guidance/los_guidance/CMakeLists.txt +++ b/guidance/los_guidance/CMakeLists.txt @@ -15,22 +15,29 @@ find_package(rclcpp_action REQUIRED) find_package(vortex_msgs REQUIRED) find_package(vortex_utils_ros REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(Eigen3 REQUIRED) find_package(spdlog REQUIRED) find_package(fmt REQUIRED) +find_package(yaml-cpp REQUIRED) + include_directories(include) set(LIB_NAME los_guidance_lib) add_library(${LIB_NAME} SHARED - src/los_guidance.cpp + src/lib/proportional_los.cpp + src/lib/integral_los.cpp + src/lib/adaptive_los.cpp src/los_guidance_ros.cpp + src/lib/vector_field_los.cpp ) ament_target_dependencies(${LIB_NAME} rclcpp rclcpp_action geometry_msgs + nav_msgs vortex_msgs vortex_utils_ros Eigen3 @@ -42,7 +49,18 @@ add_executable(los_guidance_node src/los_guidance_node.cpp ) -target_link_libraries(los_guidance_node ${LIB_NAME}) +ament_target_dependencies(los_guidance_node + rclcpp + rclcpp_action + geometry_msgs + vortex_msgs + vortex_utils_ros +) + +target_link_libraries(los_guidance_node + ${LIB_NAME} + yaml-cpp +) install(TARGETS ${LIB_NAME} @@ -59,7 +77,7 @@ install(TARGETS install( DIRECTORY include/ - DESTINATION include + DESTINATION include/ ) install(DIRECTORY @@ -68,6 +86,11 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +install(PROGRAMS + scripts/test_scenarios.py + DESTINATION share/${PROJECT_NAME}/scripts +) + if(BUILD_TESTING) add_subdirectory(test) endif() diff --git a/guidance/los_guidance/README.md b/guidance/los_guidance/README.md index 33c616c59..af54fdb90 100644 --- a/guidance/los_guidance/README.md +++ b/guidance/los_guidance/README.md @@ -1,22 +1,181 @@ -## ALSO Guidance Law for 3D Path Following +# 3D LOS Guidance Library -The guidance law gives calculates the desired heading angle $\psi_d$ and desired pitch angle $\theta_d$. The crab angles $\beta_c$ and $\alpha_c$ are estimated adaptively. The guidance law looks like +This package implements several **Line-of-Sight (LOS) guidance algorithms** for **3D path following**. + +The guidance system computes the **desired yaw** $\psi_d$ and **desired pitch** $\theta_d$ that allow a vehicle to follow a path between waypoints. + +The node receives + +- vehicle pose +- waypoint information + +and computes guidance references based on the selected LOS algorithm. The resulting guidance commands consist of + +- desired yaw +- desired pitch +- desired surge velocity. + +The vehicle surge speed is kept **constant during path following** and is defined in the configuration file using the parameter `u_desired`. + +--- + +# Implemented LOS Methods + +The library supports four LOS guidance algorithms. + +| Mode | Method | +|-----|------| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +The guidance method can be **changed during runtime** using a ROS service. + +--- + + +# Sending a LOS Goal + +A waypoint can be sent to the guidance node using the action interface: + +``` +ros2 action send_goal /orca/los_guidance \ +vortex_msgs/action/LOSGuidance \ +"{goal: {header: {frame_id: world_ned}, point: {x: 0.0, y: 0.0, z: 0.0}}}" +``` + +This command instructs the guidance node to start following a path toward the waypoint. + +--- + +# Switching LOS Method + +The active LOS guidance method can be changed during runtime. + +``` +ros2 service call /orca/set_los_mode \ +vortex_msgs/srv/SetLosMode "{mode: X}" +``` + +Where + +| X | Method | +|---|---| +| 0 | Proportional LOS | +| 1 | Integral LOS | +| 2 | Adaptive LOS | +| 3 | Vector Field LOS | + +Example: + +``` +ros2 service call /orca/set_los_mode vortex_msgs/srv/SetLosMode "{mode: 2}" +``` + +This switches the guidance system to **Adaptive LOS**. + +--- +## Path Geometry + +The path between two waypoints defines the **path direction angles** used by the guidance law. ```math -\psi_d = \pi_h - \hat{\beta}_c - \tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) +\pi_h = +\text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n - x_i^n) +``` + +```math +\pi_v = +\text{atan2} +\left( +-(z_{i+1}^n - z_i^n), +\sqrt{(x_{i+1}^n - x_i^n)^2 + +(y_{i+1}^n - y_i^n)^2} +\right) +``` + +where + +- $\pi_h$ is the **horizontal path angle (azimuth angle)** +- $\pi_v$ is the **vertical path angle (elevation angle)** + +These angles define the **direction of the current path segment** between two waypoints. + +Additionally + +- $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down (NED) frame +- $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint. + +--- + +## Path Frame Errors + +The along-, cross- and vertical-track errors in the path-tangential frame are found by + +```math +\begin{bmatrix} +x_e^p \\ +y_e^p \\ +z_e^p +\end{bmatrix} += +\mathbf{R}_{y,\pi_v}^\top +\mathbf{R}_{z,\pi_h}^\top +\left( +\begin{bmatrix} +x^n \\ +y^n \\ +z^n +\end{bmatrix} +- +\begin{bmatrix} +x_i^n \\ +y_i^n \\ +z_i^n +\end{bmatrix} +\right) +``` + +where + +- $x_e^p$ is the along-track error +- $y_e^p$ is the cross-track error +- $z_e^p$ is the vertical-track error + +and + +- $P^n = (x^n, y^n, z^n)$ is the current position of the vehicle. + +--- + +## Adaptive LOS (ALSO) +Adaptive LOS estimates **crab angles caused by disturbances** such as ocean currents or wind. + +```math +\psi_d = \pi_h - \hat{\beta}_c - \tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) ``` ```math -\dot{\hat{\beta}}_c = \gamma_h \frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} y_e^p +\dot{\hat{\beta}}_c = +\gamma_h +\frac{\Delta_h}{\sqrt{\Delta_h^2 + (y_e^p)^2}} +y_e^p ``` ```math -\theta_d = \pi_v + \hat{\alpha}_c + \tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) +\theta_d = +\pi_v + +\hat{\alpha}_c + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` ```math -\dot{\hat{\alpha}}_c = \gamma_v \frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} z_e^p +\dot{\hat{\alpha}}_c = +\gamma_v +\frac{\Delta_v}{\sqrt{\Delta_v^2 + (z_e^p)^2}} +z_e^p ``` where @@ -27,27 +186,193 @@ where - $y_e^p$ is the cross-track error - $z_e^p$ is the vertical-track error -The azimuth angle $\pi_v$ and the elevation angle $\pi_h$ can be found by +The terms + +- $\hat{\beta}_c$ is the **estimated horizontal crab angle** +- $\hat{\alpha}_c$ is the **estimated vertical crab angle** + +These angles represent the **estimated disturbance-induced deviation** between the vehicle heading and the actual direction of motion. +They allow the guidance system to **compensate for disturbances such as currents or wind**. + +Adaptive LOS is generally the **most robust method** and works well for + +- curved trajectories +- long paths +- environments with disturbances. + +--- + +## Proportional LOS (PLOS) + +Proportional LOS is the simplest LOS guidance law. ```math -\pi_h = \text{atan2}(y_{i+1}^n - y_i^n, x_{i+1}^n, - x_i^n) +\psi_d = +\pi_h - +\tan^{-1}\left(\frac{y_e^p}{\Delta_h}\right) ``` + ```math -\pi_v = \text{atan2}(-(z_{i+1}^n - z_i^n), \sqrt{(x_{i+1}^n - x_i^n)^2 + (y_{i+1}^n - y_i^n)^2}) +\theta_d = +\pi_v + +\tan^{-1}\left(\frac{z_e^p}{\Delta_v}\right) ``` -where $P_i^n = (x_i^n, y_i^n, z_i^n)$ is the previous waypoint in the north-east-down frame and $P_{i+1}^n = (x_{i+1}^n, y_{i+1}^n, z_{i+1}^n)$ is the next waypoint in north-east-down frame. +### Parameters -The along-, cross- and vertical-track errors in the path-tangential frame are found by +- $\Delta_h$ — horizontal lookahead distance +- $\Delta_v$ — vertical lookahead distance + +The lookahead distances determine **how aggressively the vehicle corrects path errors**. + +- small values → aggressive corrections +- large values → smoother but slower convergence + +### Use case + +PLOS works best for + +- simple waypoint following +- environments with minimal disturbances. + +However, it may suffer from **steady-state tracking error** when disturbances are present. + +--- + +## Integral LOS (ILOS) + +Integral LOS adds integral action to remove steady-state error. ```math -\begin{bmatrix} -x_e^p \\ y_e^p \\ z_e^p -\end{bmatrix} = \mathbf{R}_{y, \pi_v}^\top \mathbf{R}_{z, \pi_h}^\top \left( \begin{bmatrix} -x^n \\ y^n \\ z^n -\end{bmatrix} - \begin{bmatrix} -x_i^n \\ y_i^n \\ z_i^n -\end{bmatrix} -\right) +u_h = +k_{p,h}y_e^p + +k_{i,h}\int y_e^p dt +``` + +```math +u_v = +k_{p,v}z_e^p + +k_{i,v}\int z_e^p dt ``` -where $P^n = (x^n, y^n, z^n)$ is the current position of the drone. + +```math +\psi_d = +\pi_h - +\tan^{-1}(u_h) +``` + +```math +\theta_d = +\pi_v + +\tan^{-1}(u_v) +``` + +### Parameters + +- $k_{p,h}$ — horizontal proportional gain +- $k_{p,v}$ — vertical proportional gain +- $k_{i,h}$ — horizontal integral gain +- $k_{i,v}$ — vertical integral gain + +The integral term allows the controller to **eliminate steady-state cross-track errors** caused by constant disturbances. + +### Use case + +ILOS works well when there are **persistent disturbances**, such as + +- steady ocean currents +- constant wind disturbances. + +--- + +## Vector Field LOS (VF-LOS) + +Vector Field LOS generates a **bounded approach angle** toward the path. + +```math +\psi_d = \pi_h - \chi_h +``` + +```math +\chi_h = +\psi_{max} +\frac{2}{\pi} +\tan^{-1}(k_p y_e^p) +``` + +### Parameters + +- $\psi_{max}$ — maximum allowed approach angle +- $k_p$ — proportional gain controlling path convergence + +The bounded approach angle prevents excessively aggressive heading changes. + +### Use case + +VF-LOS works best for + +- long straight path following +- corridor tracking +- inspection missions along pipelines or cables. + +However it performs worse when the path contains **sharp turns or rapidly changing path segments**. + +--- + +## ROS Interfaces + +| Interface | Name | Type | Message-Type | +|----------|------|------|---------| +| Action Server | `/orca/los_guidance` | Goal input | `vortex_msgs/action/LOSGuidance` | +| Subscriber | `/orca/waypoint` | Waypoint input | `geometry_msgs/PointStamped` | +| Subscriber | `/orca/pose` | Vehicle pose | `geometry_msgs/PoseWithCovarianceStamped` | +| Subscriber | `/orca/odom` | Vehicle velocity | `nav_msgs/Odometry` | +| Publisher | `/orca/guidance/los` | Guidance reference (yaw, pitch, surge) | `vortex_msgs/LOSGuidance` | +| Publisher | `/los_debug` | LOS debug output | `vortex_msgs/LOSGuidance` | +| Publisher | `/state_debug` | Vehicle state debug | `vortex_msgs/LOSGuidance` | + +--- + +## Guidance Node Architecture +The LOS guidance node computes reference commands for the vehicle based on +the current vehicle state and the active waypoint. + +The process works as follows + +1. The node receives the **vehicle pose**. +2. The current **path segment** is defined between two waypoints. +3. The **path geometry** is computed to determine the path direction. +4. The **cross-track and vertical-track errors** are calculated in the path frame. +5. The selected **LOS guidance algorithm** computes desired yaw and pitch. +6. The node publishes the resulting **guidance reference**. + +### Data Flow + +``` +Vehicle Pose + Waypoint + │ + ▼ + Path Geometry + (π_h , π_v angles) + │ + ▼ + Path Frame Errors + (x_e^p , y_e^p , z_e^p) + │ + ▼ + LOS Algorithm + (PLOS / ILOS / ALSO / VF-LOS) + │ + ▼ + Guidance Output + (ψ_d , θ_d , u_desired) +``` + +The resulting guidance command is published as a `vortex_msgs/LOSGuidance` +message containing + +- desired yaw +- desired pitch +- desired surge velocity. + +--- diff --git a/guidance/los_guidance/config/guidance_params.yaml b/guidance/los_guidance/config/guidance_params.yaml index a580cf57c..0a546a4a8 100644 --- a/guidance/los_guidance/config/guidance_params.yaml +++ b/guidance/los_guidance/config/guidance_params.yaml @@ -1,10 +1,38 @@ -/**: - ros__parameters: - los: - lookahead_distance_h: 1.5 - lookahead_distance_v: 0.6 - gamma_h: 0.05 - gamma_v: 0.1 - time_step: 0.01 - u_desired: 0.3 - goal_reached_tol: 1.0 +# Adaptive LOS Parameters +adaptive_los: + lookahead_distance_h: 0.9 + lookahead_distance_v: 1.4 + gamma_h: 0.03 + gamma_v: 0.02 + +# Proportional LOS Parameters +prop_los: + lookahead_distance_h: 0.74 + lookahead_distance_v: 0.8 + +# Integral LOS Parameters +integer_los: + k_p_h: 0.5 + k_p_v: 0.5 + k_i_h: 0.1 + k_i_v: 0.1 + +# Vector Field LOS Parameters +vector_field_los: + max_approach_angle_h: 1.0 + max_approach_angle_v: 1.0 + k_p_h: 1.5 + k_p_v: 0.9 + +# Common Guidance Parameters +common: + active_los_method: 2 # 0: Adaptive LOS, 1: Proportional LOS, 2: Integral LOS, 3: Vector Field LOS + u_desired: 0.3 + goal_reached_tol: 0.15 + max_pitch_angle: 0.96 # 55 degrees + slow_down_distance: 0.40 + +# Debug Settings +debug: + enable_debug: true + debug_topic_name: "/los_guidance_debug" diff --git a/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp new file mode 100644 index 000000000..1f9784178 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/adaptive_los.hpp @@ -0,0 +1,57 @@ +#ifndef LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ +#define LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +// Parameter Structure +struct AdaptiveLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; + double gamma_h{}; + double gamma_v{}; + double time_step{}; +}; + +// Adaptive LOS Guidance Class +class AdaptiveLOSGuidance { + public: + // Constructor / Destructor + explicit AdaptiveLOSGuidance(const AdaptiveLosParams& params); + ~AdaptiveLOSGuidance() = default; + + // Main Output Calculation + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + // Internal Update Functions + void update_angles(const types::Inputs& inputs); + const types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + void update_adaptive_estimates( + const types::CrossTrackError& cross_track_error); + + // Parameters + AdaptiveLosParams params_{}; + + // Rotation Matrices + Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); + Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); + + // Path Angles + double pi_h_{}; + double pi_v_{}; + + // Adaptive Estimates + double beta_c_hat_ = 0.0; + double alpha_c_hat_ = 0.0; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__ADAPTIVE_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp new file mode 100644 index 000000000..bde7d0d5f --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/integral_los.hpp @@ -0,0 +1,55 @@ +#ifndef LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +// Parameter Structure +struct IntegralLosParams { + double k_p_h{}; + double k_p_v{}; + double k_i_h{}; + double k_i_v{}; + double time_step{}; +}; + +// Integral LOS Guidance Class +class IntegralLOSGuidance { + public: + // Constructor / Destructor + explicit IntegralLOSGuidance(const IntegralLosParams& params); + ~IntegralLOSGuidance() = default; + + // Main Output Calculation + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + // Internal Update Functions + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs); + + // Parameters + IntegralLosParams m_params{}; + + // Integral States + double int_h{}; + double int_v{}; + + // Path Angles + double pi_h_{}; + double pi_v_{}; + + // Rotation Representation + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__INTEGRAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp new file mode 100644 index 000000000..a2e9663e1 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/proportional_los.hpp @@ -0,0 +1,48 @@ +#ifndef LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ +#define LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +// Parameter Structure +struct ProportionalLosParams { + double lookahead_distance_h{}; + double lookahead_distance_v{}; +}; + +// Proportional LOS Guidance Class +class ProportionalLOSGuidance { + public: + // Constructor / Destructor + explicit ProportionalLOSGuidance(const ProportionalLosParams& params); + ~ProportionalLOSGuidance() = default; + + // Main Output Calculation + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + // Internal Update Functions + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + // Parameters + ProportionalLosParams m_params{}; + + // Path Angles + double pi_h_{0.0}; + double pi_v_{0.0}; + + // Rotation Representation + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__PROPORTIONAL_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/types.hpp b/guidance/los_guidance/include/los_guidance/lib/types.hpp new file mode 100644 index 000000000..52aab3d48 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/types.hpp @@ -0,0 +1,60 @@ +#ifndef LOS_GUIDANCE__LIB__TYPES_HPP_ +#define LOS_GUIDANCE__LIB__TYPES_HPP_ + +#include +#include +#include +#include +#include + +namespace vortex::guidance::los::types { + +// Point Representation +struct Point { + double x{}; + double y{}; + double z{}; + + // Point Operations + Point operator-(const Point& other) const { + return Point{x - other.x, y - other.y, z - other.z}; + } + + // Conversion Functions + Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } + + static Point point_from_ros(const geometry_msgs::msg::Point& msg) { + return Point{msg.x, msg.y, msg.z}; + } +}; + +// Cross Track Error +struct CrossTrackError { + double x_e{}; + double y_e{}; + double z_e{}; + + inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { + return CrossTrackError{vector.x(), vector.y(), vector.z()}; + } +}; + +// Guidance Outputs +struct Outputs { + double psi_d{}; + double theta_d{}; +}; + +// Guidance Inputs +struct Inputs { + Point prev_point{}; + Point next_point{}; + Point current_position{}; +}; + +// Active LOS Method +enum class ActiveLosMethod { PROPORTIONAL, INTEGRAL, ADAPTIVE, VECTOR_FIELD }; + +} // namespace vortex::guidance::los::types + +#endif // LOS_GUIDANCE__LIB__TYPES_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp new file mode 100644 index 000000000..60d332929 --- /dev/null +++ b/guidance/los_guidance/include/los_guidance/lib/vector_field_los.hpp @@ -0,0 +1,51 @@ +#ifndef LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ +#define LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ + +#include +#include +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +// Parameter Structure +struct VectorFieldLosParams { + double max_approach_angle_h{}; + double max_approach_angle_v{}; + double k_p_h{}; + double k_p_v{}; + double time_step{}; +}; + +// Vector Field LOS Guidance Class +class VectorFieldLOSGuidance { + public: + // Constructor / Destructor + explicit VectorFieldLOSGuidance(const VectorFieldLosParams& params); + ~VectorFieldLOSGuidance() = default; + + // Main Output Calculation + types::Outputs calculate_outputs(const types::Inputs& inputs); + + private: + // Internal Update Functions + void update_angles(const types::Inputs& inputs); + types::CrossTrackError calculate_crosstrack_error( + const types::Inputs& inputs) const; + + // Parameters + VectorFieldLosParams m_params{}; + + // Path Angles + double pi_h_{0.0}; + double pi_v_{0.0}; + + // Rotation Representation + Eigen::AngleAxisd rotation_y_{0.0, Eigen::Vector3d::UnitY()}; + Eigen::AngleAxisd rotation_z_{0.0, Eigen::Vector3d::UnitZ()}; +}; + +} // namespace vortex::guidance::los + +#endif // LOS_GUIDANCE__LIB__VECTOR_FIELD_LOS_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance.hpp b/guidance/los_guidance/include/los_guidance/los_guidance.hpp deleted file mode 100644 index 71ddfdb11..000000000 --- a/guidance/los_guidance/include/los_guidance/los_guidance.hpp +++ /dev/null @@ -1,77 +0,0 @@ -#ifndef LOS_GUIDANCE__LOS_GUIDANCE_HPP_ -#define LOS_GUIDANCE__LOS_GUIDANCE_HPP_ - -#include - -namespace vortex::guidance { - -namespace LOS { - -struct Point { - double x{}; - double y{}; - double z{}; - - Point operator-(const Point& other) const { - return Point{x - other.x, y - other.y, z - other.z}; - } - - Eigen::Vector3d as_vector() const { return Eigen::Vector3d(x, y, z); } -}; - -struct CrossTrackError { - double x_e{}; - double y_e{}; - double z_e{}; - - inline static CrossTrackError from_vector(const Eigen::Vector3d& vector) { - return CrossTrackError{vector.x(), vector.y(), vector.z()}; - } -}; - -struct Params { - double lookahead_distance_h{}; - double lookahead_distance_v{}; - double gamma_h{}; - double gamma_v{}; - double time_step{}; -}; - -} // namespace LOS - -/** - * @brief Adaptive Line-of-Sight (LOS) guidance algorithm based on slide 113 - * in "Fossen 2024 Lecture on 2D and 3D path-following control". - */ -class AdaptiveLOSGuidance { - public: - explicit AdaptiveLOSGuidance(const LOS::Params& params); - ~AdaptiveLOSGuidance() = default; - - void update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point); - - LOS::CrossTrackError calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const; - - double calculate_psi_d(const double& y_e) const; - - double calculate_theta_d(const double& z_e) const; - - void update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error); - - private: - LOS::Params params_; - Eigen::Matrix3d rotation_y_ = Eigen::Matrix3d::Zero(); - Eigen::Matrix3d rotation_z_ = Eigen::Matrix3d::Zero(); - double pi_h_{}; - double pi_v_{}; - double beta_c_hat_{}; - double alpha_c_hat_{}; -}; - -} // namespace vortex::guidance - -#endif // LOS_GUIDANCE__LOS_GUIDANCE_HPP_ diff --git a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp index 623dac5da..a996c42b9 100644 --- a/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp +++ b/guidance/los_guidance/include/los_guidance/los_guidance_ros.hpp @@ -1,119 +1,130 @@ #ifndef LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ #define LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ +#include #include #include #include -#include -#include +#include #include #include +#include #include +#include #include #include -#include "los_guidance.hpp" +#include + +#include +#include + +#include "los_guidance/lib/adaptive_los.hpp" +#include "los_guidance/lib/integral_los.hpp" +#include "los_guidance/lib/proportional_los.hpp" +#include "los_guidance/lib/types.hpp" +#include "los_guidance/lib/vector_field_los.hpp" -namespace vortex::guidance { +namespace vortex::guidance::los { -class LOSGuidanceNode : public rclcpp::Node { +// LOS Guidance ROS Node +class LosGuidanceNode : public rclcpp::Node { public: - LOSGuidanceNode(); + // Constructor + LosGuidanceNode(); private: - // @brief Set the subscribers and publishers + // Setup Functions void set_subscribers_and_publisher(); - - // @brief Set the action server void set_action_server(); + void set_service_server(); - // @brief Set the adaptive LOS guidance parameters - void set_adaptive_los_guidance(); + // Configuration Functions + void set_adaptive_los_guidance(YAML::Node config); + void set_proportional_los_guidance(YAML::Node config); + void set_integral_los_guidance(YAML::Node config); + void set_vector_field_guidance(YAML::Node config); + YAML::Node get_los_config(std::string yaml_file_path); + void parse_common_config(YAML::Node common_config); - // @brief Callback for the waypoint topic - // @param msg The reference message + // Callback Functions void waypoint_callback( const geometry_msgs::msg::PointStamped::SharedPtr msg); - - // @brief Callback for the pose topic - // @param msg The pose message void pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg); - // @brief Handle the goal request - // @param uuid The goal UUID - // @param goal The goal message - // @return The goal response + // Action Server Functions rclcpp_action::GoalResponse handle_goal( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); - - // @brief Handle the cancel request - // @param goal_handle The goal handle - // @return The cancel response rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle); - - // @brief Handle the accepted request - // @param goal_handle The goal handle void handle_accepted(const std::shared_ptr> goal_handle); - - // @brief Execute the goal - // @param goal_handle The goal handle void execute(const std::shared_ptr> goal_handle); - // @brief Fill the lost waypoints - // @param goal The goal message - void fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint); + // Service Functions + void set_los_mode( + const std::shared_ptr request, + std::shared_ptr response); + + // Publish Functions + void publish_state_debug( + const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr + current_pose); + vortex_msgs::msg::LOSGuidance fill_los_reference(types::Outputs output, + types::Inputs inputs); - vortex_msgs::msg::LOSGuidance fill_los_reference(); + // State Flags + bool has_active_segment_{false}; + // ROS Interfaces rclcpp_action::Server::SharedPtr action_server_; - + rclcpp::Service::SharedPtr los_mode_service_; rclcpp::Publisher::SharedPtr reference_pub_; - + rclcpp::Publisher::SharedPtr los_debug_pub_; + rclcpp::Publisher::SharedPtr + state_debug_pub_; rclcpp::Subscription::SharedPtr waypoint_sub_; - rclcpp::Subscription< geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_; - + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::TimerBase::SharedPtr reference_pub_timer_; + rclcpp::CallbackGroup::SharedPtr cb_group_; + // Timing and Synchronization std::chrono::milliseconds time_step_; - std::mutex mutex_; + // Action State rclcpp_action::GoalUUID preempted_goal_id_; - std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle_; - rclcpp::CallbackGroup::SharedPtr cb_group_; - - LOS::Point eta_; - - LOS::Point last_point_; - - LOS::Point next_point_; - - std::unique_ptr adaptive_los_guidance_; - - double yaw_d_{}; - - double pitch_d_{}; - + // Guidance State + types::Inputs path_inputs_{}; double u_desired_{}; - double goal_reached_tol_{}; + double max_pitch_angle_{}; + double slow_down_distance_{}; + types::ActiveLosMethod method_{}; + + // Guidance Modules + std::unique_ptr adaptive_los_{}; + std::unique_ptr integral_los_{}; + std::unique_ptr proportional_los_{}; + std::unique_ptr vector_field_los_{}; + + // Debug Data + nav_msgs::msg::Odometry::SharedPtr debug_current_odom_{}; }; -} // namespace vortex::guidance +} // namespace vortex::guidance::los #endif // LOS_GUIDANCE__LOS_GUIDANCE_ROS_HPP_ diff --git a/guidance/los_guidance/launch/guidance_test.launch.py b/guidance/los_guidance/launch/guidance_test.launch.py new file mode 100644 index 000000000..d5dc84073 --- /dev/null +++ b/guidance/los_guidance/launch/guidance_test.launch.py @@ -0,0 +1,163 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + IncludeLaunchDescription, + OpaqueFunction, + TimerAction, +) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from auv_setup.launch_arg_common import ( + declare_drone_and_namespace_args, + resolve_drone_and_namespace, +) + + +def launch_setup(context, *args, **kwargs): + drone, namespace = resolve_drone_and_namespace(context) + test_scenario = LaunchConfiguration("test_scenario").perform(context) + + stonefish_dir = get_package_share_directory("stonefish_sim") + vortex_sim_interface_dir = get_package_share_directory("vortex_sim_interface") + los_guidance_dir = get_package_share_directory("los_guidance") + operation_mode_manager_dir = get_package_share_directory("operation_mode_manager") + velocity_controller_dir = get_package_share_directory('velocity_controller') + + stonefish_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, "launch", "simulation.launch.py") + ), + launch_arguments={ + "scenario": "default", + "rendering": "true", + }.items(), + ) + + vortex_sim_interface = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + vortex_sim_interface_dir, "launch", "vortex_sim_interface.launch.py" + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + + operation_mode_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + operation_mode_manager_dir, + "launch", + "operation_mode_manager.launch.py", + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + + los_guidance_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(los_guidance_dir, "launch", "los_guidance.launch.py") + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + ) + + velocity_controller_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + velocity_controller_dir, 'launch', 'velocity_controller.launch.py' + ) + ), + launch_arguments={ + "drone": drone, + "namespace": namespace, + }.items(), + + ) + + orca_sim = TimerAction( + period=12.0, + actions=[ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(stonefish_dir, "launch", "drone_sim.launch.py") + ) + ) + ], + ) + + set_autonomy = TimerAction( + period=12.0, + actions=[ + ExecuteProcess( + cmd=[ + "bash", + "-lc", + ( + f"ros2 service call /{namespace}/set_killswitch " + "vortex_msgs/srv/SetKillswitch " + "\"{killswitch_on: false}\" " + "&& " + f"ros2 service call /{namespace}/set_operation_mode " + "vortex_msgs/srv/SetOperationMode " + "\"{requested_operation_mode: {operation_mode: 1}}\"" + ), + ], + output="screen", + ), + ], + ) + + run_test_scenario = TimerAction( + period=20.0, + actions=[ + ExecuteProcess( + cmd=[ + "bash", + "-lc", + ( + f"python3 {os.path.join(los_guidance_dir, 'scripts', 'test_scenarios.py')} " + f"--ros-args -p drone:={drone} -p test_scenario:={test_scenario}" + ), + ], + output="screen", + ) + ], + ) + + return [ + stonefish_sim, + #vortex_sim_interface, + #operation_mode_launch, + los_guidance_launch, + velocity_controller_launch, + orca_sim, + set_autonomy, + run_test_scenario, + ] + + +def generate_launch_description(): + return LaunchDescription( + declare_drone_and_namespace_args(default_drone="orca") + + [ + DeclareLaunchArgument( + "test_scenario", + default_value="square", + description="Scenario to run: square, circle, test_pitch, opposite_point", + ), + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/guidance/los_guidance/launch/los_guidance.launch.py b/guidance/los_guidance/launch/los_guidance.launch.py index ecef79e35..d616e0853 100644 --- a/guidance/los_guidance/launch/los_guidance.launch.py +++ b/guidance/los_guidance/launch/los_guidance.launch.py @@ -14,7 +14,7 @@ def launch_setup(context, *args, **kwargs): drone, namespace = resolve_drone_and_namespace(context) - adapt_params = os.path.join( + guidance_config = os.path.join( get_package_share_directory("los_guidance"), "config", "guidance_params.yaml", @@ -33,7 +33,13 @@ def launch_setup(context, *args, **kwargs): executable="los_guidance_node", name="los_guidance_node", namespace=namespace, - parameters=[drone_params, adapt_params], + parameters=[ + drone_params, + { + "los_config_file": guidance_config, + "time_step": 0.1, + }, + ], output="screen", ) ] diff --git a/guidance/los_guidance/package.xml b/guidance/los_guidance/package.xml index 5ff6218fe..5f696a725 100644 --- a/guidance/los_guidance/package.xml +++ b/guidance/los_guidance/package.xml @@ -13,9 +13,10 @@ rclcpp_action geometry_msgs vortex_msgs + nav_msgs vortex_utils_ros eigen - + yaml-cpp ament_cmake diff --git a/guidance/los_guidance/scripts/test_scenarios.py b/guidance/los_guidance/scripts/test_scenarios.py new file mode 100644 index 000000000..95edef2d6 --- /dev/null +++ b/guidance/los_guidance/scripts/test_scenarios.py @@ -0,0 +1,150 @@ +import math + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from std_msgs.msg import Header +from vortex_msgs.action import LOSGuidance + + +class WaypointTest(Node): + def __init__(self): + super().__init__("waypoint_test_client") + + self.declare_parameter("test_scenario", "square") + self.declare_parameter("drone", "orca") + + self.test_scenario = self.get_parameter("test_scenario").value + self.drone = self.get_parameter("drone").value + + self._action_client = ActionClient( + self, + LOSGuidance, + f"/{self.drone}/los_guidance", + ) + + self.depth = 2.5 + self.square_size = 10.0 + + self.circle_radius = 8.0 + self.circle_points = 16 + self.circle_center_x = 0.0 + self.circle_center_y = 0.0 + + self.waypoints = self.generate_waypoints(self.test_scenario) + self.current_index = 0 + + self.get_logger().info(f"Starting test scenario: {self.test_scenario}") + self.get_logger().info(f"Using drone namespace: {self.drone}") + self.get_logger().info(f"Number of waypoints: {len(self.waypoints)}") + + self.send_next_goal() + + def generate_waypoints(self, test_scenario): + if test_scenario == "square": + s = self.square_size + d = self.depth + return [ + (s, 0.0, d), + (s, s, d), + (0.0, s, d), + (0.0, 0.0, d), + ] + + elif test_scenario == "circle": + d = self.depth + waypoints = [] + + for i in range(self.circle_points): + theta = 2.0 * math.pi * i / self.circle_points + x = self.circle_center_x + self.circle_radius * math.cos(theta) + y = self.circle_center_y + self.circle_radius * math.sin(theta) + waypoints.append((x, y, d)) + + waypoints.append(waypoints[0]) + return waypoints + + elif test_scenario == "test_pitch": + # 0 = water surface, do not go above + # 3 = seabed/ground, do not touch + # Keep all depths safely between these + return [ + (3.0, 0.0, 1.0), # slight up + (6.0, 0.0, 2.0), # slight down + (9.0, 0.0, 1.0), # up again + (12.0, 0.0, 2.0), # down again + ] + + elif test_scenario == "opposite_point": + # Go to one point, then the exact opposite point + return [ + (6.0, 4.0, self.depth), + (-6.0, -4.0, self.depth), + ] + + else: + self.get_logger().warn( + f"Unknown test_scenario '{test_scenario}', defaulting to square" + ) + return self.generate_waypoints("square") + + def send_next_goal(self): + if self.current_index >= len(self.waypoints): + self.get_logger().info(f"{self.test_scenario} test completed!") + rclpy.shutdown() + return + + self._action_client.wait_for_server() + + goal_msg = LOSGuidance.Goal() + + header = Header() + header.frame_id = "world_ned" + goal_msg.goal.header = header + + x, y, z = self.waypoints[self.current_index] + goal_msg.goal.point.x = float(x) + goal_msg.goal.point.y = float(y) + goal_msg.goal.point.z = float(z) + + self.get_logger().info( + f"Sending waypoint {self.current_index + 1}/{len(self.waypoints)}: " + f"x={x:.2f}, y={y:.2f}, z={z:.2f}" + ) + + self._send_goal_future = self._action_client.send_goal_async( + goal_msg, + feedback_callback=self.feedback_callback, + ) + self._send_goal_future.add_done_callback(self.goal_response_callback) + + def goal_response_callback(self, future): + goal_handle = future.result() + + if not goal_handle.accepted: + self.get_logger().error("Goal rejected") + rclpy.shutdown() + return + + self.get_logger().info("Goal accepted") + + self._get_result_future = goal_handle.get_result_async() + self._get_result_future.add_done_callback(self.result_callback) + + def result_callback(self, future): + self.get_logger().info("Waypoint reached") + self.current_index += 1 + self.send_next_goal() + + def feedback_callback(self, feedback_msg): + pass + + +def main(args=None): + rclpy.init(args=args) + node = WaypointTest() + rclpy.spin(node) + + +if __name__ == "__main__": + main() diff --git a/guidance/los_guidance/src/lib/adaptive_los.cpp b/guidance/los_guidance/src/lib/adaptive_los.cpp new file mode 100644 index 000000000..db6177b1c --- /dev/null +++ b/guidance/los_guidance/src/lib/adaptive_los.cpp @@ -0,0 +1,78 @@ +#include + +#include "los_guidance/lib/types.hpp" + +namespace vortex::guidance::los { + +// Constructor +AdaptiveLOSGuidance::AdaptiveLOSGuidance(const AdaptiveLosParams& params) + : params_{params} {} + +// Angle Update +void AdaptiveLOSGuidance::update_angles(const types::Inputs& inputs) { + const double dx = inputs.next_point.x - inputs.prev_point.x; + const double dy = inputs.next_point.y - inputs.prev_point.y; + const double dz = inputs.next_point.z - inputs.prev_point.z; + + pi_h_ = std::atan2(dy, dx); + pi_v_ = std::atan2(-dz, std::sqrt(dx * dx + dy * dy)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +// Cross-Track Error Calculation +const types::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const types::Point difference = inputs.current_position - inputs.prev_point; + const Eigen::Vector3d difference_vector = difference.as_vector(); + + const Eigen::Vector3d cross_track_error = + rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; + + return types::CrossTrackError::from_vector(cross_track_error); +} + +// Adaptive Estimate Update +void AdaptiveLOSGuidance::update_adaptive_estimates( + const types::CrossTrackError& cross_track_error) { + const double denom_h = + std::sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + + cross_track_error.y_e * cross_track_error.y_e); + const double denom_v = + std::sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + + cross_track_error.z_e * cross_track_error.z_e); + + const double beta_dot = params_.gamma_h * + (params_.lookahead_distance_h / denom_h) * + cross_track_error.y_e; + const double alpha_dot = params_.gamma_v * + (params_.lookahead_distance_v / denom_v) * + cross_track_error.z_e; + + beta_c_hat_ += beta_dot * params_.time_step; + alpha_c_hat_ += alpha_dot * params_.time_step; +} + +// Output Calculation +types::Outputs AdaptiveLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + update_adaptive_estimates(cross_track_error); + + const double psi_d = + pi_h_ - beta_c_hat_ - + std::atan(cross_track_error.y_e / params_.lookahead_distance_h); + + const double theta_d = + pi_v_ + alpha_c_hat_ + + std::atan(cross_track_error.z_e / params_.lookahead_distance_v); + + return types::Outputs{psi_d, theta_d}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/integral_los.cpp b/guidance/los_guidance/src/lib/integral_los.cpp new file mode 100644 index 000000000..c8f0b4d59 --- /dev/null +++ b/guidance/los_guidance/src/lib/integral_los.cpp @@ -0,0 +1,56 @@ +#include + +namespace vortex::guidance::los { + +// Constructor +IntegralLOSGuidance::IntegralLOSGuidance(const IntegralLosParams& params) + : m_params{params} {} + +// Angle Update +void IntegralLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +// Cross-Track Error Calculation +types::CrossTrackError IntegralLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +// Output Calculation +types::Outputs IntegralLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + int_h += cross_track_error.y_e * m_params.time_step; + int_v += cross_track_error.z_e * m_params.time_step; + + const double u_h = + m_params.k_p_h * cross_track_error.y_e + m_params.k_i_h * int_h; + const double u_v = + m_params.k_p_v * cross_track_error.z_e + m_params.k_i_v * int_v; + + const double psi_d = pi_h_ - std::atan(u_h); + const double theta_d = pi_v_ + std::atan(u_v); + + return types::Outputs{psi_d, theta_d}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/proportional_los.cpp b/guidance/los_guidance/src/lib/proportional_los.cpp new file mode 100644 index 000000000..f76df385c --- /dev/null +++ b/guidance/los_guidance/src/lib/proportional_los.cpp @@ -0,0 +1,52 @@ +#include + +namespace vortex::guidance::los { + +// Constructor +ProportionalLOSGuidance::ProportionalLOSGuidance( + const ProportionalLosParams& params) + : m_params{params} {} + +// Angle Update +void ProportionalLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +// Cross-Track Error Calculation +types::CrossTrackError ProportionalLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +// Output Calculation +types::Outputs ProportionalLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + const double k_p_h = 1.0 / m_params.lookahead_distance_h; + const double k_p_v = 1.0 / m_params.lookahead_distance_v; + + const double psi_d = pi_h_ - std::atan(k_p_h * cross_track_error.y_e); + const double theta_d = pi_v_ + std::atan(k_p_v * cross_track_error.z_e); + + return types::Outputs{psi_d, theta_d}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/lib/vector_field_los.cpp b/guidance/los_guidance/src/lib/vector_field_los.cpp new file mode 100644 index 000000000..edf352a6e --- /dev/null +++ b/guidance/los_guidance/src/lib/vector_field_los.cpp @@ -0,0 +1,55 @@ +#include + +namespace vortex::guidance::los { + +// Constructor +VectorFieldLOSGuidance::VectorFieldLOSGuidance( + const VectorFieldLosParams& params) + : m_params{params} {} + +// Angle Update +void VectorFieldLOSGuidance::update_angles(const types::Inputs& inputs) { + const types::Point difference = inputs.next_point - inputs.prev_point; + + pi_h_ = std::atan2(difference.y, difference.x); + pi_v_ = std::atan2(-difference.z, std::sqrt(difference.x * difference.x + + difference.y * difference.y)); + + rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); + rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); +} + +// Cross-Track Error Calculation +types::CrossTrackError VectorFieldLOSGuidance::calculate_crosstrack_error( + const types::Inputs& inputs) const { + const Eigen::Vector3d diff_vec = + (inputs.current_position - inputs.prev_point).as_vector(); + + const Eigen::Vector3d path_frame_error = + rotation_y_.toRotationMatrix().transpose() * + rotation_z_.toRotationMatrix().transpose() * diff_vec; + + return types::CrossTrackError::from_vector(path_frame_error); +} + +// Output Calculation +types::Outputs VectorFieldLOSGuidance::calculate_outputs( + const types::Inputs& inputs) { + update_angles(inputs); + + const types::CrossTrackError cross_track_error = + calculate_crosstrack_error(inputs); + + const double approach_h = m_params.max_approach_angle_h * (2.0 / M_PI) * + std::atan(m_params.k_p_h * cross_track_error.y_e); + + const double approach_v = m_params.max_approach_angle_v * (2.0 / M_PI) * + std::atan(m_params.k_p_v * cross_track_error.z_e); + + const double psi_d = pi_h_ - approach_h; + const double theta_d = pi_v_ - approach_v; + + return types::Outputs{psi_d, theta_d}; +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/src/los_guidance.cpp b/guidance/los_guidance/src/los_guidance.cpp deleted file mode 100644 index 3171d5442..000000000 --- a/guidance/los_guidance/src/los_guidance.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include "los_guidance/los_guidance.hpp" - -namespace vortex::guidance { - -AdaptiveLOSGuidance::AdaptiveLOSGuidance(const LOS::Params& params) - : params_(params) {} - -void AdaptiveLOSGuidance::update_angles(const LOS::Point& prev_point, - const LOS::Point& next_point) { - const LOS::Point difference = next_point - prev_point; - - pi_h_ = atan2(difference.y, difference.x); - pi_v_ = atan2(-difference.z, sqrt(difference.x * difference.x + - difference.y * difference.y)); - - rotation_y_ = Eigen::AngleAxisd(pi_v_, Eigen::Vector3d::UnitY()); - rotation_z_ = Eigen::AngleAxisd(pi_h_, Eigen::Vector3d::UnitZ()); -} - -LOS::CrossTrackError AdaptiveLOSGuidance::calculate_crosstrack_error( - const LOS::Point& prev_point, - const LOS::Point& current_position) const { - const LOS::Point difference = current_position - prev_point; - const Eigen::Vector3d difference_vector = difference.as_vector(); - - const Eigen::Vector3d cross_track_error = - rotation_y_.transpose() * rotation_z_.transpose() * difference_vector; - - return LOS::CrossTrackError::from_vector(cross_track_error); -} - -double AdaptiveLOSGuidance::calculate_psi_d(const double& y_e) const { - return pi_h_ - beta_c_hat_ - atan(y_e / params_.lookahead_distance_h); -} - -double AdaptiveLOSGuidance::calculate_theta_d(const double& z_e) const { - return pi_v_ + alpha_c_hat_ + atan(z_e / params_.lookahead_distance_v); -} - -void AdaptiveLOSGuidance::update_adaptive_estimates( - const LOS::CrossTrackError& crosstrack_error) { - double beta_c_hat_dot = - params_.gamma_h * - (params_.lookahead_distance_h / - sqrt(params_.lookahead_distance_h * params_.lookahead_distance_h + - crosstrack_error.y_e * crosstrack_error.y_e)) * - crosstrack_error.y_e; - double alpha_c_hat_dot = - params_.gamma_v * - (params_.lookahead_distance_v / - sqrt(params_.lookahead_distance_v * params_.lookahead_distance_v + - crosstrack_error.z_e * crosstrack_error.z_e)) * - crosstrack_error.z_e; - - beta_c_hat_ += beta_c_hat_dot * params_.time_step; - alpha_c_hat_ += alpha_c_hat_dot * params_.time_step; -} - -} // namespace vortex::guidance diff --git a/guidance/los_guidance/src/los_guidance_node.cpp b/guidance/los_guidance/src/los_guidance_node.cpp index 931a57ded..31ad2eb74 100644 --- a/guidance/los_guidance/src/los_guidance_node.cpp +++ b/guidance/los_guidance/src/los_guidance_node.cpp @@ -1,10 +1,12 @@ #include + #include "los_guidance/los_guidance_ros.hpp" +// Main Entry Point int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); diff --git a/guidance/los_guidance/src/los_guidance_ros.cpp b/guidance/los_guidance/src/los_guidance_ros.cpp index 0b63f50c9..976d73980 100644 --- a/guidance/los_guidance/src/los_guidance_ros.cpp +++ b/guidance/los_guidance/src/los_guidance_ros.cpp @@ -1,7 +1,12 @@ #include "los_guidance/los_guidance_ros.hpp" + +#include #include +#include #include +#include "los_guidance/lib/types.hpp" + const auto start_message = R"( _ ___ ____ ____ _ _ | | / _ \/ ___| / ___|_ _(_) __| | __ _ _ __ ___ ___ @@ -11,107 +16,199 @@ const auto start_message = R"( )"; -namespace vortex::guidance { +namespace vortex::guidance::los { -LOSGuidanceNode::LOSGuidanceNode() : Node("los_guidance_node") { - time_step_ = std::chrono::milliseconds(10); +// Constructor +LosGuidanceNode::LosGuidanceNode() : Node("los_guidance_node") { + double time_step_s = this->declare_parameter("time_step"); + time_step_ = + std::chrono::milliseconds(static_cast(time_step_s * 1000)); - set_subscribers_and_publisher(); + const std::string yaml_path = + this->declare_parameter("los_config_file"); - set_action_server(); + YAML::Node config = get_los_config(yaml_path); - set_adaptive_los_guidance(); + parse_common_config(config["common"]); + set_subscribers_and_publisher(); + set_action_server(); + set_service_server(); + set_adaptive_los_guidance(config); + set_proportional_los_guidance(config); + set_integral_los_guidance(config); + set_vector_field_guidance(config); spdlog::info(start_message); } -void LOSGuidanceNode::set_subscribers_and_publisher() { +// ROS Setup +void LosGuidanceNode::set_subscribers_and_publisher() { this->declare_parameter("topics.pose"); this->declare_parameter("topics.guidance.los"); this->declare_parameter("topics.waypoint"); + this->declare_parameter("topics.odom"); std::string pose_topic = this->get_parameter("topics.pose").as_string(); std::string guidance_topic = this->get_parameter("topics.guidance.los").as_string(); std::string waypoint_topic = this->get_parameter("topics.waypoint").as_string(); + std::string odom_topic = this->get_parameter("topics.odom").as_string(); auto qos_sensor_data = vortex::utils::qos_profiles::sensor_data_profile(1); reference_pub_ = this->create_publisher( guidance_topic, qos_sensor_data); + los_debug_pub_ = this->create_publisher( + "los_debug", qos_sensor_data); + + state_debug_pub_ = this->create_publisher( + "state_debug", qos_sensor_data); + waypoint_sub_ = this->create_subscription( waypoint_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::waypoint_callback, this, + std::bind(&LosGuidanceNode::waypoint_callback, this, std::placeholders::_1)); pose_sub_ = this->create_subscription< geometry_msgs::msg::PoseWithCovarianceStamped>( pose_topic, qos_sensor_data, - std::bind(&LOSGuidanceNode::pose_callback, this, + std::bind(&LosGuidanceNode::pose_callback, this, + std::placeholders::_1)); + + odom_sub_ = this->create_subscription( + odom_topic, qos_sensor_data, + std::bind(&LosGuidanceNode::odom_callback, this, std::placeholders::_1)); } -void LOSGuidanceNode::set_action_server() { +void LosGuidanceNode::set_action_server() { this->declare_parameter("action_servers.los"); std::string action_server_name = this->get_parameter("action_servers.los").as_string(); + cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); action_server_ = rclcpp_action::create_server( this, action_server_name, - std::bind(&LOSGuidanceNode::handle_goal, this, + std::bind(&LosGuidanceNode::handle_goal, this, std::placeholders::_1, std::placeholders::_2), - std::bind(&LOSGuidanceNode::handle_cancel, this, + std::bind(&LosGuidanceNode::handle_cancel, this, std::placeholders::_1), - std::bind(&LOSGuidanceNode::handle_accepted, this, + std::bind(&LosGuidanceNode::handle_accepted, this, std::placeholders::_1), rcl_action_server_get_default_options(), cb_group_); } -void LOSGuidanceNode::set_adaptive_los_guidance() { - this->declare_parameter("los.lookahead_distance_h"); - this->declare_parameter("los.lookahead_distance_v"); - this->declare_parameter("los.gamma_h"); - this->declare_parameter("los.gamma_v"); - this->declare_parameter("los.time_step"); - this->declare_parameter("los.u_desired"); - this->declare_parameter("los.goal_reached_tol"); +void LosGuidanceNode::set_service_server() { + this->declare_parameter("services.los_mode", "set_los_mode"); + std::string service_name = + this->get_parameter("services.los_mode").as_string(); + + los_mode_service_ = this->create_service( + service_name, std::bind(&LosGuidanceNode::set_los_mode, this, + std::placeholders::_1, std::placeholders::_2)); +} + +// Guidance Configuration +void LosGuidanceNode::set_adaptive_los_guidance(YAML::Node config) { + auto adaptive_los_config = config["adaptive_los"]; + auto params = AdaptiveLosParams{}; + + params.lookahead_distance_h = + adaptive_los_config["lookahead_distance_h"].as(); + params.lookahead_distance_v = + adaptive_los_config["lookahead_distance_v"].as(); + params.gamma_h = adaptive_los_config["gamma_h"].as(); + params.gamma_v = adaptive_los_config["gamma_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + adaptive_los_ = std::make_unique(params); +} + +void LosGuidanceNode::set_proportional_los_guidance(YAML::Node config) { + auto proportional_los_config = config["prop_los"]; + auto params = ProportionalLosParams{}; - LOS::Params params; params.lookahead_distance_h = - this->get_parameter("los.lookahead_distance_h").as_double(); + proportional_los_config["lookahead_distance_h"].as(); params.lookahead_distance_v = - this->get_parameter("los.lookahead_distance_v").as_double(); - params.gamma_h = this->get_parameter("los.gamma_h").as_double(); - params.gamma_v = this->get_parameter("los.gamma_v").as_double(); - params.time_step = this->get_parameter("los.time_step").as_double(); + proportional_los_config["lookahead_distance_v"].as(); - adaptive_los_guidance_ = std::make_unique(params); + proportional_los_ = std::make_unique(params); } -void LOSGuidanceNode::waypoint_callback( - const geometry_msgs::msg::PointStamped::SharedPtr los_waypoint) { - fill_los_waypoints(*los_waypoint); - adaptive_los_guidance_->update_angles(last_point_, next_point_); - spdlog::info("Received waypoint"); +void LosGuidanceNode::set_integral_los_guidance(YAML::Node config) { + auto integral_los_config = config["integer_los"]; + auto params = IntegralLosParams{}; + + params.k_p_h = integral_los_config["k_p_h"].as(); + params.k_p_v = integral_los_config["k_p_v"].as(); + params.k_i_h = integral_los_config["k_i_h"].as(); + params.k_i_v = integral_los_config["k_i_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + integral_los_ = std::make_unique(params); +} + +void LosGuidanceNode::set_vector_field_guidance(YAML::Node config) { + auto vector_field_config = config["vector_field_los"]; + auto params = VectorFieldLosParams{}; + + params.max_approach_angle_h = + vector_field_config["max_approach_angle_h"].as(); + params.max_approach_angle_v = + vector_field_config["max_approach_angle_v"].as(); + params.k_p_h = vector_field_config["k_p_h"].as(); + params.k_p_v = vector_field_config["k_p_v"].as(); + params.time_step = static_cast(time_step_.count()) / 1000.0; + + vector_field_los_ = std::make_unique(params); +} + +// Topic Callbacks +void LosGuidanceNode::waypoint_callback( + const geometry_msgs::msg::PointStamped::SharedPtr wp_msg) { + std::lock_guard lock(mutex_); + + const auto new_wp = types::Point::point_from_ros(wp_msg->point); + + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } + + spdlog::info("Received waypoint: ({}, {}, {})", new_wp.x, new_wp.y, + new_wp.z); } -void LOSGuidanceNode::pose_callback( +void LosGuidanceNode::pose_callback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose) { - eta_.x = current_pose->pose.pose.position.x; - eta_.y = current_pose->pose.pose.position.y; - eta_.z = current_pose->pose.pose.position.z; + std::lock_guard lock(mutex_); + path_inputs_.current_position = + types::Point::point_from_ros(current_pose->pose.pose.position); +} + +void LosGuidanceNode::odom_callback( + const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(mutex_); + debug_current_odom_ = msg; } -rclcpp_action::GoalResponse LOSGuidanceNode::handle_goal( +// Action Server Callbacks +rclcpp_action::GoalResponse LosGuidanceNode::handle_goal( const rclcpp_action::GoalUUID&, std::shared_ptr goal) { (void)goal; + { std::lock_guard lock(mutex_); if (goal_handle_) { @@ -121,11 +218,12 @@ rclcpp_action::GoalResponse LOSGuidanceNode::handle_goal( } } } + spdlog::info("Accepted goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( +rclcpp_action::CancelResponse LosGuidanceNode::handle_cancel( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { @@ -134,34 +232,64 @@ rclcpp_action::CancelResponse LOSGuidanceNode::handle_cancel( return rclcpp_action::CancelResponse::ACCEPT; } -void LOSGuidanceNode::handle_accepted( +void LosGuidanceNode::handle_accepted( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { execute(goal_handle); } -void LOSGuidanceNode::fill_los_waypoints( - const geometry_msgs::msg::PointStamped& los_waypoint) { - last_point_.x = eta_.x; - last_point_.y = eta_.y; - last_point_.z = eta_.z; - - next_point_.x = los_waypoint.point.x; - next_point_.y = los_waypoint.point.y; - next_point_.z = los_waypoint.point.z; +// Service Callback +void LosGuidanceNode::set_los_mode( + const std::shared_ptr request, + std::shared_ptr response) { + method_ = static_cast(request->mode); + spdlog::info("LOS mode set to {}", static_cast(method_)); + response->success = true; } -vortex_msgs::msg::LOSGuidance LOSGuidanceNode::fill_los_reference() { +// Message Helpers +vortex_msgs::msg::LOSGuidance LosGuidanceNode::fill_los_reference( + types::Outputs outputs, + types::Inputs inputs) { vortex_msgs::msg::LOSGuidance reference_msg; - reference_msg.pitch = pitch_d_; - reference_msg.yaw = yaw_d_; - reference_msg.surge = u_desired_; + const double clamped_pitch = + std::clamp(outputs.theta_d, -max_pitch_angle_, max_pitch_angle_); + reference_msg.pitch = clamped_pitch; + reference_msg.yaw = outputs.psi_d; + + const double distance_to_goal = + (inputs.current_position - inputs.next_point).as_vector().norm(); + + const double u_slow_min = 0.2; // bytt ut dette med config + + if (distance_to_goal <= slow_down_distance_) { + const double alpha = distance_to_goal / slow_down_distance_; + reference_msg.surge = u_slow_min + alpha * (u_desired_ - u_slow_min); + } else { + reference_msg.surge = u_desired_; + } return reference_msg; } -void LOSGuidanceNode::execute( +YAML::Node LosGuidanceNode::get_los_config(std::string yaml_file_path) { + YAML::Node config = YAML::LoadFile(yaml_file_path); + return config; +} + +void LosGuidanceNode::parse_common_config(YAML::Node common_config) { + std::lock_guard lock(mutex_); + u_desired_ = common_config["u_desired"].as(); + max_pitch_angle_ = common_config["max_pitch_angle"].as(); + goal_reached_tol_ = common_config["goal_reached_tol"].as(); + slow_down_distance_ = common_config["slow_down_distance"].as(); + method_ = static_cast( + common_config["active_los_method"].as()); +} + +// Goal Execution +void LosGuidanceNode::execute( const std::shared_ptr< rclcpp_action::ServerGoalHandle> goal_handle) { @@ -170,17 +298,21 @@ void LOSGuidanceNode::execute( this->goal_handle_ = goal_handle; } - u_desired_ = this->get_parameter("los.u_desired").as_double(); - goal_reached_tol_ = this->get_parameter("los.goal_reached_tol").as_double(); - spdlog::info("Executing goal"); const geometry_msgs::msg::PointStamped los_waypoint = goal_handle->get_goal()->goal; - fill_los_waypoints(los_waypoint); + const auto new_wp = types::Point::point_from_ros(los_waypoint.point); - adaptive_los_guidance_->update_angles(last_point_, next_point_); + if (!has_active_segment_) { + path_inputs_.prev_point = path_inputs_.current_position; + path_inputs_.next_point = new_wp; + has_active_segment_ = true; + } else { + path_inputs_.prev_point = path_inputs_.next_point; + path_inputs_.next_point = new_wp; + } auto feedback = std::make_shared(); @@ -197,6 +329,7 @@ void LOSGuidanceNode::execute( return; } } + if (goal_handle->is_canceling()) { result->success = false; goal_handle->canceled(result); @@ -204,28 +337,68 @@ void LOSGuidanceNode::execute( return; } - LOS::CrossTrackError errors = - adaptive_los_guidance_->calculate_crosstrack_error(last_point_, - eta_); + types::Inputs inputs_copy; + { + std::lock_guard lock(mutex_); + inputs_copy = path_inputs_; + } - yaw_d_ = adaptive_los_guidance_->calculate_psi_d(errors.y_e); - pitch_d_ = adaptive_los_guidance_->calculate_theta_d(errors.z_e); + types::Outputs outputs; + + switch (method_) { + case types::ActiveLosMethod::ADAPTIVE: + outputs = adaptive_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::PROPORTIONAL: + outputs = proportional_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::INTEGRAL: + outputs = integral_los_->calculate_outputs(inputs_copy); + break; + case types::ActiveLosMethod::VECTOR_FIELD: + outputs = vector_field_los_->calculate_outputs(inputs_copy); + break; + default: + spdlog::error("Invalid LOS method selected"); + result->success = false; + goal_handle->abort(result); + return; + } - adaptive_los_guidance_->update_adaptive_estimates(errors); + vortex_msgs::msg::LOSGuidance reference_msg = + fill_los_reference(outputs, inputs_copy); + feedback->feedback = reference_msg; - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); + los_debug_pub_->publish(reference_msg); - feedback->feedback = reference_msg; + const auto& v = debug_current_odom_->twist.twist.linear; + double surge = std::sqrt(v.x * v.x + v.y * v.y + v.z * v.z); + vortex_msgs::msg::LOSGuidance state_debug_msg; + Eigen::Vector3d euler = vortex::utils::math::quat_to_euler( + Eigen::Quaterniond(debug_current_odom_->pose.pose.orientation.w, + debug_current_odom_->pose.pose.orientation.x, + debug_current_odom_->pose.pose.orientation.y, + debug_current_odom_->pose.pose.orientation.z)); + + state_debug_msg.pitch = euler.y(); + state_debug_msg.yaw = euler.z(); + state_debug_msg.surge = surge; + + state_debug_pub_->publish(state_debug_msg); goal_handle->publish_feedback(feedback); reference_pub_->publish(reference_msg); - - if ((eta_ - next_point_).as_vector().norm() < goal_reached_tol_) { + if ((inputs_copy.current_position - inputs_copy.next_point) + .as_vector() + .norm() < goal_reached_tol_) { + auto stop_ref = reference_msg; + stop_ref.surge = 0.0; + stop_ref.pitch = 0.0; + stop_ref.yaw = reference_msg.yaw; + + reference_pub_->publish(stop_ref); result->success = true; goal_handle->succeed(result); - u_desired_ = 0.0; - vortex_msgs::msg::LOSGuidance reference_msg = fill_los_reference(); - reference_pub_->publish(reference_msg); spdlog::info("Goal reached"); return; } @@ -234,4 +407,4 @@ void LOSGuidanceNode::execute( } } -} // namespace vortex::guidance +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/CMakeLists.txt b/guidance/los_guidance/test/CMakeLists.txt index 69c0dfa52..c0cdc1ae7 100644 --- a/guidance/los_guidance/test/CMakeLists.txt +++ b/guidance/los_guidance/test/CMakeLists.txt @@ -6,13 +6,21 @@ include(GoogleTest) set(TEST_BINARY_NAME ${PROJECT_NAME}_test) add_executable( ${TEST_BINARY_NAME} - test_los_guidance.cpp + test_main.cpp + adaptive_los_test.cpp + proportional_los_test.cpp + integral_los_test.cpp + vector_field_los_test.cpp + ) -target_link_libraries( - ${TEST_BINARY_NAME} +target_link_libraries(${TEST_BINARY_NAME} PRIVATE ${LIB_NAME} + yaml-cpp + Eigen3::Eigen + spdlog::spdlog + fmt::fmt GTest::GTest ) diff --git a/guidance/los_guidance/test/adaptive_los_test.cpp b/guidance/los_guidance/test/adaptive_los_test.cpp new file mode 100644 index 000000000..5fd71027a --- /dev/null +++ b/guidance/los_guidance/test/adaptive_los_test.cpp @@ -0,0 +1,173 @@ +#include "los_guidance/lib/adaptive_los.hpp" +#include + +namespace vortex::guidance::los { // new namespace added los + +class AdaptiveLosTest : public ::testing::Test { + protected: + AdaptiveLosTest() : los_{get_params()} {} + + AdaptiveLosParams get_params() { + AdaptiveLosParams p; + p.lookahead_distance_h = 1.0; + p.lookahead_distance_v = 1.0; + p.gamma_h = 1.0; + p.gamma_v = 1.0; + p.time_step = 0.01; + return p; + } + + AdaptiveLOSGuidance los_; + const double tol = 1e-9; +}; +/* +TEST_F(AdaptiveLosTest, T01_test_cross_track_error_on_track){ + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T02_test_cross_track_error_right_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T03_test_cross_track_error_left_off_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, -0.5, tol); + EXPECT_NEAR(e.z_e, 0.0, tol); +} + +TEST_F(AdaptiveLosTests, T04_test_cross_track_error_under_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, 0.5, tol); +} + +TEST_F(AdaptiveLosTests, T05_test_cross_track_error_over_track) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Output O = los_.calculate_outputs(inputs); + + EXPECT_NEAR(e.x_e, 0.0, tol); + EXPECT_NEAR(e.y_e, 0.0, tol); + EXPECT_NEAR(e.z_e, -0.5, tol); +} +*/ + +// Test commanded angles when drone is to the right of the track +TEST_F(AdaptiveLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(AdaptiveLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(AdaptiveLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(AdaptiveLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(AdaptiveLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = los_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/integral_los_test.cpp b/guidance/los_guidance/test/integral_los_test.cpp new file mode 100644 index 000000000..1999abac8 --- /dev/null +++ b/guidance/los_guidance/test/integral_los_test.cpp @@ -0,0 +1,108 @@ +#include "los_guidance/lib/integral_los.hpp" +#include +#include "los_guidance/lib/adaptive_los.hpp" + +namespace vortex::guidance::los { + +class IntegralLosTest : public ::testing::Test { + protected: + IntegralLosTest() : Ilos_{get_params()} {} + + IntegralLosParams get_params() { + IntegralLosParams params; + params.k_i_h = 0.1; // needs tuning + params.k_i_v = 0.1; // needs tuning + params.k_p_h = 0.667; // needs tuning + params.k_p_v = 0.582; // needs tuning + params.time_step = 0.01; + return params; + } + + IntegralLOSGuidance Ilos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(IntegralLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(IntegralLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(IntegralLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(IntegralLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(IntegralLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Ilos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/proportional_los_test.cpp b/guidance/los_guidance/test/proportional_los_test.cpp new file mode 100644 index 000000000..b35a3ba6f --- /dev/null +++ b/guidance/los_guidance/test/proportional_los_test.cpp @@ -0,0 +1,104 @@ +#include "los_guidance/lib/proportional_los.hpp" +#include + +namespace vortex::guidance::los { + +class ProportionalLosTest : public ::testing::Test { + protected: + ProportionalLosTest() : Plos_{get_params()} {} + + ProportionalLosParams get_params() { + ProportionalLosParams params; + params.lookahead_distance_h = 10.0; + params.lookahead_distance_v = 10.0; + return params; + } + + ProportionalLOSGuidance Plos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(ProportionalLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(ProportionalLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(ProportionalLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(ProportionalLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track + +TEST_F(ProportionalLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Plos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los diff --git a/guidance/los_guidance/test/test_los_guidance.cpp b/guidance/los_guidance/test/test_los_guidance.cpp deleted file mode 100644 index 78b3d4fe1..000000000 --- a/guidance/los_guidance/test/test_los_guidance.cpp +++ /dev/null @@ -1,182 +0,0 @@ -#include - -#include "los_guidance/los_guidance.hpp" - -namespace vortex::guidance { - -class LOSTests : public ::testing::Test { - protected: - LOSTests() : los_guidance_{get_los_params()} {} - - LOS::Params get_los_params() { - LOS::Params params; - params.lookahead_distance_h = 1.0; - params.lookahead_distance_v = 1.0; - params.gamma_h = 1.0; - params.gamma_v = 1.0; - params.time_step = 0.01; - return params; - } - - AdaptiveLOSGuidance los_guidance_; - const double tol = 1e-9; -}; - -TEST_F(LOSTests, T01_test_cross_track_error_on_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T02_test_cross_track_error_right_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T03_test_cross_track_error_left_off_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, -0.5, tol); - EXPECT_NEAR(e.z_e, 0.0, tol); -} - -TEST_F(LOSTests, T04_test_cross_track_error_under_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, 0.5, tol); -} - -TEST_F(LOSTests, T05_test_cross_track_error_over_track) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - - EXPECT_NEAR(e.x_e, 0.0, tol); - EXPECT_NEAR(e.y_e, 0.0, tol); - EXPECT_NEAR(e.z_e, -0.5, tol); -} - -// Test commanded angles when drone is to the right of the track -TEST_F(LOSTests, T06_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is to the left of the track -TEST_F(LOSTests, T07_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, -0.5, 0.0}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between 0 and pi/2 - EXPECT_GT(psi_d, 0.0); - EXPECT_LT(psi_d, 1.57); - // Pitch cmd should be zero - EXPECT_NEAR(theta_d, 0.0, tol); -} - -// Test commanded angles when drone is under the track -TEST_F(LOSTests, T08_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, 0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between 0 and pi/2 - EXPECT_GT(theta_d, 0.0); - EXPECT_LT(theta_d, 1.57); -} - -// Test commanded angles when drone is over the track -TEST_F(LOSTests, T09_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.0, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be 0 - EXPECT_NEAR(psi_d, 0.0, tol); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -// Test commanded angles when drone is over and to the right of the track -TEST_F(LOSTests, T10_test_commanded_angles) { - LOS::Point prev{0.0, 0.0, 0.0}; - LOS::Point next{1.0, 0.0, 0.0}; - los_guidance_.update_angles(prev, next); - LOS::Point current{0.0, 0.5, -0.5}; - LOS::CrossTrackError e = - los_guidance_.calculate_crosstrack_error(prev, current); - double psi_d{los_guidance_.calculate_psi_d(e.y_e)}; - double theta_d{los_guidance_.calculate_theta_d(e.z_e)}; - // Heading cmd should be between -pi/2 and 0 - EXPECT_LT(psi_d, 0.0); - EXPECT_GT(psi_d, -1.57); - // Pitch cmd should be between -pi/2 and 0 - EXPECT_LT(theta_d, 0.0); - EXPECT_GT(theta_d, -1.57); -} - -} // namespace vortex::guidance - -int main(int argc, char** argv) { - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/guidance/los_guidance/test/test_main.cpp b/guidance/los_guidance/test/test_main.cpp new file mode 100644 index 000000000..af89a87e6 --- /dev/null +++ b/guidance/los_guidance/test/test_main.cpp @@ -0,0 +1,8 @@ +// test/test_main.cpp +#include + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/guidance/los_guidance/test/vector_field_los_test.cpp b/guidance/los_guidance/test/vector_field_los_test.cpp new file mode 100644 index 000000000..956e9b5f0 --- /dev/null +++ b/guidance/los_guidance/test/vector_field_los_test.cpp @@ -0,0 +1,111 @@ +#include "los_guidance/lib/vector_field_los.hpp" +#include +#include "los_guidance/lib/proportional_los.hpp" + +namespace vortex::guidance::los { + +class VectorFieldLosTest : public ::testing::Test { + protected: + VectorFieldLosTest() : Vflos_{get_params()} {} + + VectorFieldLosParams get_params() { + VectorFieldLosParams params; + params.max_approach_angle_h = 30.0 * M_PI / 180.0; // 30 degrees in rad + params.max_approach_angle_v = 20.0 * M_PI / 180.0; // 20 degrees in rad + params.k_p_h = 0.1; // needs tuning + params.k_p_v = 0.1; // needs tuning + params.time_step = 0.01; + return params; + } + + VectorFieldLOSGuidance Vflos_; + const double tol = 1e-9; +}; + +// Test commanded angles when drone is to the right of the track +TEST_F(VectorFieldLosTest, T06_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is to the left of the track +TEST_F(VectorFieldLosTest, T07_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, -0.5, 0.0}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between 0 and pi/2 + EXPECT_GT(O.psi_d, 0.0); + EXPECT_LT(O.psi_d, 1.57); + + // Pitch cmd should be zero + EXPECT_NEAR(O.theta_d, 0.0, tol); +} + +// Test commanded angles when drone is under the track +TEST_F(VectorFieldLosTest, T08_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, 0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between 0 and pi/2 + EXPECT_GT(O.theta_d, 0.0); + EXPECT_LT(O.theta_d, 1.57); +} + +// Test commanded angles when drone is over the track +TEST_F(VectorFieldLosTest, T09_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.0, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be 0 + EXPECT_NEAR(O.psi_d, 0.0, tol); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +// Test commanded angles when drone is over and to the right of the track +TEST_F(VectorFieldLosTest, T10_test_commanded_angles) { + types::Inputs inputs; + inputs.prev_point = types::Point{0.0, 0.0, 0.0}; + inputs.next_point = types::Point{1.0, 0.0, 0.0}; + inputs.current_position = types::Point{0.0, 0.5, -0.5}; + + const types::Outputs O = Vflos_.calculate_outputs(inputs); + + // Heading cmd should be between -pi/2 and 0 + EXPECT_LT(O.psi_d, 0.0); + EXPECT_GT(O.psi_d, -1.57); + + // Pitch cmd should be between -pi/2 and 0 + EXPECT_LT(O.theta_d, 0.0); + EXPECT_GT(O.theta_d, -1.57); +} + +} // namespace vortex::guidance::los