Contacts.hpp

Go to the documentation of this file.

// Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
// Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
// RDL - Robot Dynamics Library
// Licensed under the zlib license. See LICENSE for more details.
 
#ifndef RDL_CONTACTS_HPP
#define RDL_CONTACTS_HPP
 
#include <rdl_dynamics/rdl_eigenmath.hpp>
#include <rdl_dynamics/rdl_mathutils.hpp>
#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