Add spacecraft landing model (#46)

CMakeLists.txt

@@ -190,6 +190,7 @@ set(dynamics_model_srcs
   src/dynamics_model/quadrotor.cpp
   src/dynamics_model/manipulator.cpp
   src/dynamics_model/spacecraft_linear.cpp
+  src/dynamics_model/spacecraft_landing2d.cpp
 )
 
 add_library(${PROJECT_NAME} 

include/cddp-cpp/cddp.hpp

@@ -39,6 +39,7 @@
 #include "dynamics_model/quadrotor.hpp"
 #include "dynamics_model/manipulator.hpp"
 #include "dynamics_model/spacecraft_linear.hpp"
+#include "dynamics_model/spacecraft_landing2d.hpp"
 
 
 #include "matplotlibcpp.hpp"

include/cddp-cpp/dynamics_model/spacecraft_landing2d.hpp

@@ -0,0 +1,113 @@
+/*
+ Copyright 2024 Tomo Sasaki
+
+ 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
+
+      https://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 CDDP_SPACECRAFT_LANDING2D_HPP
+#define CDDP_SPACECRAFT_LANDING2D_HPP
+
+#include "cddp_core/dynamical_system.hpp"
+
+namespace cddp {
+
+class SpacecraftLanding2D : public DynamicalSystem {
+public:
+    /**
+     * Constructor for the 2D Spacecraft Landing model
+     * @param timestep Time step for discretization
+     * @param integration_type Integration method ("euler" by default)
+     * @param mass Mass of the spacecraft [kg]
+     * @param length Length of the spacecraft [m]
+     * @param width Width of the spacecraft [m]
+     * @param min_thrust Minimum thrust [N]
+     * @param max_thrust Maximum thrust [N]
+     * @param max_gimble Maximum gimble angle [rad]
+     * @ref https://thomas-godden.medium.com/how-spacex-lands-starship-sort-of-ee96cdde650b 
+     */
+    SpacecraftLanding2D(double timestep = 0.1,
+                        std::string integration_type = "rk4",
+                        double mass = 100000.0,
+                        double length = 50.0,
+                        double width = 10.0,
+                        double min_thrust = 880000.0,
+                        double max_thrust = 2210000.0,
+                        double max_gimble = 0.349066); // 20 degrees in radians
+
+    /**
+     * Computes continuous-time dynamics
+     * State vector: [x, x_dot, y, y_dot, theta, theta_dot]
+     * Control vector: [thrust_percent, thrust_angle]
+     */
+    Eigen::VectorXd getContinuousDynamics(const Eigen::VectorXd& state, 
+                                         const Eigen::VectorXd& control) const override;
+
+    Eigen::VectorXd getDiscreteDynamics(const Eigen::VectorXd& state, 
+                                       const Eigen::VectorXd& control) const override {
+        return DynamicalSystem::getDiscreteDynamics(state, control);
+    }
+
+    Eigen::MatrixXd getStateJacobian(const Eigen::VectorXd& state, 
+                                    const Eigen::VectorXd& control) const override;
+
+    Eigen::MatrixXd getControlJacobian(const Eigen::VectorXd& state, 
+                                      const Eigen::VectorXd& control) const override;
+
+    Eigen::MatrixXd getStateHessian(const Eigen::VectorXd& state, 
+                                   const Eigen::VectorXd& control) const override;
+
+    Eigen::MatrixXd getControlHessian(const Eigen::VectorXd& state, 
+                                     const Eigen::VectorXd& control) const override;
+
+    // Accessors
+    int getStateDim() const { return STATE_DIM; }
+    int getControlDim() const { return CONTROL_DIM; }
+
+    double getMass() const { return mass_; }
+    double getLength() const { return length_; }
+    double getWidth() const { return width_; }
+    double getInertia() const { return inertia_; }
+    double getMinThrust() const { return min_thrust_; }
+    double getMaxThrust() const { return max_thrust_; }
+    double getMaxGimble() const { return max_gimble_; }
+    double getGravity() const { return gravity_; }
+
+private:
+    // System parameters
+    double mass_;        // Mass of spacecraft [kg]
+    double length_;      // Length of spacecraft [m]
+    double width_;       // Width of spacecraft [m]
+    double inertia_;     // Moment of inertia [kg*m^2]
+    double min_thrust_;  // Minimum thrust [N]
+    double max_thrust_;  // Maximum thrust [N]
+    double max_gimble_;  // Maximum gimble angle [rad]
+    double gravity_{9.81};  // Gravitational acceleration [m/s^2]
+
+    // State indices
+    static constexpr int STATE_X = 0;           // x position
+    static constexpr int STATE_X_DOT = 1;       // x velocity
+    static constexpr int STATE_Y = 2;           // y position
+    static constexpr int STATE_Y_DOT = 3;       // y velocity
+    static constexpr int STATE_THETA = 4;       // angle
+    static constexpr int STATE_THETA_DOT = 5;   // angular velocity
+    static constexpr int STATE_DIM = 6;         // state dimension
+
+    // Control indices
+    static constexpr int CONTROL_THRUST = 0;     // thrust percentage
+    static constexpr int CONTROL_ANGLE = 1;      // thrust angle
+    static constexpr int CONTROL_DIM = 2;        // control dimension
+};
+
+} // namespace cddp
+
+#endif // CDDP_SPACECRAFT_LANDING2D_HPP

src/dynamics_model/spacecraft_landing2d.cpp

@@ -0,0 +1,102 @@
+/*
+ Copyright 2024 Tomo Sasaki
+
+ 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
+
+      https://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.
+*/
+
+#include "dynamics_model/spacecraft_landing2d.hpp"
+#include <cmath>
+
+namespace cddp {
+
+SpacecraftLanding2D::SpacecraftLanding2D(double timestep, 
+                                        std::string integration_type,
+                                        double mass,
+                                        double length,
+                                        double width,
+                                        double min_thrust,
+                                        double max_thrust,
+                                        double max_gimble)
+    : DynamicalSystem(STATE_DIM, CONTROL_DIM, timestep, integration_type),
+      mass_(mass),
+      length_(length),
+      width_(width),
+      min_thrust_(min_thrust),
+      max_thrust_(max_thrust),
+      max_gimble_(max_gimble) {
+    // Calculate moment of inertia for uniform density rod
+    inertia_ = (1.0/12.0) * mass_ * length_ * length_;
+}
+
+Eigen::VectorXd SpacecraftLanding2D::getContinuousDynamics(
+    const Eigen::VectorXd& state, const Eigen::VectorXd& control) const {
+
+    Eigen::VectorXd state_dot = Eigen::VectorXd::Zero(STATE_DIM);
+
+    // Extract state variables
+    const double theta = state(STATE_THETA);
+    const double theta_dot = state(STATE_THETA_DOT);
+
+    // Extract control variables
+    const double thrust_percent = control(CONTROL_THRUST);
+    const double thrust_angle = control(CONTROL_ANGLE);
+
+    // Calculate total thrust angle (thrust angle + spacecraft angle)
+    const double total_angle = thrust_angle + theta;
+
+    // Calculate forces
+    const double thrust = max_thrust_ * thrust_percent;
+    const double F_x = thrust * std::sin(total_angle);
+    const double F_y = thrust * std::cos(total_angle);
+
+    // Calculate torque
+    const double T = -length_ / 2.0 * thrust * std::sin(thrust_angle);
+
+    // Position derivatives (velocities)
+    state_dot(STATE_X) = state(STATE_X_DOT);
+    state_dot(STATE_Y) = state(STATE_Y_DOT);
+    state_dot(STATE_THETA) = theta_dot;
+
+    // Velocity derivatives (accelerations)
+    state_dot(STATE_X_DOT) = F_x / mass_;
+    state_dot(STATE_Y_DOT) = F_y / mass_ - gravity_;
+    state_dot(STATE_THETA_DOT) = T / inertia_;
+
+    return state_dot;
+}
+
+Eigen::MatrixXd SpacecraftLanding2D::getStateJacobian(
+    const Eigen::VectorXd& state, const Eigen::VectorXd& control) const {
+
+    Eigen::MatrixXd A = getFiniteDifferenceStateJacobian(state, control);
+    return A;
+}
+
+Eigen::MatrixXd SpacecraftLanding2D::getControlJacobian(
+    const Eigen::VectorXd& state, const Eigen::VectorXd& control) const {
+
+    Eigen::MatrixXd B = getFiniteDifferenceControlJacobian(state, control);
+    return B;
+}
+
+Eigen::MatrixXd SpacecraftLanding2D::getStateHessian(
+    const Eigen::VectorXd& state, const Eigen::VectorXd& control) const {
+    return Eigen::MatrixXd::Zero(STATE_DIM * STATE_DIM, STATE_DIM);
+}
+
+Eigen::MatrixXd SpacecraftLanding2D::getControlHessian(
+    const Eigen::VectorXd& state, const Eigen::VectorXd& control) const {
+    return Eigen::MatrixXd::Zero(STATE_DIM * CONTROL_DIM, CONTROL_DIM);
+}
+
+} // namespace cddp

tests/CMakeLists.txt

@@ -31,6 +31,10 @@ add_executable(test_spacecraft_linear dynamics_model/test_spacecraft_linear.cpp)
 target_link_libraries(test_spacecraft_linear gtest gmock gtest_main cddp)
 gtest_discover_tests(test_spacecraft_linear)
 
+add_executable(test_spacecraft_landing2d dynamics_model/test_spacecraft_landing2d.cpp)
+target_link_libraries(test_spacecraft_landing2d gtest gmock gtest_main cddp Eigen3::Eigen)
+gtest_discover_tests(test_spacecraft_landing2d)
+
 add_executable(test_objective cddp_core/test_objective.cpp)
 target_link_libraries(test_objective gtest gmock gtest_main cddp)
 gtest_discover_tests(test_objective)