Skip to content

Commit 17db3ca

Browse files
JTylerBoylanJTylerBoylan
committed
work
1 parent 838c4c3 commit 17db3ca

File tree

8 files changed

+332
-7
lines changed

8 files changed

+332
-7
lines changed

‎docker/Dockerfile

+2-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,8 @@ FROM ubuntu:latest
22

33
RUN apt-get update -y && \
44
apt-get install -y g++ cmake git gdb && \
5-
apt-get install -y libeigen3-dev
5+
apt-get install -y libeigen3-dev && \
6+
apt-get install -y libginac-dev libcln-dev
67

78
RUN git clone https://github.com/osqp/osqp
89
RUN cd osqp && mkdir build

‎docker/Dockerfile.cuda

+3-2
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,9 @@
11
FROM nvidia/cuda:12.2.0-devel-ubuntu22.04
22

33
RUN apt-get update -y && \
4-
apt-get install -y cmake git gdb && \
5-
apt-get install -y libeigen3-dev
4+
apt-get install -y g++ cmake git gdb && \
5+
apt-get install -y libeigen3-dev && \
6+
apt-get install -y libginac-dev libcln-dev
67

78
RUN git clone https://github.com/osqp/osqp
89
RUN cd osqp && mkdir build

‎include/orlqp/MPCProblem.hpp

-4
Original file line numberDiff line numberDiff line change
@@ -77,13 +77,9 @@ namespace orlqp
7777
QPProblem::Ptr QP;
7878

7979
void calculateQPHessian();
80-
8180
void calculateQPGradient();
82-
8381
void calculateQPLinearConstraint();
84-
8582
void calculateQPLowerBound();
86-
8783
void calculateQPUpperBound();
8884
};
8985

‎include/orlqp/SymbolicQPProblem.hpp

+64
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
#ifndef ORLQP_SYMBOLIC_QP_PROBLEM_HPP_
2+
#define ORLQP_SYMBOLIC_QP_PROBLEM_HPP_
3+
4+
#include "orlqp/symbolic/types.hpp"
5+
#include "orlqp/QPProblem.hpp"
6+
7+
namespace orlqp
8+
{
9+
10+
class SymbolicQPProblem
11+
{
12+
/*************************************
13+
14+
Symbolic QP Problem
15+
16+
minimize f(x,c)
17+
subject to lb <= h(x,c) <= ub
18+
19+
x : Decision variables
20+
c : Constants
21+
f : Objective function
22+
h : Inequality constraints
23+
lb : Constraint lower bound
24+
ub : Constraint upper bound
25+
26+
*************************************/
27+
28+
public:
29+
using Ptr = std::shared_ptr<SymbolicQPProblem>;
30+
31+
SymbolVector x;
32+
SymbolVector c;
33+
GinacEx objective;
34+
GinacMatrix constraints;
35+
GinacMatrix lower_bound;
36+
GinacMatrix upper_bound;
37+
38+
SymbolicQPProblem();
39+
40+
QPProblem::Ptr getQP();
41+
42+
private:
43+
GinacMatrix sym_hessian;
44+
GinacMatrix sym_gradient;
45+
GinacMatrix sym_lin_constraints;
46+
GinacMatrix sym_lower_bound;
47+
GinacMatrix sym_upper_bound;
48+
void calculateSymbolicHessian();
49+
void calculateSymbolicGradient();
50+
void calculateSymbolicLinearConstraint();
51+
void calculateSymbolicLowerBound();
52+
void calculateSymbolicUpperBound();
53+
54+
QPProblem::Ptr QP;
55+
void calculateQPHessian();
56+
void calculateQPGradient();
57+
void calculateQPLinearConstraint();
58+
void calculateQPLowerBound();
59+
void calculateQPUpperBound();
60+
};
61+
62+
}
63+
64+
#endif

‎include/orlqp/symbolic/types.hpp

+18
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#ifndef ORLQP_SYMBOLIC_TYPES_HPP_
2+
#define ORLQP_SYMBOLIC_TYPES_HPP_
3+
4+
#include "orlqp/types.hpp"
5+
6+
#include "ginac/ginac.h"
7+
8+
namespace orlqp
9+
{
10+
11+
using GinacEx = GiNaC::ex;
12+
using GinacMatrix = GiNaC::matrix;
13+
using GinacSymbol = GiNaC::symbol;
14+
using SymbolVector = std::vector<GinacSymbol>;
15+
16+
}
17+
18+
#endif

‎include/orlqp/symbolic/util.hpp

+21
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
#ifndef ORLQP_SYMBOLIC_UTIL_HPP_
2+
#define ORLQP_SYMBOLIC_UTIL_HPP_
3+
4+
#include <string>
5+
6+
#include "orlqp/symbolic/types.hpp"
7+
8+
namespace orlqp
9+
{
10+
SymbolVector createSymbolVector(const int, const std::string &);
11+
12+
GinacMatrix calculateExpressionGradient(GinacEx &, SymbolVector &);
13+
14+
GinacMatrix calculateVectorJacobian(GinacMatrix &, SymbolVector &);
15+
16+
GinacMatrix calculateExpressionHessian(GinacEx &, SymbolVector &);
17+
18+
SymbolVector combineSymbolVectors(const std::vector<SymbolVector> &);
19+
}
20+
21+
#endif

‎src/symbolic.cpp

+123
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,123 @@
1+
/* SYMBOLIC UTILITY */
2+
#include "orlqp/symbolic/util.hpp"
3+
namespace orlqp
4+
{
5+
6+
SymbolVector createSymbolVector(const int size, const std::string &var)
7+
{
8+
SymbolVector vec(size);
9+
for (int i = 0; i < size; i++)
10+
{
11+
const std::string var_i = var + "[" + std::to_string(i) + "]";
12+
vec[i] = GinacSymbol(var_i);
13+
}
14+
return vec;
15+
}
16+
17+
GinacMatrix calculateExpressionGradient(GinacEx &f, SymbolVector &x)
18+
{
19+
const int N = x.size();
20+
GinacMatrix G(N, 1);
21+
for (int i = 0; i < N; i++)
22+
{
23+
G(i, 0) = f.diff(x[i]);
24+
}
25+
return G;
26+
}
27+
28+
GinacMatrix calculateVectorJacobian(GinacMatrix &G, SymbolVector &x)
29+
{
30+
const int N1 = G.rows();
31+
const int N2 = x.size();
32+
GinacMatrix J(N1, N2);
33+
for (int i = 0; i < N1; i++)
34+
{
35+
const GinacEx &expr = G[i];
36+
for (int j = 0; j < N2; j++)
37+
{
38+
const GinacSymbol sym = x[j];
39+
J(i, j) = expr.diff(sym);
40+
}
41+
}
42+
return J;
43+
}
44+
45+
GinacMatrix calculateExpressionHessian(GinacEx &f, SymbolVector &x)
46+
{
47+
auto G = calculateExpressionGradient(f, x);
48+
auto H = calculateVectorJacobian(G, x);
49+
return H;
50+
}
51+
52+
SymbolVector combineSymbolVectors(const std::vector<SymbolVector> &vecs)
53+
{
54+
size_t totalSize = 0;
55+
for (const auto &vec : vecs)
56+
totalSize += vec.size();
57+
SymbolVector result;
58+
result.reserve(totalSize);
59+
for (const auto &vec : vecs)
60+
result.insert(result.cend(), vec.cbegin(), vec.cend());
61+
return result;
62+
}
63+
64+
}
65+
66+
/* SYMBOLIC QP PROBLEM */
67+
#include "orlqp/SymbolicQPProblem.hpp"
68+
namespace orlqp
69+
{
70+
71+
SymbolicQPProblem::SymbolicQPProblem()
72+
{
73+
/* TODO */
74+
}
75+
76+
QPProblem::Ptr SymbolicQPProblem::getQP()
77+
{
78+
/* TODO */
79+
}
80+
81+
void SymbolicQPProblem::calculateSymbolicHessian()
82+
{
83+
/* TODO */
84+
}
85+
void SymbolicQPProblem::calculateSymbolicGradient()
86+
{
87+
/* TODO */
88+
}
89+
void SymbolicQPProblem::calculateSymbolicLinearConstraint()
90+
{
91+
/* TODO */
92+
}
93+
void SymbolicQPProblem::calculateSymbolicLowerBound()
94+
{
95+
/* TODO */
96+
}
97+
void SymbolicQPProblem::calculateSymbolicUpperBound()
98+
{
99+
/* TODO */
100+
}
101+
102+
void SymbolicQPProblem::calculateQPHessian()
103+
{
104+
/* TODO */
105+
}
106+
void SymbolicQPProblem::calculateQPGradient()
107+
{
108+
/* TODO */
109+
}
110+
void SymbolicQPProblem::calculateQPLinearConstraint()
111+
{
112+
/* TODO */
113+
}
114+
void SymbolicQPProblem::calculateQPLowerBound()
115+
{
116+
/* TODO */
117+
}
118+
void SymbolicQPProblem::calculateQPUpperBound()
119+
{
120+
/* TODO */
121+
}
122+
123+
}

‎tests/tianze_lip.cpp

+101
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
#include <iostream>
2+
3+
#include "orlqp/symbolic/util.hpp"
4+
#include "orlqp/SymbolicQPProblem.hpp"
5+
6+
using namespace orlqp;
7+
8+
int main(void)
9+
{
10+
11+
// Params
12+
const int nodes_per_step = 4;
13+
const int num_steps = 3;
14+
const int num_nodes = nodes_per_step * num_steps;
15+
16+
/* Objective Function */
17+
18+
const int num_states = num_nodes * 4;
19+
auto q = createSymbolVector(num_states, "q");
20+
auto q_ref = createSymbolVector(num_states, "Qr"); // const
21+
auto w_target = createSymbolVector(num_states, "Wtar"); // const
22+
GinacEx J_target;
23+
for (int i = 0; i < num_states; i++)
24+
{
25+
J_target += w_target[i] * (q[i] - q_ref[i]) * (q[i] - q_ref[i]);
26+
}
27+
28+
auto sx = createSymbolVector(num_steps, "sx");
29+
auto sx_ref = createSymbolVector(num_steps, "Sxr"); // const
30+
auto w_stepx = createSymbolVector(num_states, "Wstpx"); // const
31+
GinacEx J_stepx;
32+
for (int i = 0; i < num_steps; i++)
33+
{
34+
J_stepx += w_stepx[i] * (sx[i] - sx_ref[i]) * (sx[i] - sx_ref[i]);
35+
}
36+
37+
auto sy = createSymbolVector(num_steps, "sy");
38+
auto rho_step = createSymbolVector(num_nodes, "rho_s");
39+
auto sy_ref = createSymbolVector(num_steps, "Syr"); // const
40+
auto w_stepy = createSymbolVector(num_nodes, "Wstpy"); // const
41+
GinacEx J_stepy;
42+
for (int i = 0; i < num_nodes; i++)
43+
{
44+
J_stepy += w_stepy[i] * rho_step[i] * rho_step[i];
45+
}
46+
47+
const int num_controls = num_nodes * 2;
48+
auto u = createSymbolVector(num_controls, "u");
49+
auto p_foot = createSymbolVector(2, "Pft"); // const
50+
GinacMatrix u_ref(num_controls, 1);
51+
for (int i = 0; i < num_nodes; i++)
52+
{
53+
const int step_i = std::floor(i / nodes_per_step);
54+
GinacEx sum_sx, sum_sy;
55+
for (int j = 0; j < step_i; j++)
56+
{
57+
sum_sx += sx[j];
58+
sum_sy += sy[j];
59+
}
60+
u_ref[2 * i - 1] = p_foot[0] + sum_sx;
61+
u_ref[2 * i] = p_foot[1] + sum_sy;
62+
}
63+
auto w_effort = createSymbolVector(num_controls, "Weff"); // const
64+
GinacEx J_effort;
65+
for (int i = 0; i < num_controls; i++)
66+
{
67+
J_effort += w_effort[i] * (u[i] - u_ref[i]) * (u[i] - u_ref[i]);
68+
}
69+
70+
auto rho_avoid = createSymbolVector(num_nodes, "rho_a");
71+
auto w_avoid = createSymbolVector(num_nodes, "Wavd"); // const
72+
GinacEx J_avoid;
73+
for (int i = 0; i < num_nodes; i++)
74+
{
75+
J_avoid += w_avoid[i] * rho_avoid[i] * rho_avoid[i];
76+
}
77+
78+
auto rho_input = createSymbolVector(num_nodes, "rho_i");
79+
auto w_input = createSymbolVector(num_nodes, "Winp"); // const
80+
GinacEx J_input;
81+
for (int i = 0; i < num_nodes; i++)
82+
{
83+
J_input += w_input[i] * rho_input[i] * rho_input[i];
84+
}
85+
86+
GinacEx J = J_target + J_stepx + J_stepy + J_effort + J_avoid + J_input;
87+
88+
/* Constraints + Bounds */
89+
/* TODO */
90+
91+
SymbolVector x = combineSymbolVectors({q, u, sx, sy, rho_step, rho_avoid, rho_input});
92+
SymbolVector c = combineSymbolVectors({q_ref, sx_ref, sy_ref, p_foot, w_target, w_stepx, w_stepy, w_effort, w_avoid, w_input});
93+
94+
/* Symbolic QP Problem */
95+
/* TODO */
96+
97+
GinacMatrix hessian = calculateExpressionHessian(J, x);
98+
GinacMatrix gradient = calculateExpressionGradient(J, x);
99+
100+
return 0;
101+
}

0 commit comments

Comments
 (0)