Create an animation class (#51)

Author: astomodynamics

examples/cddp_car.cpp

@@ -6,6 +6,7 @@
 #include <filesystem>
 #include <random>
 #include <cmath>
+#include "animation.hpp"
 #include "cddp.hpp"
 
 namespace plt = matplotlibcpp;
@@ -114,17 +115,14 @@ void plotCarBox(const Eigen::VectorXd& state, Eigen::VectorXd& control,
     plt::plot(base_x, base_y, keywords);
 
     // Plot steering direction
-    // Calculate front axle center point
     double front_x = x + length/2 * cos(theta);
     double front_y = y + length/2 * sin(theta);
 
-    // Calculate steering indicator endpoint
-    double steering_length = width/2;  // Length of steering indicator line
-    double steering_angle = theta + steering;  // Global steering angle
+    double steering_length = width/2;
+    double steering_angle = theta + steering;
     double steering_end_x = front_x + steering_length * cos(steering_angle);
     double steering_end_y = front_y + steering_length * sin(steering_angle);
 
-    // Plot steering indicator
     std::vector<double> steer_x = {front_x, steering_end_x};
     std::vector<double> steer_y = {front_y, steering_end_y};
     keywords["color"] = "green";
@@ -133,7 +131,15 @@ void plotCarBox(const Eigen::VectorXd& state, Eigen::VectorXd& control,
 }
 
 int main() {
-    // Problem parameters
+    // Set up animation configuration
+    Animation::AnimationConfig config;
+    config.width = 800;
+    config.height = 800;
+    config.frame_skip = 5;  // Save every 5th frame
+    config.frame_delay = 10;  // 10/100 = 0.1 seconds between frames
+    Animation animation(config);
+
+   // Problem parameters
     int state_dim = 4;     // [x y theta v]
     int control_dim = 2;   // [wheel_angle acceleration]
     int horizon = 500;     // Same as MATLAB example
@@ -224,61 +230,44 @@ int main() {
     auto U_sol = solution.control_sequence;
     auto t_sol = solution.time_sequence;
 
-    // Create directory for plots
-    const std::string plotDirectory = "../results/tests";
-    if (!fs::exists(plotDirectory)) {
-        fs::create_directories(plotDirectory);
-    }
-
-    // Prepare visualization data
-    std::vector<double> x_hist, y_hist, theta_hist, v_hist;
+    // Prepare trajectory data
+    std::vector<double> x_hist, y_hist;
     for(const auto& x : X_sol) {
         x_hist.push_back(x(0));
         y_hist.push_back(x(1));
-        theta_hist.push_back(x(2));
-        v_hist.push_back(x(3));
     }
 
-    // Visualization matching MATLAB style
-    plt::figure_size(800, 800);
-    
-    // Set plot limits and properties matching MATLAB
-    plt::xlim(-4, 4);
-    plt::ylim(-4, 4);
-    plt::grid(true);
-    
-    // Plot goal configuration
-    double car_length = 2.1;  // From MATLAB body = [0.9 2.1 0.3]
-    double car_width = 0.9;   
+    // Car dimensions
+    double car_length = 2.1;
+    double car_width = 0.9;
+
+    // Create animation
     Eigen::VectorXd empty_control = Eigen::VectorXd::Zero(control_dim);
-    plotCarBox(goal_state, empty_control, car_length, car_width, "r");
-    
-    // Animation loop
     for(size_t i = 0; i < X_sol.size(); i++) {
-        if(i % 5 == 0) {
-            plt::clf();
-            
-            // Plot full trajectory
-            plt::plot(x_hist, y_hist, "b-");
-            
-            // Plot current car position
-            plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k");
-            
-            // Plot settings
-            plt::grid(true);
-            plt::axis("equal");
-            plt::xlim(-4, 4);
-            plt::ylim(-4, 4);
-            
-            std::string filename = plotDirectory + "/car_frame_" + 
-                                 std::to_string(i) + ".png";
-            plt::save(filename);
-            plt::pause(0.01);
-        }
+        animation.newFrame();
+        
+        // Plot full trajectory
+        plt::plot(x_hist, y_hist, "b-");
+        
+        // Plot goal configuration
+        plotCarBox(goal_state, empty_control, car_length, car_width, "r");
+        
+        // Plot current car position
+        plotCarBox(X_sol[i], U_sol[i], car_length, car_width, "k");
+        
+        // Plot settings
+        plt::grid(true);
+        // plt::axis("equal");
+        plt::xlim(-4, 4);
+        plt::ylim(-4, 4);
+        
+        animation.saveFrame(i);
     }
 
+    // Create the final GIF
+    animation.createGif("car_parking.gif");
+
     return 0;
 }
 
-// Create animation from frames using ImageMagick:
-// convert -delay 1 ../results/tests/car_frame_*.png ../results/tests/car_motion.gif
+// convert -delay 3 ../results/frames/frame_*.png ../results/animations/car_parking.gif

include/cddp-cpp/animation.hpp

@@ -0,0 +1,122 @@
+/*
+ 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 ANIMATION_HPP
+#define ANIMATION_HPP
+
+#include <string>
+#include <vector>
+#include <filesystem>
+#include <memory>
+#include <stdexcept>
+#include <cstdlib>
+#include "matplotlibcpp.hpp"
+
+namespace plt = matplotlibcpp;
+namespace fs = std::filesystem;
+
+class Animation {
+public:
+    struct AnimationConfig {
+        int width;               // Figure width in pixels
+        int height;             // Figure height in pixels
+        int frame_skip;         // Save every nth frame
+        int frame_delay;        // Delay between frames in 1/100ths of a second
+        std::string temp_dir;   // Directory for temporary frame files
+        std::string output_dir; // Directory for output GIFs
+        bool cleanup_frames;    // Whether to delete frame files after creating GIF
+
+        AnimationConfig() :
+            width(800),
+            height(600),
+            frame_skip(1),
+            frame_delay(20),
+            temp_dir("../results/frames"),
+            output_dir("../results/animations"),
+            cleanup_frames(true)
+        {}
+    };
+
+    // Constructor
+    explicit Animation(const AnimationConfig& config = AnimationConfig()) 
+        : config_(config) {
+        // Create directories if they don't exist
+        fs::create_directories(config_.temp_dir);
+        fs::create_directories(config_.output_dir);
+        
+        // Initialize figure
+        plt::figure_size(config_.width, config_.height);
+    }
+
+    // Start a new frame
+    void newFrame() {
+        plt::clf();  // Clear current figure
+    }
+
+    // Save current frame
+    void saveFrame(const int frame_number) {
+        if (frame_number % config_.frame_skip == 0) {
+            std::string filename = config_.temp_dir + "/frame_" + 
+                                 std::to_string(frame_number/config_.frame_skip) + ".png";
+            plt::save(filename);
+        }
+    }
+
+    // Create GIF from saved frames
+    bool createGif(const std::string& output_filename) {
+        std::string command = "convert -delay " + std::to_string(config_.frame_delay) + " " +
+                             config_.temp_dir + "/frame_*.png " +
+                             config_.output_dir + "/" + output_filename;
+        
+        int result = std::system(command.c_str());
+        
+        if (result != 0) {
+            std::cerr << "Failed to create GIF. Is ImageMagick installed?" << std::endl;
+            return false;
+        }
+
+        // Clean up temporary frame files if requested
+        if (config_.cleanup_frames) {
+            cleanupFrames();
+        }
+
+        return true;
+    }
+
+    // Clean up temporary frame files
+    void cleanupFrames() {
+        for (const auto& entry : fs::directory_iterator(config_.temp_dir)) {
+            fs::remove(entry.path());
+        }
+    }
+
+    // Setter for configuration
+    void setConfig(const AnimationConfig& config) {
+        config_ = config;
+        // Update figure size if changed
+        plt::figure_size(config_.width, config_.height);
+    }
+
+    // Getter for configuration
+    const AnimationConfig& getConfig() const { 
+        return config_; 
+    }
+
+private:
+    AnimationConfig config_;
+};
+
+#endif // ANIMATION_HPP

include/cddp-cpp/cddp.hpp

@@ -44,6 +44,7 @@
 #include "dynamics_model/lti_system.hpp"
 
 #include "matplotlibcpp.hpp"
+#include "animation.hpp"
 
 
 #endif // CDDP_HPP

results/animations/car_parking.gif

@@ -84,6 +84,8 @@ add_executable(test_logcddp_core cddp_core/test_logcddp_core.cpp)
 target_link_libraries(test_logcddp_core gtest gmock gtest_main cddp)
 gtest_discover_tests(test_logcddp_core)
 
+add_executable(test_animation test_animation.cpp)
+target_link_libraries(test_animation gtest gmock gtest_main cddp)
 
 # Test for Eigen installation
 add_executable(test_eigen test_eigen.cpp)

tests/CMakeLists.txt

@@ -0,0 +1,80 @@
+/*
+ Example showing how to use the Animation class with the cart-pole system
+*/
+
+#include "animation.hpp"
+#include <cmath>
+
+int main() {
+    // Create animation with custom configuration
+    Animation::AnimationConfig config;
+    config.width = 800;
+    config.height = 600;
+    config.frame_skip = 2;  // Save every other frame
+    config.frame_delay = 10;  // Faster animation
+    Animation animation(config);
+
+    // Set up cart-pole parameters
+    double cart_width = 0.3;
+    double cart_height = 0.2;
+    double pole_width = 0.05;
+    double pole_length = 1.0;
+
+    // Simulate cart-pole motion
+    int num_frames = 200;
+    double dt = 0.05;
+    double theta = M_PI/3;  // Initial angle (60 degrees)
+    double omega = 0.0;     // Initial angular velocity
+    double x = 0.0;         // Initial cart position
+
+    for (int i = 0; i < num_frames; ++i) {
+        animation.newFrame();
+
+        // Update simulation
+        double g = 9.81;
+        omega += -3*g/(2*pole_length) * sin(theta) * dt;
+        theta += omega * dt;
+        x = sin(i * 0.05) * 0.5;  // Simple sinusoidal cart motion
+
+        // Draw cart
+        std::vector<double> cart_x = {
+            x - cart_width/2, x + cart_width/2,
+            x + cart_width/2, x - cart_width/2,
+            x - cart_width/2
+        };
+        std::vector<double> cart_y = {
+            -cart_height/2, -cart_height/2,
+            cart_height/2, cart_height/2,
+            -cart_height/2
+        };
+        plt::plot(cart_x, cart_y, "k-");
+
+        // Draw pole
+        double pole_end_x = x + pole_length * sin(theta);
+        double pole_end_y = pole_length * cos(theta);
+        std::vector<double> pole_x = {x, pole_end_x};
+        std::vector<double> pole_y = {0, pole_end_y};
+        plt::plot(pole_x, pole_y, "b-");
+
+        // Draw pole bob
+        std::vector<double> circle_x, circle_y;
+        for (int j = 0; j <= 20; ++j) {
+            double t = 2 * M_PI * j / 20;
+            circle_x.push_back(pole_end_x + pole_width * cos(t));
+            circle_y.push_back(pole_end_y + pole_width * sin(t));
+        }
+        plt::plot(circle_x, circle_y, "b-");
+
+        // Set fixed axis limits
+        plt::xlim(x - 2, x + 2);
+        plt::ylim(-2, 2);
+        
+        // Save the frame
+        animation.saveFrame(i);
+    }
+
+    // Create the final GIF
+    animation.createGif("cartpole_animation.gif");
+
+    return 0;
+}