From 95c6b9d40bac029888bb049c5f3dfd262c8f2770 Mon Sep 17 00:00:00 2001
From: Prajwal <gprajwalpoojari@gmail.com>
Date: Wed, 21 Jun 2023 15:08:51 -0400
Subject: [PATCH] create derived classes for cost calculation

---
 .../trajectory_cost_calculator.cpp            | 24 ++++++++
 .../trajectory_cost_calculator.hpp            | 55 +++++++++++++++++++
 2 files changed, 79 insertions(+)
 create mode 100644 src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.cpp
 create mode 100644 src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.hpp

diff --git a/src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.cpp b/src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.cpp
new file mode 100644
index 0000000..7f1bdd1
--- /dev/null
+++ b/src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.cpp
@@ -0,0 +1,24 @@
+# include <trajectory_cost_calculator.hpp>
+
+
+namespace trajectory_generation::cost_calculation {
+    TrajectoryCostCalculator::TrajectoryCostCalculator(const std::vector<core_datastructures::DynamicPosture>& trajectory) : trajectory(trajectory) {
+
+    }
+
+    double TrajectoryCostCalculator::get_cost() const {
+        return get_static_cost() + get_dynamic_cost();
+    }
+    
+    double TrajectoryCostCalculator::get_static_cost() const {
+        return 0;
+    }
+
+    double TrajectoryCostCalculator::get_dynamic_cost() const {
+        return 0;
+    }
+
+    void TrajectoryCostCalculator::set(const std::vector<core_datastructures::DynamicPosture>& trajectory) {
+        this->trajectory = trajectory;
+    }
+}
\ No newline at end of file
diff --git a/src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.hpp b/src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.hpp
new file mode 100644
index 0000000..cd99176
--- /dev/null
+++ b/src/modules/trajectory_generation/domain/cost_calculation/trajectory_cost_calculator.hpp
@@ -0,0 +1,55 @@
+#ifndef TRAJECTORY_GENERATION__COST_CALCULATION__TRAJECTORY_COST_CALCULATOR
+#define TRAJECTORY_GENERATION__COST_CALCULATION__TRAJECTORY_COST_CALCULATOR
+
+
+#include <i_cost_calculator.hpp>
+#include <core_datastructures/dynamic_posture.hpp>
+#include <vector>
+
+namespace trajectory_generation::cost_calculation {
+
+class TrajectoryCostCalculator : public ICostCalculator {
+    public:
+    /**
+     * @brief Construct a new Trajectory Cost Calculator object
+     * 
+     * @param trajectory 
+     */
+    TrajectoryCostCalculator(const std::vector<core_datastructures::DynamicPosture>& trajectory);
+
+    /**
+     * @brief Get the sum of static and dynamic cost
+     * 
+     * @return double 
+     */
+    double get_cost() const;
+
+    /**
+     * @brief Get the static cost
+     * 
+     * @return double 
+     */
+    double get_static_cost() const;
+
+    /**
+     * @brief Get the dynamic cost
+     * 
+     * @return double 
+     */
+    double get_dynamic_cost() const;
+
+    /**
+     * @brief Set the trajectory for computing cost
+     * 
+     * @param[in] trajectory The trajectory to be set 
+     * @return void 
+     */
+    void set(const std::vector<core_datastructures::DynamicPosture>& trajectory);
+
+    private:
+    std::vector<core_datastructures::DynamicPosture> trajectory;
+};
+
+}
+
+#endif /*TRAJECTORY_GENERATION__COST_CALCULATION__TRAJECTORY_COST_CALCULATOR*/
\ No newline at end of file