aligator  0.15.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
Loading...
Searching...
No Matches
wheeled-inverted-pendulum.hpp
Go to the documentation of this file.
1#pragma once
2
4
5namespace aligator::dynamics {
6
7template <typename _Scalar>
9 using Scalar = _Scalar;
14 WheeledInvertedPendulumDynamicsTpl(const double gravity, const double length)
15 : Base(VectorSpace{}, 2)
16 , length_(length)
17 , gravity_(gravity) {}
18
19 double length_;
20 double gravity_;
21
22 void forward(const ConstVectorRef &x, const ConstVectorRef &u,
23 ODEData &data) const override {
24 Scalar rdot = x[0], phidot = x[1], theta = x[2], thetadot = x[3],
25 phi = x[4], posx = x[5], posy = x[6];
26 Scalar rdotdot = u[0], phidotdot = u[1];
27
28 data.xdot_[0] = rdotdot;
29 data.xdot_[1] = phidotdot;
30 data.xdot_[2] = thetadot;
31 data.xdot_[3] = std::sin(theta) * gravity_ / length_ -
32 std::cos(theta) * rdotdot / length_;
33 data.xdot_[4] = phidot;
34 data.xdot_[5] = rdot * std::cos(phi);
35 data.xdot_[6] = rdot * std::sin(phi);
36 }
37
38 void dForward(const ConstVectorRef &x, const ConstVectorRef &u,
39 ODEData &data) const override {
40 Scalar rdot = x[0], phidot = x[1], theta = x[2], thetadot = x[3],
41 phi = x[4], rdotdot = u[0];
42
43 data.Jx_.setZero();
44 data.Jx_(2, 3) = 1;
45 data.Jx_(3, 2) = std::cos(theta) * gravity_ / length_ +
46 std::sin(theta) * rdotdot / length_;
47
48 data.Jx_(4, 1) = 1;
49 data.Jx_(5, 0) = std::cos(phi);
50 data.Jx_(5, 4) = -rdot * std::sin(phi);
51 data.Jx_(6, 0) = std::sin(phi);
52 data.Jx_(6, 4) = rdot * std::cos(phi);
53
54 data.Ju_.setZero();
55 data.Ju_(0, 0) = 1;
56 data.Ju_(1, 1) = 1;
57 data.Ju_(3, 0) = -1 * std::cos(theta) / length_;
58 }
59};
60
61} // namespace aligator::dynamics
Namespace for modelling system dynamics.
Defines a class representing ODEs.
Standard Euclidean vector space.
Data struct for ContinuousDynamicsAbstractTpl.
Base class for ODE dynamics .
dynamics::ContinuousDynamicsDataTpl< Scalar > ODEData
WheeledInvertedPendulumDynamicsTpl(const double gravity, const double length)
void dForward(const ConstVectorRef &x, const ConstVectorRef &u, ODEData &data) const override
Evaluate the vector field Jacobians.
void forward(const ConstVectorRef &x, const ConstVectorRef &u, ODEData &data) const override
Evaluate the ODE vector field: this returns the value of .