Skip to content

Commit c2d7379

Browse files
astomodynamicsastomodynamics
authored
Merge pull request #8 from astomodynamics:ros_integration_test
Add TrackingCost object
2 parents 845a1bf + ef162f2 commit c2d7379

File tree

3 files changed

+89
-24
lines changed

3 files changed

+89
-24
lines changed

‎include/cddp/cddp_core/Objective.hpp

Lines changed: 69 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -9,16 +9,16 @@ namespace cddp {
99
class Objective {
1010
public:
1111

12-
virtual double calculateRunningCost(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0;
13-
virtual std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostGradient(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0;
14-
virtual std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control) const = 0;
12+
virtual double calculateRunningCost(const Eigen::VectorXd& state, const Eigen::VectorXd& control, int index) const = 0;
13+
virtual std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostGradient(const Eigen::VectorXd& state, const Eigen::VectorXd& control, int index) const = 0;
14+
virtual std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostHessian(const Eigen::VectorXd& state, const Eigen::VectorXd& control, int index) const = 0;
1515
virtual double calculateFinalCost(const Eigen::VectorXd& state) const = 0;
1616
virtual Eigen::VectorXd calculateFinalCostGradient(const Eigen::VectorXd& state) const = 0;
1717
virtual Eigen::MatrixXd calculateFinalCostHessian(const Eigen::VectorXd& state) const = 0;
18-
virtual double calculateCost(const Eigen::MatrixXd& state, const Eigen::MatrixXd& control) const = 0;
18+
virtual double calculateCost(const std::vector<Eigen::VectorXd>& X, const std::vector<Eigen::VectorXd>& U) const = 0;
1919
};
2020

21-
// Example: Cost Function
21+
// Example: Quadratic Cost Function
2222
class QuadraticCost : public Objective{
2323
private:
2424
Eigen::MatrixXd Q_;
@@ -30,29 +30,29 @@ class QuadraticCost : public Objective{
3030
public:
3131
QuadraticCost(const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const Eigen::MatrixXd &Qf, Eigen::VectorXd &goal_state, double timestep) : Q_(Q), R_(R), Qf_(Qf), goal_state_(goal_state), timestep_(timestep) {}
3232

33-
double calculateCost(const Eigen::MatrixXd& X, const Eigen::MatrixXd& U) const override {
33+
double calculateCost(const std::vector<Eigen::VectorXd>& X, const std::vector<Eigen::VectorXd>& U) const override {
3434
double cost = 0.0;
35-
for (int i = 0; i < U.cols(); i++) {
36-
cost += calculateRunningCost(X.col(i), U.col(i));
35+
for (int i = 0; i < U.size(); i++) {
36+
cost += calculateRunningCost(X[i], U[i], i);
3737
}
38-
cost += calculateFinalCost(X.col(X.cols() - 1));
38+
cost += calculateFinalCost(X[X.size() - 1]);
3939
return cost;
4040
}
41-
42-
double calculateRunningCost(const Eigen::VectorXd& x, const Eigen::VectorXd& u) const override {
41+
42+
double calculateRunningCost(const Eigen::VectorXd& x, const Eigen::VectorXd& u, int k) const override {
4343
double cost = 0.0; // Initialize cost to 0
4444
cost += 0.5 * ((x - goal_state_).transpose() * Q_ * (x - goal_state_)).value() * timestep_;
4545
cost += 0.5 * (u.transpose() * R_ * u).value() * timestep_;
4646
return cost;
4747
}
4848

49-
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostGradient(const Eigen::VectorXd& x, const Eigen::VectorXd& u) const override {
49+
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostGradient(const Eigen::VectorXd& x, const Eigen::VectorXd& u, int k) const override {
5050
Eigen::VectorXd l_x = Q_ * (x - goal_state_) * timestep_;
5151
Eigen::VectorXd l_u = R_ * u * timestep_;
5252
return std::make_tuple(l_x, l_u);
5353
}
5454

55-
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostHessian(const Eigen::VectorXd& x, const Eigen::VectorXd& u) const override {
55+
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostHessian(const Eigen::VectorXd& x, const Eigen::VectorXd& u, int k) const override {
5656
Eigen::MatrixXd l_xx = Q_ * timestep_;
5757
Eigen::MatrixXd l_ux = Eigen::MatrixXd::Zero(R_.rows(), Q_.rows());
5858
Eigen::MatrixXd l_uu = R_ * timestep_;
@@ -71,6 +71,62 @@ class QuadraticCost : public Objective{
7171
return Qf_;
7272
}
7373
};
74+
75+
// Example: Quadratic Tracking Cost Function
76+
class QuadraticTrackingCost : public Objective {
77+
private:
78+
Eigen::MatrixXd Q_;
79+
Eigen::MatrixXd R_;
80+
Eigen::MatrixXd Qf_;
81+
std::vector<Eigen::VectorXd> X_ref_;
82+
Eigen::VectorXd goal_state_;
83+
double timestep_;
84+
85+
public:
86+
QuadraticTrackingCost(const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, const Eigen::MatrixXd &Qf, std::vector<Eigen::VectorXd> &X_ref, Eigen::VectorXd &goal_state, double timestep) : Q_(Q), R_(R), Qf_(Qf), X_ref_(X_ref), goal_state_(goal_state), timestep_(timestep) {}
87+
88+
double calculateCost(const std::vector<Eigen::VectorXd>& X, const std::vector<Eigen::VectorXd>& U) const override {
89+
double cost = 0.0;
90+
for (int i = 0; i < U.size(); i++) {
91+
cost += calculateRunningCost(X[i], U[i], i);
92+
}
93+
cost += calculateFinalCost(X[X.size() - 1]);
94+
return cost;
95+
}
96+
97+
double calculateRunningCost(const Eigen::VectorXd& x, const Eigen::VectorXd& u, int k) const override {
98+
double cost = 0.0; // Initialize cost to 0
99+
cost += 0.5 * ((x - X_ref_[k]).transpose() * Q_ * (x - X_ref_[k])).value() * timestep_;
100+
cost += 0.5 * (u.transpose() * R_ * u).value() * timestep_;
101+
return cost;
102+
}
103+
104+
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostGradient(const Eigen::VectorXd& x, const Eigen::VectorXd& u, int k) const override {
105+
Eigen::VectorXd l_x = Q_ * (x - X_ref_[k]) * timestep_;
106+
Eigen::VectorXd l_u = R_ * u * timestep_;
107+
return std::make_tuple(l_x, l_u);
108+
}
109+
110+
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> calculateRunningCostHessian(const Eigen::VectorXd& x, const Eigen::VectorXd& u, int k) const override {
111+
Eigen::MatrixXd l_xx = Q_ * timestep_;
112+
Eigen::MatrixXd l_ux = Eigen::MatrixXd::Zero(R_.rows(), Q_.rows());
113+
Eigen::MatrixXd l_uu = R_ * timestep_;
114+
return std::make_tuple(l_xx, l_ux, l_uu);
115+
}
116+
117+
double calculateFinalCost(const Eigen::VectorXd& x) const override {
118+
return 0.5 * ((x - goal_state_).transpose() * Qf_ * (x - goal_state_)).value();
119+
}
120+
121+
Eigen::VectorXd calculateFinalCostGradient(const Eigen::VectorXd& x) const override {
122+
return Qf_ * (x - goal_state_);
123+
}
124+
125+
Eigen::MatrixXd calculateFinalCostHessian(const Eigen::VectorXd& x) const override {
126+
return Qf_;
127+
}
128+
129+
};
74130
} // namespace cddp
75131

76132
#endif // CDDP_OBJECTIVE_HPP

‎src/cddp_core/CDDPProblem.cpp

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -114,7 +114,7 @@ void CDDPProblem::setInitialTrajectory(const std::vector<Eigen::VectorXd>& X, co
114114
void CDDPProblem::initializeCost() {
115115
J_ = 0.0;
116116
for (int i = 0; i < horizon_; ++i) {
117-
J_ += objective_->calculateRunningCost(X_.at(i), U_.at(i));
117+
J_ += objective_->calculateRunningCost(X_.at(i), U_.at(i), i);
118118
}
119119
J_ += objective_->calculateFinalCost(X_.back());
120120

@@ -251,6 +251,7 @@ bool CDDPProblem::solveForwardPass() {
251251

252252
// Trust Region Loop
253253
while (is_feasible == false && iter < options_.active_set_max_iterations) {
254+
std::cout << "Forward pass Iteration: " << iter << std::endl;
254255
double J_new = 0.0;
255256
double dJ = 0.0;
256257
double expected_dV = 0.0;
@@ -324,9 +325,10 @@ bool CDDPProblem::solveForwardPass() {
324325

325326
U_new.at(i) += delta_u; // Update control
326327

327-
J_new += objective_->calculateRunningCost(x, U_new.at(i)); // Running cost
328+
J_new += objective_->calculateRunningCost(x, U_new.at(i), i); // Running cost
328329
x = dynamics_->getDynamics(x, U_new.at(i)); // Simulate forward
329330
X_new.at(i + 1) = x; // Update trajectory
331+
330332
}
331333
J_new += objective_->calculateFinalCost(X_new.back()); // Final cost
332334

@@ -384,11 +386,11 @@ bool CDDPProblem::solveBackwardPass() {
384386
Eigen::MatrixXd B = dt_ * f_u;
385387

386388
// Calculate cost gradients and Hessians
387-
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> gradients = objective_->calculateRunningCostGradient(x, u);
389+
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> gradients = objective_->calculateRunningCostGradient(x, u, i);
388390
Eigen::VectorXd l_x = std::get<0>(gradients);
389391
Eigen::VectorXd l_u = std::get<1>(gradients);
390392

391-
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> hessians = objective_->calculateRunningCostHessian(x, u);
393+
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd, Eigen::MatrixXd> hessians = objective_->calculateRunningCostHessian(x, u, i);
392394
Eigen::MatrixXd l_xx = std::get<0>(hessians);
393395
Eigen::MatrixXd l_ux = std::get<1>(hessians);
394396
Eigen::MatrixXd l_uu = std::get<2>(hessians);
@@ -507,7 +509,7 @@ bool CDDPProblem::solveBackwardPass() {
507509
K_.at(i) = K;
508510

509511
// Update Value Function
510-
double cost = objective_->calculateRunningCost(x, u);
512+
double cost = objective_->calculateRunningCost(x, u, i);
511513
V_X_.at(i) = Q_x + K.transpose() * Q_uu * k + Q_ux.transpose() * k + K.transpose() * Q_u;
512514
V_XX_.at(i) = Q_xx + K.transpose() * Q_uu * K + Q_ux.transpose() * K + K.transpose() * Q_ux;
513515

‎test/ObjectiveFunctionTests.cpp

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -30,24 +30,31 @@ void testQuadraticCost() {
3030
Eigen::MatrixXd Q = Eigen::MatrixXd::Identity(state_dim, state_dim);
3131
Eigen::MatrixXd R = Eigen::MatrixXd::Identity(control_dim, control_dim) * 0.1;
3232
Eigen::MatrixXd Qf = Eigen::MatrixXd::Identity(state_dim, state_dim) * 2.0;
33-
Eigen::VectorXd goal_state = Eigen::VectorXd::Zero(state_dim);
33+
Eigen::VectorXd goal_state = Eigen::VectorXd::Ones(state_dim);
34+
std::vector<Eigen::VectorXd> X_ref(horizon, goal_state);
3435

3536
// Instantiate QuadraticCost object
3637
QuadraticCost cost_fn(Q, R, Qf, goal_state, dt);
3738

39+
// Instantiate QuadraticTrackingCost object
40+
QuadraticTrackingCost tracking_cost_fn(Q, R, Qf, X_ref, goal_state, dt);
41+
3842
// Example state and control
3943
Eigen::VectorXd x(state_dim);
4044
x << 1.0, 0.5;
45+
std::vector<Eigen::VectorXd> X(horizon, x);
46+
4147
Eigen::VectorXd u(control_dim);
4248
u << 0.8;
49+
std::vector<Eigen::VectorXd> U(horizon-1, u);
4350

44-
// Test running cost
45-
double expected_running_cost = ((x - goal_state).transpose() * Q * (x - goal_state))[0] + (u.transpose() * R * u)[0] * dt;
46-
double calculated_running_cost = cost_fn.calculateRunningCost(x, u);
47-
assert(std::abs(calculated_running_cost - expected_running_cost) < 1e-6);
51+
// Test Trajectory cost
52+
double expected_cost = cost_fn.calculateCost(X, U);
53+
double expected_tracking_cost = tracking_cost_fn.calculateCost(X, U);
54+
assert(expected_cost - expected_tracking_cost < 1e-6);
4855

4956
// Test running cost gradient
50-
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> grads = cost_fn.calculateRunningCostGradient(x, u);
57+
std::tuple<Eigen::MatrixXd, Eigen::MatrixXd> grads = cost_fn.calculateRunningCostGradient(x, u, 0);
5158
Eigen::VectorXd expected_grad_x = 2 * Q * (x - goal_state) * dt;
5259
Eigen::VectorXd expected_grad_u = 2 * R * u * dt;
5360
assert(compareVectors(std::get<0>(grads), expected_grad_x));

0 commit comments

Comments
 (0)