Contacts.h

Go to the documentation of this file.

/*
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
 * RDL - Robot Dynamics Library
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * Licensed under the zlib license. See LICENSE for more details.
 */

#ifndef RDL_CONTACTS_H
#define RDL_CONTACTS_H

#include <rdl_dynamics/rdl_eigenmath.h>
#include <rdl_dynamics/rdl_mathutils.h>
#include <rdl_dynamics/SpatialForce.hpp>

namespace RobotDynamics
{
struct Model;

struct ConstraintSet
{
    ConstraintSet() : linear_solver(Math::LinearSolverColPivHouseholderQR), bound(false)
    {
    }

    unsigned int addConstraint(unsigned int body_id, const Math::Vector3d& body_point, const Math::Vector3d& world_normal, const char* constraint_name = NULL,
                               double normal_acceleration = 0.);

    ConstraintSet Copy()
    {
        ConstraintSet result(*this);

        result.bound = false;

        return result;
    }

    void setSolver(Math::LinearSolver solver)
    {
        linear_solver = solver;
    }

    bool bind(const Model& model);

    size_t size() const
    {
        return acceleration.size();
    }

    void clear();

    Math::LinearSolver linear_solver;

    bool bound;

    std::vector<std::string> name;
    std::vector<unsigned int> body;
    std::vector<Math::Vector3d> point;
    std::vector<Math::Vector3d> normal;

    Math::VectorNd acceleration;

    Math::VectorNd force;

    Math::VectorNd impulse;

    Math::VectorNd v_plus;

    // Variables used by the Lagrangian methods

    Math::MatrixNd H;

    Math::VectorNd C;

    Math::VectorNd gamma;
    Math::MatrixNd G;

    Math::MatrixNd A;

    Math::VectorNd b;

    Math::VectorNd x;

    Eigen::HouseholderQR<Math::MatrixNd> GT_qr;

    Math::MatrixNd GT_qr_Q;
    Math::MatrixNd Y;
    Math::MatrixNd Z;
    Math::VectorNd qddot_y;
    Math::VectorNd qddot_z;

    // Variables used by the IABI methods

    Math::MatrixNd K;

    Math::VectorNd a;

    Math::VectorNd QDDot_t;

    Math::VectorNd QDDot_0;

    Math::SpatialForceV f_t;

    Math::SpatialForceV f_ext_constraints;

    std::vector<Math::Vector3d> point_accel_0;

    Math::SpatialForceV d_pA;

    std::vector<Math::SpatialVector> d_a;
    Math::VectorNd d_u;

    std::vector<Math::SpatialMatrix> d_IA;

    std::vector<Math::SpatialVector> d_U;

    Math::VectorNd d_d;

    std::vector<Math::Vector3d> d_multdof3_u;
};

void calcContactJacobian(Model& model, const Math::VectorNd& Q, const ConstraintSet& CS, Math::MatrixNd& G, bool update_kinematics = true);

void calcContactSystemVariables(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS);

void forwardDynamicsContactsDirect(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
                                   Math::VectorNd& QDDot);

void forwardDynamicsContactsRangeSpaceSparse(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
                                             Math::VectorNd& QDDot);

void forwardDynamicsContactsNullSpace(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
                                      Math::VectorNd& QDDot);

void forwardDynamicsContactsKokkevis(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDot, const Math::VectorNd& Tau, ConstraintSet& CS,
                                     Math::VectorNd& QDDot);

void computeContactImpulsesDirect(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);

void computeContactImpulsesRangeSpaceSparse(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);

void computeContactImpulsesNullSpace(Model& model, const Math::VectorNd& Q, const Math::VectorNd& QDotMinus, ConstraintSet& CS, Math::VectorNd& QDotPlus);

void solveContactSystemDirect(Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma, Math::VectorNd& qddot,
                              Math::VectorNd& lambda, Math::MatrixNd& A, Math::VectorNd& b, Math::VectorNd& x, Math::LinearSolver& linear_solver);

void solveContactSystemRangeSpaceSparse(Model& model, Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma,
                                        Math::VectorNd& qddot, Math::VectorNd& lambda, Math::MatrixNd& K, Math::VectorNd& a, Math::LinearSolver linear_solver);

void solveContactSystemNullSpace(Math::MatrixNd& H, const Math::MatrixNd& G, const Math::VectorNd& c, const Math::VectorNd& gamma, Math::VectorNd& qddot,
                                 Math::VectorNd& lambda, Math::MatrixNd& Y, Math::MatrixNd& Z, Math::VectorNd& qddot_y, Math::VectorNd& qddot_z,
                                 Math::LinearSolver& linear_solver);

} /* namespace RobotDynamics */

/* RDL_CONTACTS_H */
#endif  // ifndef RDL_CONTACTS_H