init uuv-cpp-simulator

CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.5)
+project(simulation)
+
+add_definitions(-std=c++17)
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+find_package(ruckig REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(LAPACK REQUIRED)
+find_package(Matplot++ REQUIRED)
+add_subdirectory(libs/lin_alg_tools)
+find_package(Python3 COMPONENTS NumPy)
+
+add_executable(${PROJECT_NAME} src/main.cpp src/optimal_trajectory.cpp src/plant.cpp src/lqr.cpp)
+
+target_link_libraries(${PROJECT_NAME} ${EIGEN3_LIBRARIES} ${LAPACK_LIBRARIES} ruckig ipopt Matplot++::matplot   Python3::Python Python3::Module)
+target_include_directories(${PROJECT_NAME}
+  PUBLIC
+  ${CMAKE_CURRENT_SOURCE_DIR}/include/}
+  ${EIGEN3_INCLUDE_DIRS}
+  libs/lin_alg_tools/include)
+

README.md

@@ -0,0 +1,9 @@
+--------
+### uuv_cpp_simualtor
+------
+
+WORK IN PROGRESS 
+
+
+![p](./imgs/result.png)
+

include/lqr.hpp

@@ -0,0 +1,65 @@
+// Copyright 2022 VorteX-co
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef TRAJECTORY_TRACKING_LQR__LQR_HPP_
+#define TRAJECTORY_TRACKING_LQR__LQR_HPP_
+#include "lin_alg_tools/care.h"
+#include "lin_alg_tools/schur.h"
+#include "plant.hpp"
+
+class LQR
+{
+public:
+  void set_params(
+    const double & m, const double & volume, const Vector6d & Ib,
+    const Vector3d & r_cob, const Vector3d & r_cog,
+    const Vector6d & Ma, const Vector6d & Dlinear,
+    const Vector6d & Dquad, const Vector12d & Q, const Vector6d & R,
+    const Vector6d & tau_max, const Vector12d & error_max);
+
+  // LQR control action  given the current state and desired state
+  Vector6d action(Vector12d x, Vector12d xd, Vector6d feedforward_acc);
+
+  // Method of testing the LQR control action on nonlinear vehicle model
+  // @params (inital state and control vector)
+  Vector12d simulation_rk4(const Vector12d & x0, const Vector6d & u);
+  Vector12d simulation_euler(const Vector12d & x0, const Vector6d & u);
+  Vector12d simulation_rk4_linearized(const Vector12d & x0, const Vector6d & u);
+  Matrix6d ItoB_transformation(Vector3d & euler);
+
+
+
+private:
+  void to_SNAME(Vector12d & x);
+  void saturate_control(Vector6d & tau);
+  void saturate_error(Vector12d & delta);
+  // Class for kinematics and dynamics computation
+  // as well as linearizing these computed functions
+  Plant vehicle_;
+  void display_log(const Vector12d & x, const Vector12d & xd, const Vector6d & u);
+
+
+
+  // Continuous algebraic Riccati equation (Care) solver [12 states, 6 controls]
+  CareSolver<12, 6> care_solver;
+  // The control forces weighting matrix
+  Matrix6d R_;
+  // The state weighting matrix
+  Matrix12d Q_;
+  // Saturation values for control forces and torques
+  Vector6d tau_max_;
+  // Saturation values for tracking error
+  Vector12d error_max_;
+  Vector12d error_integral_;
+};
+#endif  // TRAJECTORY_TRACKING_LQR__LQR_HPP_

include/optimal_trajectory.hpp

@@ -0,0 +1,50 @@
+// Copyright 2022 VorteX-co
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MOTION_PLANNING__LOCAL_PLANNING__OPTIMAL_TRAJECTORY_HPP_
+#define MOTION_PLANNING__LOCAL_PLANNING__OPTIMAL_TRAJECTORY_HPP_
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <ruckig/ruckig.hpp>
+#include <tuple>
+#include <vector>
+
+typedef Eigen::Matrix<double, 6, 1> Vector6d;
+typedef Eigen::Matrix<double, 3, 1> Vector3d;
+typedef Eigen::Matrix<double, 2, 1> Vector2d;
+typedef Eigen::Matrix<double, 12, 1> Vector12d;
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorXd;
+
+class Trajectory
+{
+public:
+  Trajectory();
+
+  void set_params(
+    const Vector3d & translation_constraints,
+    const Vector3d & rotation_constraints);
+
+  void generate_trajectory(const Vector12d & state, const Vector12d & reference);
+
+  void evaluate_generated_trajectory(
+    const double & t, Vector6d & pos,
+    Vector6d & vel, Vector6d & acc);
+
+private:
+  ruckig::Trajectory<6> desired_trajectory_;
+  Vector3d translation_constraints_;
+  Vector3d rotation_constraints_;
+};
+#endif  // MOTION_PLANNING__LOCAL_PLANNING__OPTIMAL_TRAJECTORY_HPP_

include/plant.hpp

@@ -0,0 +1,106 @@
+// Copyright 2022 VorteX-co
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef TRAJECTORY_TRACKING_LQR__PLANT_HPP_
+#define TRAJECTORY_TRACKING_LQR__PLANT_HPP_
+
+#include <cppad/example/cppad_eigen.hpp>
+#include <Eigen/Dense>
+
+
+using Vector12d = Eigen::Matrix<double, 12, 1>;
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+using Vector3d = Eigen::Matrix<double, 3, 1>;
+using Matrix12d = Eigen::Matrix<double, 12, 12>;
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Matrix3d = Eigen::Matrix<double, 3, 3>;
+
+using Vector12dAD = Eigen::Matrix<CppAD::AD<double>, 12, 1>;
+using Matrix12dAD = Eigen::Matrix<CppAD::AD<double>, 12, 12>;
+using Vector6dAD = Eigen::Matrix<CppAD::AD<double>, 6, 1>;
+using Matrix6dAD = Eigen::Matrix<CppAD::AD<double>, 6, 6>;
+using Vector3dAD = Eigen::Matrix<CppAD::AD<double>, 3, 1>;
+using Matrix3dAD = Eigen::Matrix<CppAD::AD<double>, 3, 3>;
+
+using VectorXd = Eigen::Matrix<double, Eigen::Dynamic, 1>;
+using VectorXdAD = Eigen::Matrix<CppAD::AD<double>, Eigen::Dynamic, 1>;
+
+class Plant
+{
+public:
+  // Model parameters initialization
+  void initialize(
+    const double & m, const double & volume, const Vector6d & Ib,
+    const Vector3d & r_cob, const Vector3d & r_cog,
+    const Vector6d & Ma, const Vector6d & Dlinear,
+    const Vector6d & Dquad);
+
+  // Compute AUV motion model, x_next = integration of ẋ = f(x, u)
+  Vector12d nonlinear_update(const Vector12d & x, const Vector6d & u);
+
+  // Linearize f(x, u) and obtain Jacobians A and B,  ẋ = A x + B u
+  void linearize(
+    const Vector12d & x, Matrix12d & A,
+    Eigen::Matrix<double, 12, 6> & B);
+
+private:
+  // AUV 6DOF Kinematics, η̇ =  J * nu
+  // euler is the attitude rpy and nu is the body velocities
+  Vector6dAD nonlinear_kinematics(
+    const Vector3dAD & euler,
+    const Vector6dAD & nu);
+  //  Compute Damping forces and moments D(nu) nu
+  Vector6dAD nonlinear_damping_forces(const Vector6dAD & nu);
+  // Compute Coriolis–Centripetal forces and moments C(nu) nu
+  Vector6dAD nonlinear_coriolis_forces(const Vector6dAD & nu);
+  // Compute restoring forces and moments  g(η)
+  Vector6dAD nonlinear_restoring_forces(const Vector3dAD & euler);
+
+  // Linearization of Kinematics
+  void kinematics_jacobian(
+    const Vector6d & eta, const Vector6d & nu, Matrix6d & J,
+    Matrix6d & J_star);
+  // Linearization of Damping forces
+  Matrix6d damping_forces_jacobian(const Vector6d & nu);
+  // Linearization of Coriolis–Centripetal forces
+  Matrix6d coriolis_forces_jacobian(const Vector6d & nu);
+  //  Linearization of restoring forces
+  Matrix6d restoring_forces_jacobian(const Vector6d & eta);
+
+  // Compute Rotation matrix from Body to inertial frame
+  Matrix3dAD RBtoI(const Vector3dAD & euler);
+  // Compute Transformation matrix from Body to inertial frame
+  Matrix3dAD TBtoI(const Vector3dAD & euler);
+  // Compute Transformation matrix from Body to inertial frame
+  Matrix3dAD TItoB(const Vector3dAD & euler);
+  // skew-symmetric matrix using CppAD::AD<double>
+  Matrix3dAD skew_ad(const Vector3dAD & v);
+  // using double
+  Matrix3d skew(const Vector3d & v);
+
+  CppAD::AD<double> mass_{0.0};
+  CppAD::AD<double> volume_{0.0};
+  // Origin to CB [xb, yb, zb ]
+  Vector3dAD r_cob_;
+  // Origin to CG [xg, yg, zg ]
+  Vector3dAD r_cog_;
+  // Total mass matrix
+  Matrix6dAD M_ad_;
+  // Total mass matrix
+  Matrix6d M_;
+  // Linear Damping  matrix
+  Vector6dAD Dlinear_;
+  // NonLinear Damping matrix
+  Vector6dAD Dquad_;
+};
+#endif  // TRAJECTORY_TRACKING_LQR__PLANT_HPP_