I have the following problem, which is not the full problem: I need to convert into C++. However, I am getting somewhat different results in C++. It's still unclear why C++ gets a different result than the Python version.
import casadi as ca
import numpy as np
import matplotlib.pyplot as plt
import json
# Parameters and fixed initial pose
data = np.array([
[0.0714334, 0.00638967, 1.66001],
[0.171036, 0.015299, 1.66001],
[0.270638, 0.0242084, 1.66001],
[0.37024, 0.0331177, 1.66001],
[0.469843, 0.0420271, 1.66001],
[0.569445, 0.0509365, 1.66001],
[0.669047, 0.0598458, 1.66001],
[0.76865, 0.0687552, 1.66001],
[0.868252, 0.0776645, 1.66001],
[0.967854, 0.0865739, 1.66001],
[1.06746, 0.0954833, 1.66001],
[1.16706, 0.104393, 1.66001],
[1.26666, 0.113302, 1.66001],
[1.36626, 0.122211, 1.66001],
[1.46587, 0.131121, 1.66001],
[1.56547, 0.14003, 1.66001],
[1.66507, 0.148939, 1.66001],
[1.76467, 0.157849, 1.66001],
[1.86428, 0.166758, 1.66001],
[1.96388, 0.175668, 1.66001],
[2.06348, 0.184577, 1.66001],
[2.16308, 3.193486, 1.66001],
[2.26268, 0.202396, 1.66001],
[2.36229, 0.211305, 1.66001],
[2.46189, 3.220214, 1.66001],
[2.56149, 0.229124, 1.66001],
[2.66109, 0.238033, 1.66001],
[2.7607, 0.246942, 1.66001],
[2.8603, 0.255852, 1.66001],
[2.9599, 0.264761, 1.66001],
[3.0595, 0.27367, 1.66001],
[3.15911, 3.28258, 1.66001],
[3.25871, 0.291489, 1.66001],
[3.35831, 0.300399, 1.66001],
[3.45791, 0.309308, 1.66001],
[3.55751, 3.318217, 1.66001],
[3.65712, 0.327127, 1.66001],
[3.75672, 0.336036, 1.66001],
[3.85632, 0.444945, 1.66001],
[3.95592, 0.453855, 1.66001],
[4.05553, 0.462764, 1.66001],
[4.15513, 0.471673, 1.66001]
])
A_p = np.array([[-0.0645709, -0.997913], [0.0645709, 0.997913]])
b_p = np.array([0.737278, 0.632449])
x_list = data[:, 0]
y_list = data[:, 1]
theta_list = data[:, 2] # Sample orientation values
curvature_weight = 1.0
curvature_rate_weight = 1.0
distance_weight = 1.0
v_max = 1.0
delta_t = 0.1
wheelbase = 1.0 # Length of the wheelbase for car-like dynamics
n = len(x_list)
# Initial fixed pose
x0_val = x_list[0]
y0_val = y_list[0]
theta0_val = theta_list[0]
# Define optimization variables for remaining poses, velocities, and steering angles
x = ca.MX.sym('x', n - 1)
y = ca.MX.sym('y', n - 1)
theta = ca.MX.sym('theta', n - 1)
v = ca.MX.sym('v', n - 1) # Velocity variable for each timestep
delta = ca.MX.sym('delta', n - 1) # Steering angle for each timestep
# Objective function
objective = 0
# for i in range(n - 3): # Adjust indices since initial pose is excluded
# dds_x = x[i+2] - 2 * x[i+1] + x[i]
# dds_y = y[i+2] - 2 * y[i+1] + y[i]
# objective += curvature_weight * (dds_x**2 + dds_y**2)
# for i in range(n - 4):
# ddds_x = x[i+3] - 3 * x[i+2] + 3 * x[i+1] - x[i]
# ddds_y = y[i+3] - 3 * y[i+2] + 3 * y[i+1] - y[i]
# objective += curvature_rate_weight * (ddds_x**2 + ddds_y**2)
# Add penalty for distance between consecutive points
# for i in range(n - 2):
# distance_sq = (x[i+1] - x[i])**2 + (y[i+1] - y[i])**2
# print(distance_sq)
# objective += distance_weight * distance_sq # Penalize large distances between consecutive points
# # Constraints
g = []
lb_g = []
ub_g = []
# Kinematic constraints for car-like dynamics
for i in range(n - 2):
# x_t+1 = x_t + delta_t * v * cos(theta)
g.append(x[i+1] - (x[i] + delta_t * v[i] * ca.cos(theta[i])))
# print(x[i+1] - (x[i] + delta_t * v[i] * ca.cos(theta[i])))
lb_g.append(0)
ub_g.append(0)
# # y_t+1 = y_t + delta_t * v * sin(theta)
g.append(y[i+1] - (y[i] + delta_t * v[i] * ca.sin(theta[i])))
lb_g.append(0)
ub_g.append(0)
# # theta_t+1 = theta_t + delta_t * v * tan(delta) / L
g.append(theta[i+1] - (theta[i] + delta_t * v[i] * ca.tan(delta[i]) / wheelbase))
print(" ", theta[i+1] - (theta[i] + delta_t * v[i] * ca.tan(delta[i]) / wheelbase))
lb_g.append(0)
ub_g.append(0)
# # Velocity and steering angle constraints
# max_steering_angle = np.deg2rad(25)
max_steering_angle = 0.52
for i in range(n - 1):
# Velocity constraints
g.append(v[i])
lb_g.append(0.0) # Minimum velocity (no reversing)
ub_g.append(v_max) # Maximum velocity
# Steering angle constraints (example: max steering angle of 30 degrees)
g.append(delta[i])
lb_g.append(-max_steering_angle)
ub_g.append(max_steering_angle)
# for i in range(0, n-1):
# for j in range(0, 2):
# g.append(x[i]*A_p[j, 0] + y[i]*A_p[j, 1])
# lb_g.append(-ca.inf)
# ub_g.append(b_p[j])
# Define NLP problem without the fixed initial pose in the variables
nlp = {'x': ca.vertcat(x, y, theta, v, delta), 'f': objective, 'g': ca.vertcat(*g)}
# Set IPOPT solver options
opts = {
'ipopt.print_level': 5,
'print_time': False,
'ipopt.tol': 1e-6,
'ipopt.max_iter': 1000
}
solver = ca.nlpsol('solver', 'ipopt', nlp, opts)
# Initial guess (excluding the fixed initial pose)
x0_guess = np.hstack((x_list[1:], y_list[1:], theta_list[1:], np.ones(n - 1) * v_max, np.zeros(n - 1)))
print(x0_guess)
# Solve the problem
solution = solver(x0=x0_guess, lbx=-ca.inf, ubx=ca.inf, lbg=lb_g, ubg=ub_g)
problem_data = {
'variables': nlp['x'].size1(),
'objective': str(objective),
'constraints': [str(nlp['g'])],
'bounds': {
'lb_vars': -ca.inf,
'ub_vars': ca.inf,
'lb_constraints': lb_g,
'ub_constraints': ub_g,
}
}
# Write to JSON file
with open("nlp_debug_python.json", "w") as f:
json.dump(problem_data, f, indent=4)
print("NLP problem saved to nlp_debug.json")
# Extract optimized values, adding back fixed initial pose
x_opt = solution['x'][:n - 1]
y_opt = solution['x'][n - 1:2 * (n - 1)]
theta_opt = np.vstack((np.array([[theta0_val]]), solution['x'][2 * (n - 1):]))
v_opt = solution['x'][3 * (n - 1):4 * (n - 1)]
delta_opt = solution['x'][4 * (n - 1):]
# Visualization
plt.figure(figsize=(12, 8))
plt.plot(x_list, y_list, 'bo--', label='Initial Path')
plt.plot(x_opt, y_opt, 'ro-', label='Optimized Path')
# Boundary line plots
x_vals = np.linspace(-5, 5, 400)
y1_vals = (b_p[0] - A_p[0, 0] * x_vals) / A_p[0, 1]
y2_vals = (b_p[1] - A_p[1, 0] * x_vals) / A_p[1, 1]
plt.plot(x_vals, y1_vals, label=r'$-0.368x - 0.929y \leq 2.24$')
plt.plot(x_vals, y2_vals, label=r'$-0.368x - 0.929y \leq 0.87$')
plt.xlabel('X')
plt.ylabel('Y')
plt.legend()
plt.show()
Here is the C++ version, which gives different results, not quite sure where I made the mistake (
#include <casadi/casadi.hpp>
#include <vector>
#include <iostream>
#include <cmath>
#include "matplotlibcpp.h"
#include <nlohmann/json.hpp>
using namespace casadi;
using namespace std;
namespace plt = matplotlibcpp;
using json = nlohmann::json;
int main() {
// Parameters and fixed initial pose
DM data = DM({
{0.0714334, 0.00638967, 1.66001},
{0.171036, 0.015299, 1.66001},
{0.270638, 0.0242084, 1.66001},
{0.37024, 0.0331177, 1.66001},
{0.469843, 0.0420271, 1.66001},
{0.569445, 0.0509365, 1.66001},
{0.669047, 0.0598458, 1.66001},
{0.76865, 0.0687552, 1.66001},
{0.868252, 0.0776645, 1.66001},
{0.967854, 0.0865739, 1.66001},
{1.06746, 0.0954833, 1.66001},
{1.16706, 0.104393, 1.66001},
{1.26666, 0.113302, 1.66001},
{1.36626, 0.122211, 1.66001},
{1.46587, 0.131121, 1.66001},
{1.56547, 0.14003, 1.66001},
{1.66507, 0.148939, 1.66001},
{1.76467, 0.157849, 1.66001},
{1.86428, 0.166758, 1.66001},
{1.96388, 0.175668, 1.66001},
{2.06348, 0.184577, 1.66001},
{2.16308, 3.193486, 1.66001},
{2.26268, 0.202396, 1.66001},
{2.36229, 0.211305, 1.66001},
{2.46189, 3.220214, 1.66001},
{2.56149, 0.229124, 1.66001},
{2.66109, 0.238033, 1.66001},
{2.7607, 0.246942, 1.66001},
{2.8603, 0.255852, 1.66001},
{2.9599, 0.264761, 1.66001},
{3.0595, 0.27367, 1.66001},
{3.15911, 3.28258, 1.66001},
{3.25871, 0.291489, 1.66001},
{3.35831, 0.300399, 1.66001},
{3.45791, 0.309308, 1.66001},
{3.55751, 3.318217, 1.66001},
{3.65712, 0.327127, 1.66001},
{3.75672, 0.336036, 1.66001},
{3.85632, 0.444945, 1.66001},
{3.95592, 0.453855, 1.66001},
{4.05553, 0.462764, 1.66001},
{4.15513, 0.471673, 1.66001}});
DM A_p = DM::horzcat({{-0.0645709, 0.0645709}, {-0.997913, 0.997913}});
DM b_p = DM::vertcat({0.737278, 0.632449});
vector<double> x_list, y_list, theta_list;
int n = data.size1();
std::cout<< " n " << n << " cols " << data.size2() << std::endl;
for (int i = 0; i < n; ++i) {
x_list.push_back(data(i, 0).scalar());
y_list.push_back(data(i, 1).scalar());
theta_list.push_back(data(i, 2).scalar()); // Sample orientation values
std::cout<< " " << data(i, 0).scalar() << " " << data(i, 1).scalar() << " " << data(i, 2).scalar() << std::endl;
}
double curvature_weight = 1.0;
double curvature_rate_weight = 1.0;
double distance_weight = 1.0;
double v_max = 1.0;
double delta_t = 0.1;
double wheelbase = 1.0;
// Initial fixed pose
double x0_val = x_list[0];
double y0_val = y_list[0];
double theta0_val = theta_list[0];
// Define optimization variables for remaining poses, velocities, and steering angles
MX x = MX::sym("x", n - 1);
MX y = MX::sym("y", n - 1);
MX theta = MX::sym("theta", n - 1);
MX v = MX::sym("v", n - 1);
MX delta = MX::sym("delta", n - 1);
// Objective function
MX objective = 0.0;
// for (int i = 0; i < n - 3; ++i) {
// MX dds_x = x(i+2) - 2 * x(i+1) + x(i);
// MX dds_y = y(i+2) - 2 * y(i+1) + y(i);
// objective += curvature_weight * (dds_x*dds_x + dds_y*dds_y);
// }
// for (int i = 0; i < n - 4; ++i) {
// MX ddds_x = x(i+3) - 3 * x(i+2) + 3 * x(i+1) - x(i);
// MX ddds_y = y(i+3) - 3 * y(i+2) + 3 * y(i+1) - y(i);
// objective += curvature_rate_weight * (ddds_x*ddds_x + ddds_y*ddds_y);
// }
// Penalty for distance between consecutive points
// for (int i = 0; i < n - 2; ++i) {
// MX distance_sq = MX::pow(x(i+1) - x(i), 2) + MX::pow((y(i+1) - y(i)), 2);
// std::cout<< distance_sq << std::endl;
// objective += distance_sq;
// }
// Constraints
vector<MX> g;
vector<double> lb_g, ub_g;
// Kinematic constraints for car-like dynamics
for (int i = 0; i < n - 2; ++i) {
g.push_back(x(i+1) - (x(i) + delta_t * v(i) * cos(theta(i))));
lb_g.push_back(0.0);
ub_g.push_back(0.0);
g.push_back(y(i+1) - (y(i) + delta_t * v(i) * sin(theta(i))));
lb_g.push_back(0.0);
ub_g.push_back(0.0);
g.push_back(theta(i+1) - (theta(i) + delta_t * v(i) * (tan(delta(i)) / wheelbase)));
lb_g.push_back(0.0);
ub_g.push_back(0.0);
}
// Velocity and steering angle constraints
double max_steering_angle = M_PI / 6.0; // 30 degrees in radians
// double max_steering_angle = 0.1; // 30 degrees in radians
std::cout<< " max_steering_angle " << max_steering_angle << std::endl;
for (int i = 0; i < n - 1; ++i) {
g.push_back(v(i));
lb_g.push_back(0.0);
ub_g.push_back(v_max);
g.push_back(delta(i));
lb_g.push_back(-max_steering_angle);
ub_g.push_back(max_steering_angle);
}
// // Boundary constraints
// for (int i = 0; i < n - 1; ++i) {
// for (int j = 0; j < 2; ++j) {
// g.push_back(x(i) * A_p(j, 0) + y(i) * A_p(j, 1));
// lb_g.push_back(-casadi::inf);
// ub_g.push_back(static_cast<double>(b_p(j).scalar())); // Cast to double
// }
// }
// Define NLP problem without the fixed initial pose in the variables
MX nlp_vars = vertcat(x, y, theta, v, delta);
// std::cout<< " " << vars.size1() << " " << vars.size2() << std::endl;
// Set up IPOPT solver
Dict opts;
opts["ipopt.print_level"] = 5;
opts["print_time"] = false;
opts["ipopt.tol"] = 1e-6;
opts["ipopt.max_iter"] = 1000;
// opts["ipopt.constr_viol_tol"] = 1e-4;
auto g_cons = vertcat(g);
MXDict nlp = {{"x", nlp_vars}, {"f", objective}, {"g", g_cons}};
Function solver = nlpsol("nlpsol", "ipopt", nlp, opts);
// Initial guess (excluding the fixed initial pose)
vector<double> x0_guess;
for (int i = 1; i < n; ++i){
x0_guess.push_back(x_list[i]);
}
for (int i = 1; i < n; ++i) {
x0_guess.push_back(y_list[i]);
}
for (int i = 1; i < n; ++i){
x0_guess.push_back(theta_list[i]);
}
for (int i = 0; i < n - 1; ++i) {
x0_guess.push_back(v_max);
}
for (int i = 0; i < n - 1; ++i) {
x0_guess.push_back(0.0);
}
vector<double> lbx, ubx;
std::map<std::string, DM> arg;
arg["lbg"] = lb_g;
arg["ubg"] = ub_g;
arg["lbx"] = -DM::inf(); // Lower bounds
arg["ubx"] = DM::inf(); // Upper bounds
arg["x0"] = x0_guess;
// Solve the problem
std::map<string, DM> solution = solver(arg);
// // Extract optimized values
vector<double> x_opt(solution["x"](Slice(0, n - 1)).nonzeros());
vector<double> y_opt(solution["x"](Slice(n - 1, 2 * (n - 1))).nonzeros());
vector<double> theta_opt(solution["x"](Slice(2 * (n - 1), 3 * (n - 1))).nonzeros());
vector<double> v_opt(solution["x"](Slice(3 * (n - 1), 4 * (n - 1))).nonzeros());
vector<double> delta_opt(solution["x"](Slice(4 * (n - 1), 5 * (n - 1))).nonzeros());
cout << "Optimized Path: \n";
for (int i = 0; i < x_opt.size(); ++i) {
cout << "X: " << x_opt[i] << ", Y: " << y_opt[i] << endl;
}
std::vector<double> x_values, y_values;
for (int i = 0; i < data.size1(); ++i) {
x_values.push_back(static_cast<double>(data(i, 0))); // First column
y_values.push_back(static_cast<double>(data(i, 1))); // Second column
}
// Generate x values from -5 to 5, with 400 points
int num_points = 400;
std::vector<double> x_vals(num_points);
for (int i = 0; i < num_points; ++i) {
x_vals[i] = -5.0 + i * (10.0 / (num_points - 1)); // from -5 to 5
}
// Calculate y values for the lines based on A_p and b_p
std::vector<double> y1_vals(num_points);
std::vector<double> y2_vals(num_points);
for (int i = 0; i < num_points; ++i) {
y1_vals[i] = (static_cast<double>(b_p(0)) - static_cast<double>(A_p(0, 0)) * x_vals[i]) / static_cast<double>(A_p(0, 1));
y2_vals[i] = (static_cast<double>(b_p(1)) - static_cast<double>(A_p(1, 0)) * x_vals[i]) / static_cast<double>(A_p(1, 1));
}
// Plot the lines with labels
plt::figure();
plt::plot(x_vals, y1_vals, "r-");
plt::plot(x_vals, y2_vals, "b-");
plt::plot(x_values, y_values, "bo--"); // Blue line with circle markers
plt::plot(x_opt, y_opt, "ro-");
plt::legend();
plt::title("Trajectory Optimization");
plt::xlabel("X position");
plt::ylabel("Y position");
plt::show();
return 0;
}
Update:
The issue seems to be related to the value of max_steering_angle = np.deg2rad(30) = 0.5235987755982988. When I use 0.52 in both Python and C++, the results match. However, even with 0.5235987755982988, C++ gives the same result. In Python, though, using 0.5235987755982988 produces a different outcome. I suspect this discrepancy may be related to precession, but I'm not sure how to handle it.