# RobotLocomotion/drake

Switch branches/tags
Nothing to show
Fetching contributors…
Cannot retrieve contributors at this time
121 lines (100 sloc) 3.83 KB
 #include "drake/systems/analysis/lyapunov.h" #include #include #include "drake/examples/pendulum/pendulum_plant.h" #include "drake/systems/framework/vector_system.h" namespace drake { namespace systems { namespace analysis { namespace { using symbolic::Variable; using symbolic::Expression; /// Cubic Polynomial System: /// ẋ = -x + x³ /// y = x class CubicPolynomialSystem : public systems::VectorSystem { public: CubicPolynomialSystem() : systems::VectorSystem(0, 0) { // Zero inputs, zero outputs. this->DeclareContinuousState(1); // One state variable. } private: // ẋ = -x + x³ virtual void DoCalcVectorTimeDerivatives( const systems::Context&, const Eigen::VectorBlock&, const Eigen::VectorBlock& state, Eigen::VectorBlock* derivatives) const { using std::pow; (*derivatives)(0) = -state(0) + pow(state(0), 3.0); } }; VectorX cubic_bases(VectorX x) { return Vector1{x.dot(x)}; } // Verifies by only taking samples inside the region of attraction of the // origin, x ∈ [-1, 1]. This is taken from the example in // http://underactuated.mit.edu/underactuated.html?chapter=lyapunov . GTEST_TEST(LyapunovTest, CubicPolynomialTest) { CubicPolynomialSystem cubic; auto context = cubic.CreateDefaultContext(); Eigen::RowVectorXd x_samples = Eigen::RowVectorXd::LinSpaced(21, -1., 1.); Eigen::VectorXd params = SampleBasedLyapunovAnalysis( cubic, *context, &cubic_bases, x_samples, Vector1d{0.}); // Make sure that the solver found some reasonable, well-conditioned answer. EXPECT_GE(params(0), .1); EXPECT_LE(params(0), 10.); } template VectorX pendulum_bases(const VectorX& x) { const T s = sin(x[0]); const T c = cos(x[0]); const T qd = x[1]; VectorX monomials(9); monomials << 1, s, c, qd, s * s, s * c, s * qd, c * qd, qd * qd; return monomials; } GTEST_TEST(LyapunovTest, PendulumSampleBasedLyapunov) { examples::pendulum::PendulumPlant pendulum; auto context = pendulum.CreateDefaultContext(); context->FixInputPort(0, Vector1d{0.0}); Eigen::VectorXd q_samples = Eigen::VectorXd::LinSpaced(31, -1.5 * M_PI, 1.5 * M_PI); Eigen::VectorXd qd_samples = Eigen::VectorXd::LinSpaced(21, -10., 10.); Eigen::Matrix2Xd x_samples(2, q_samples.size() * qd_samples.size()); int xi = 0; for (int qi = 0; qi < q_samples.size(); qi++) { for (int qdi = 0; qdi < qd_samples.size(); qdi++) { x_samples.col(xi++) = Eigen::Vector2d{q_samples(qi), qd_samples(qdi)}; } } // V(0) = 0. const Eigen::Vector2d x_zero = Eigen::Vector2d::Zero(); Eigen::VectorXd params = SampleBasedLyapunovAnalysis( pendulum, *context, &pendulum_bases, x_samples, x_zero); // Zero the small coefficients, for textual display. for (int i = 0; i < params.size(); i++) { if (fabs(params(i)) < 1e-4) { params(i) = 0.; } } // Check if V ends up near a constant factor times mechanical energy: const auto& p = pendulum.get_parameters(*context); Eigen::VectorXd energy_params = Eigen::VectorXd::Zero(9); energy_params(0) = p.mass() * p.gravity() * p.length(); energy_params(2) = -p.mass() * p.gravity() * p.length(); energy_params(8) = p.mass() * p.length() * p.length(); const double factor = energy_params(0) / params(0); params *= factor; EXPECT_NEAR(params[2], energy_params[2], 1e-4); Vector2 state{Variable("q"), Variable("qd")}; drake::log()->info( "E = " + energy_params.dot(pendulum_bases(state)).to_string()); drake::log()->info("V = " + params.dot(pendulum_bases(state)).to_string()); } } // namespace } // namespace analysis } // namespace systems } // namespace drake