dynamics_solver.h

Go to the documentation of this file.

//
// Copyright (c) 2019, Wolfgang Merkt
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#ifndef EXOTICA_CORE_DYNAMICS_SOLVER_H_
#define EXOTICA_CORE_DYNAMICS_SOLVER_H_

#include <exotica_core/factory.h>
#include <exotica_core/object.h>
#include <exotica_core/property.h>
#include <exotica_core/tools.h>

#include <exotica_core/dynamics_solver_initializer.h>

#define REGISTER_DYNAMICS_SOLVER_TYPE(TYPE, DERIV) EXOTICA_CORE_REGISTER(exotica::DynamicsSolver, TYPE, DERIV)

namespace exotica
{
class Scene;

enum Integrator
{
    RK1 = 0,          
    SymplecticEuler,  
    RK2,              
    RK4,              
};

template <typename T, int NX, int NU>
class AbstractDynamicsSolver : public Object, Uncopyable, public virtual InstantiableBase
{
public:
    typedef Eigen::Matrix<T, NX, 1> StateVector;         
    typedef Eigen::Matrix<T, NU, 1> ControlVector;       
    typedef Eigen::Matrix<T, NX, NX> StateDerivative;    
    typedef Eigen::Matrix<T, NX, NU> ControlDerivative;  

    AbstractDynamicsSolver();
    virtual ~AbstractDynamicsSolver();

    virtual void InstantiateBase(const Initializer& init);

    virtual void AssignScene(std::shared_ptr<Scene> scene_in);

    virtual void SetDt(double dt_in);

    virtual StateVector f(const StateVector& x, const ControlVector& u) = 0;

    virtual StateVector F(const StateVector& x, const ControlVector& u);

    virtual void ComputeDerivatives(const StateVector& x, const ControlVector& u);

    const StateDerivative& get_Fx() const;

    const ControlDerivative& get_Fu() const;

    const StateDerivative& get_fx() const;

    const ControlDerivative& get_fu() const;

    virtual StateDerivative fx(const StateVector& x, const ControlVector& u);

    virtual ControlDerivative fu(const StateVector& x, const ControlVector& u);

    StateDerivative fx_fd(const StateVector& x, const ControlVector& u);

    ControlDerivative fu_fd(const StateVector& x, const ControlVector& u);

    const bool& get_has_second_order_derivatives() const
    {
        return has_second_order_derivatives_;
    }

    // NOTE: Second order derivatives a 3D matrices, i.e. tensors
    //  We use the numerator convention (see https://en.wikipedia.org/wiki/Matrix_calculus)
    // X_i,j,k = d(X_i,j)/d x_k
    //
    // Additionally, the first subscript is the *second* partial derivative.
    //  I.e. f_xu = (f_u)_x
    // TODO: Eigen::Tensor to be replaced with exotica::Hessian
    virtual Eigen::Tensor<T, 3> fxx(const StateVector& x, const ControlVector& u);
    virtual Eigen::Tensor<T, 3> fuu(const StateVector& x, const ControlVector& u);
    virtual Eigen::Tensor<T, 3> fxu(const StateVector& x, const ControlVector& u);

    // TODO: To be deprecated - or at least remove its use - as it's difficult to get partial derivatives.
    StateVector Simulate(const StateVector& x, const ControlVector& u, T t);

    virtual StateVector StateDelta(const StateVector& x_1, const StateVector& x_2)
    {
        assert(x_1.size() == x_2.size());
        return x_1 - x_2;
    }

    void StateDelta(const StateVector& x_1, const StateVector& x_2, Eigen::VectorXdRef xout)
    {
        xout = StateDelta(x_1, x_2);
    }

    virtual Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> dStateDelta(const StateVector& x_1, const StateVector& x_2, const ArgumentPosition first_or_second)
    {
        assert(x_1.size() == x_2.size());
        assert(first_or_second == ArgumentPosition::ARG0 || first_or_second == ArgumentPosition::ARG1);

        if (!second_order_derivatives_initialized_) InitializeSecondOrderDerivatives();

        if (first_or_second == ArgumentPosition::ARG0)
            return dStateDelta_;
        else
            return -1.0 * dStateDelta_;
    }

    virtual Hessian ddStateDelta(const StateVector& x_1, const StateVector& x_2, const ArgumentPosition first_or_second)
    {
        // In Euclidean spaces, this is zero.

        assert(x_1.size() == x_2.size());
        assert(first_or_second == ArgumentPosition::ARG0 || first_or_second == ArgumentPosition::ARG1);

        if (!second_order_derivatives_initialized_) InitializeSecondOrderDerivatives();

        return ddStateDelta_;
    }

    virtual Eigen::Matrix<T, Eigen::Dynamic, 1> GetPosition(Eigen::VectorXdRefConst x_in);

    int get_num_controls() const;

    int get_num_positions() const;

    int get_num_velocities() const;

    int get_num_state() const;

    int get_num_state_derivative() const;

    T get_dt() const;

    Integrator get_integrator() const;

    void set_integrator(Integrator integrator_in);

    void SetIntegrator(const std::string& integrator_in);

    //  returns: Two-column matrix, first column contains low control limits,
    //      second - the high control limits
    const Eigen::MatrixXd& get_control_limits();

    void set_control_limits(Eigen::VectorXdRefConst control_limits_low, Eigen::VectorXdRefConst control_limits_high);

    const bool& get_has_state_limits() const
    {
        return has_state_limits_;
    }

    void ClampToStateLimits(Eigen::Ref<Eigen::VectorXd> state_in);

    virtual ControlVector InverseDynamics(const StateVector& state);

    virtual void Integrate(const StateVector& x, const StateVector& dx, const double dt, StateVector& xout);

private:
    bool control_limits_initialized_ = false;
    Eigen::VectorXd raw_control_limits_low_, raw_control_limits_high_;
    Eigen::MatrixXd dStateDelta_;
    Hessian ddStateDelta_;

protected:
    int num_controls_ = -1;          
    int num_positions_ = -1;         
    int num_velocities_ = -1;        
    int num_state_ = -1;             
    int num_state_derivative_ = -1;  

    bool has_second_order_derivatives_ = false;          
    bool second_order_derivatives_initialized_ = false;  

    bool has_state_limits_ = false;       
    Eigen::VectorXd state_limits_lower_;  
    Eigen::VectorXd state_limits_upper_;  

    T dt_ = 0.01;                              
    Integrator integrator_ = Integrator::RK1;  
    // TODO: Need to enforce control limits.
    // First column is the low limits, second is the high limits.
    Eigen::MatrixXd control_limits_;  

    // TODO: To be deprecated in favour of explicit call to Integrate in Simulate
    virtual StateVector SimulateOneStep(const StateVector& x, const ControlVector& u);

    void InitializeSecondOrderDerivatives();
    Eigen::Tensor<T, 3> fxx_default_, fuu_default_, fxu_default_;

    Eigen::MatrixXd fx_;  
    Eigen::MatrixXd fu_;  
    Eigen::MatrixXd Fx_;  
    Eigen::MatrixXd Fu_;  
};

typedef AbstractDynamicsSolver<double, Eigen::Dynamic, Eigen::Dynamic> DynamicsSolver;

typedef std::shared_ptr<exotica::DynamicsSolver> DynamicsSolverPtr;
}  // namespace exotica

#endif  // EXOTICA_CORE_DYNAMICS_SOLVER_H_