dynamics.cpp

Go to the documentation of this file.

#include <mpc/dynamics.h>

namespace MPC {

        bool setDynamicFunction(Dynamics f, DynamicsD fd) {
                dyn = f;
                dynd = fd;

                return true;
        }

        bool setCostFunction(CostFunction f, CostFunctionD fd) {
                cost = f;
                costd = fd;

                return true;
        }

        float dynamicsAndCost(const VectorT &x, const VectorT &u) {
                // Compute cost only
                return cost(x, u);
        }

        void dynamicsAndCost(const VectorT &x, const VectorT &u, VectorT &xNew, float &cNew) {
                dyn(x, u, xNew);
                cNew = cost(x, u);
        }

        void dynamicsAndCost(const VectorT &x, const VectorT &u, MatrixT::ColXpr xNew, float &cNew) {
                // TODO: remove this workaround with the temporary vector!
                VectorT tmp;
                dyn(x, u, tmp);
                xNew = tmp;
                cNew = cost(x, u);
        }

        void dynamicsAndCost(Trajectory &t) {
                int N = t.x.cols();
                VectorT tmp;
                t.cost.resize(N);
                if (not t.started) {
                        // just update trajectory cost
                        for (int i = 0; i < N; ++i) {
                                t.cost(i) = cost(t.x.col(i), t.u.col(i));
                        }
                } else {
                        // if we have some actions also compute dynamics
                        for (int i = 0; i < N; ++i) {
                                if (i < N-1) {
                                        dyn(t.x.col(i), t.u.col(i), tmp);
                                        t.x.col(i+1) = tmp;
                                }
                                t.cost(i) = cost(t.x.col(i), t.u.col(i));
                        }
                }
        }

        void dynamicsAndCostDerivative(Trajectory &t, TrajectoryDeriv &d) {
                int N = t.u.cols();
                assert(N == (int)d.size() - 1);
                // zero action in the last step
                VectorT uN(VectorT::Zero(t.u.rows()));
                dynd(t.x.col(N), uN, d[N]);
                costd(t.x.col(N), uN, d[N]);
                for (int i = N-1; i >= 0; --i) {
                        // compute dynamics derivative first
                        dynd(t.x.col(i), t.u.col(i), d[i]);
                        costd(t.x.col(i), t.u.col(i), d[i]);
                }
        }
}