-
Notifications
You must be signed in to change notification settings - Fork 9
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
f743b0d
commit 75a3bdf
Showing
5 changed files
with
258 additions
and
12 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,145 @@ | ||
| #include "dynamics_model/car.hpp" | ||
| #include <cmath> | ||
|
|
||
| namespace cddp { | ||
|
|
||
| Car::Car(double timestep, double wheelbase, std::string integration_type) | ||
| : DynamicalSystem(STATE_DIM, CONTROL_DIM, timestep, integration_type), | ||
| wheelbase_(wheelbase) { | ||
| } | ||
|
|
||
| Eigen::VectorXd Car::getDiscreteDynamics( | ||
| const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { | ||
|
|
||
| // Extract states | ||
| const double x = state(STATE_X); // x position | ||
| const double y = state(STATE_Y); // y position | ||
| const double theta = state(STATE_THETA); // car angle | ||
| const double v = state(STATE_V); // velocity | ||
|
|
||
| // Extract controls | ||
| const double delta = control(CONTROL_DELTA); // steering angle | ||
| const double a = control(CONTROL_A); // acceleration | ||
|
|
||
| // Constants | ||
| const double d = wheelbase_; // distance between back and front axles | ||
| const double h = timestep_; // timestep | ||
|
|
||
| // Compute unit vector in car direction | ||
| const double cos_theta = std::cos(theta); | ||
| const double sin_theta = std::sin(theta); | ||
| Eigen::Vector2d z(cos_theta, sin_theta); | ||
|
|
||
| // Front wheel rolling distance | ||
| const double f = h * v; | ||
|
|
||
| // Back wheel rolling distance | ||
| // b = d + f*cos(w) - sqrt(d^2 - (f*sin(w))^2) | ||
| const double b = d + f * std::cos(delta) - | ||
| std::sqrt(d*d - std::pow(f * std::sin(delta), 2)); | ||
|
|
||
| // Change in car angle | ||
| // dtheta = asin(sin(w)*f/d) | ||
| const double dtheta = std::asin(std::sin(delta) * f / d); | ||
|
|
||
| // Compute state change | ||
| Eigen::VectorXd dy = Eigen::VectorXd::Zero(STATE_DIM); | ||
| dy(STATE_X) = b * cos_theta; | ||
| dy(STATE_Y) = b * sin_theta; | ||
| dy(STATE_THETA) = dtheta; | ||
| dy(STATE_V) = h * a; | ||
|
|
||
| // Return next state | ||
| return state + dy; | ||
| } | ||
|
|
||
| Eigen::MatrixXd Car::getStateJacobian( | ||
| const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { | ||
|
|
||
| const int n = STATE_DIM; | ||
| Eigen::MatrixXd J(n, n); | ||
| const double h = 2e-17; // Small perturbation value | ||
|
|
||
| // Get nominal output | ||
| Eigen::VectorXd f0 = getDiscreteDynamics(state, control); | ||
|
|
||
| // Compute Jacobian column by column using finite differences | ||
| Eigen::VectorXd perturbed_state = state; | ||
| for (int i = 0; i < n; ++i) { | ||
| // Perturb state element | ||
| perturbed_state(i) = state(i) + h; | ||
|
|
||
| // Get perturbed output | ||
| Eigen::VectorXd f1 = getDiscreteDynamics(perturbed_state, control); | ||
|
|
||
| // Compute derivative using central difference | ||
| J.col(i) = (f1 - f0) / h; | ||
|
|
||
| // Reset perturbation | ||
| perturbed_state(i) = state(i); | ||
| } | ||
|
|
||
| // Expected output of the state Jacobian is continuous dynamics Jacobian so modify this discrete one | ||
| // Ref: in cddp_core.cpp | ||
| /* | ||
| // Convert continuous dynamics to discrete time | ||
| A = timestep_ * Fx; | ||
| A.diagonal().array() += 1.0; // More efficient way to add identity | ||
| B = timestep_ * Fu; | ||
| */ | ||
| J.diagonal().array() -= 1.0; | ||
| J /= timestep_; | ||
| return J; | ||
| } | ||
|
|
||
| Eigen::MatrixXd Car::getControlJacobian( | ||
| const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { | ||
|
|
||
| const int n = STATE_DIM; | ||
| const int m = CONTROL_DIM; | ||
| Eigen::MatrixXd J(n, m); | ||
| const double h = 2e-17; // Small perturbation value | ||
|
|
||
| // Get nominal output | ||
| Eigen::VectorXd f0 = getDiscreteDynamics(state, control); | ||
|
|
||
| // Compute Jacobian column by column using finite differences | ||
| Eigen::VectorXd perturbed_control = control; | ||
| for (int i = 0; i < m; ++i) { | ||
| // Perturb control element | ||
| perturbed_control(i) = control(i) + h; | ||
|
|
||
| // Get perturbed output | ||
| Eigen::VectorXd f1 = getDiscreteDynamics(state, perturbed_control); | ||
|
|
||
| // Compute derivative using forward difference | ||
| J.col(i) = (f1 - f0) / h; | ||
|
|
||
| // Reset perturbation | ||
| perturbed_control(i) = control(i); | ||
| } | ||
|
|
||
| // Expected output of the control Jacobian is continuous dynamics Jacobian so modify this discrete one | ||
| // Ref: in cddp_core.cpp | ||
| /* | ||
| // Convert continuous dynamics to discrete time | ||
| A = timestep_ * Fx; | ||
| A.diagonal().array() += 1.0; // More efficient way to add identity | ||
| B = timestep_ * Fu; | ||
| */ | ||
| J /= timestep_; | ||
|
|
||
| return J; | ||
| } | ||
|
|
||
| Eigen::MatrixXd Car::getStateHessian( | ||
| const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { | ||
| return Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM); | ||
| } | ||
|
|
||
| Eigen::MatrixXd Car::getControlHessian( | ||
| const Eigen::VectorXd& state, const Eigen::VectorXd& control) const { | ||
| return Eigen::MatrixXd::Zero(CONTROL_DIM, CONTROL_DIM); | ||
| } | ||
|
|
||
| } // namespace cddp |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,96 @@ | ||
| /* | ||
| 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. | ||
| */ | ||
| // Description: Test the car dynamics model. | ||
| #include <iostream> | ||
| #include <vector> | ||
| #include <filesystem> | ||
|
|
||
| #include "gmock/gmock.h" | ||
| #include "gtest/gtest.h" | ||
|
|
||
| #include "dynamics_model/car.hpp" | ||
| #include "matplotlibcpp.hpp" | ||
|
|
||
| namespace plt = matplotlibcpp; | ||
| namespace fs = std::filesystem; | ||
| using namespace cddp; | ||
|
|
||
| TEST(CarTest, DiscreteDynamics) { | ||
| // Create a car instance | ||
| double timestep = 0.03; // From original MATLAB code | ||
| double wheelbase = 2.0; // From original MATLAB code | ||
| std::string integration_type = "euler"; | ||
| cddp::Car car(timestep, wheelbase, integration_type); | ||
|
|
||
| // Store states for plotting | ||
| std::vector<double> time_data, x_data, y_data, theta_data, v_data; | ||
|
|
||
| // Initial state and control (from MATLAB demo) | ||
| Eigen::VectorXd state(4); | ||
| state << 1.0, 1.0, 3*M_PI/2, 0.0; // Initial state from MATLAB demo | ||
| Eigen::VectorXd control(2); | ||
| control << 0.1, 0.1; // Small steering angle and acceleration | ||
|
|
||
| // Simulate for a few steps | ||
| int num_steps = 100; | ||
| for (int i = 0; i < num_steps; ++i) { | ||
| // Store data for plotting | ||
| time_data.push_back(i * timestep); | ||
| x_data.push_back(state[0]); | ||
| y_data.push_back(state[1]); | ||
| theta_data.push_back(state[2]); | ||
| v_data.push_back(state[3]); | ||
|
|
||
| // Compute the next state | ||
| state = car.getDiscreteDynamics(state, control); | ||
| } | ||
|
|
||
| // Basic assertions | ||
| ASSERT_EQ(car.getStateDim(), 4); | ||
| ASSERT_EQ(car.getControlDim(), 2); | ||
| ASSERT_DOUBLE_EQ(car.getTimestep(), 0.03); | ||
| ASSERT_EQ(car.getIntegrationType(), "euler"); | ||
|
|
||
| // // Create directory for saving plot (if it doesn't exist) | ||
| // const std::string plotDirectory = "../plots/test"; | ||
| // if (!fs::exists(plotDirectory)) { | ||
| // fs::create_directory(plotDirectory); | ||
| // } | ||
|
|
||
| // // Plot the results | ||
| // // Plot trajectory in X-Y plane | ||
| // plt::figure(1); | ||
| // plt::plot(x_data, y_data, {{"label", "Trajectory"}}); | ||
| // plt::xlabel("X Position"); | ||
| // plt::ylabel("Y Position"); | ||
| // plt::legend(); | ||
| // plt::save(plotDirectory + "/car_trajectory.png"); | ||
|
|
||
| // // Plot states over time | ||
| // plt::figure(2); | ||
| // plt::plot(time_data, theta_data, {{"label", "Heading"}}); | ||
| // plt::plot(time_data, v_data, {{"label", "Velocity"}}); | ||
| // plt::xlabel("Time"); | ||
| // plt::ylabel("State"); | ||
| // plt::legend(); | ||
| // plt::save(plotDirectory + "/car_states.png"); | ||
| // plt::show(); | ||
| } | ||
|
|
||
| int main(int argc, char **argv) { | ||
| testing::InitGoogleTest(&argc, argv); | ||
| return RUN_ALL_TESTS(); | ||
| } |