EEContactModel.h

Go to the documentation of this file.

/**********************************************************************************************************************
This file is part of the Control Toolbox (https://github.com/ethz-adrl/control-toolbox), copyright by ETH Zurich.
Licensed under the BSD-2 license (see LICENSE file in main directory)
**********************************************************************************************************************/

#pragma once

#include <ct/rbd/state/RBDState.h>

#pragma GCC diagnostic push  // include IIT headers and disable warnings
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wunused-value"
#include <iit/rbd/rbd.h>
#include <iit/rbd/robcogen_commons.h>
#include <iit/rbd/traits/TraitSelector.h>
#pragma GCC diagnostic pop

namespace ct {
namespace rbd {


template <class Kinematics>
class EEContactModel
{
public:
    static const size_t NUM_EE = Kinematics::NUM_EE;
    static const size_t NJOINTS = Kinematics::NJOINTS;

    typedef typename Kinematics::SCALAR SCALAR;

    typedef typename iit::rbd::tpl::TraitSelector<SCALAR>::Trait TRAIT;

    typedef std::array<bool, NUM_EE> ActiveMap;
    typedef typename Kinematics::EEForceLinear EEForceLinear;
    typedef std::array<EEForceLinear, NUM_EE> EEForcesLinear;

    typedef Eigen::Matrix<SCALAR, 3, 1> Vector3s;
    typedef kindr::Position<SCALAR, 3> Position3S;
    typedef kindr::Velocity<SCALAR, 3> Velocity3S;


    enum VELOCITY_SMOOTHING
    {
        NONE = 0,     
        SIGMOID = 1,  
        TANH = 2,     
        ABS = 3       
    };

    EEContactModel(const SCALAR& k = SCALAR(5000),
        const SCALAR& d = SCALAR(500.0),
        const SCALAR& alpha = SCALAR(100.0),
        const SCALAR& alpha_n = SCALAR(-1.0),
        const SCALAR& zOffset = SCALAR(0.0),
        const VELOCITY_SMOOTHING& smoothing = NONE,
        const std::shared_ptr<Kinematics>& kinematics = std::shared_ptr<Kinematics>(new Kinematics()))
        : kinematics_(kinematics),
          smoothing_(smoothing),
          k_(k),
          d_(d),
          alpha_(alpha),
          alpha_n_(alpha_n),
          zOffset_(zOffset)
    {
        for (size_t i = 0; i < NUM_EE; i++)
            EEactive_[i] = true;
    }

    EEContactModel(const EEContactModel& other)
        : kinematics_(other.kinematics_->clone()),
          smoothing_(other.smoothing_),
          k_(other.k_),
          d_(other.d_),
          alpha_(other.alpha_),
          alpha_n_(other.alpha_n_),
          zOffset_(other.zOffset_),
          EEactive_(other.EEactive_)
    {
    }

    EEContactModel* clone() const { return new EEContactModel(*this); }
    void setActiveEE(const ActiveMap& activeMap) { EEactive_ = activeMap; }
    EEForcesLinear computeContactForces(const RBDState<NJOINTS, SCALAR>& state)
    {
        EEForcesLinear eeForces;

        for (size_t i = 0; i < NUM_EE; i++)
        {
            if (EEactive_[i])
            {
                Vector3s eePenetration = computePenetration(i, state.basePose(), state.jointPositions());

                if (eeInContact(eePenetration))
                {
                    Velocity3S eeVelocity = kinematics_->getEEVelocityInWorld(i, state);
                    eeForces[i] = computeEEForce(eePenetration, eeVelocity);
                }
                else
                {
                    eeForces[i].setZero();
                }
            }
        }

        return eeForces;
    }

    SCALAR& alpha() { return alpha_; }
    SCALAR& alpha_n() { return alpha_n_; }
    SCALAR& k() { return k_; }
    SCALAR& d() { return d_; }
    SCALAR& zOffset() { return zOffset_; }
    VELOCITY_SMOOTHING& smoothing() { return smoothing_; }

private:
    bool eeInContact(const Vector3s& eePenetration)
    {
        if (smoothing_ == NONE && eePenetration(2) > 0.0)
            return false;
        else
            return true;
    }


    Vector3s computePenetration(const size_t& eeId,
        const tpl::RigidBodyPose<SCALAR>& basePose,
        const typename JointState<NJOINTS, SCALAR>::Position& jointPosition)
    {
        Position3S pos = kinematics_->getEEPositionInWorld(eeId, basePose, jointPosition);

        // we currently assume flat ground at height zero penetration is only z height
        Vector3s penetration;
        penetration << SCALAR(0.0), SCALAR(0.0), pos.z();

        return penetration;
    }

    EEForceLinear computeEEForce(const Vector3s& eePenetration, const Velocity3S& eeVelocity)
    {
        EEForceLinear eeForce;

        computeDamperForce(eeForce, eePenetration, eeVelocity);

        smoothEEForce(eeForce, eePenetration);

        computeNormalSpring(eeForce, eePenetration(2) - zOffset_, eeVelocity.toImplementation()(2));

        return eeForce;
    }

    void smoothEEForce(EEForceLinear& eeForce, const Vector3s& eePenetration)
    {
        switch (smoothing_)
        {
            case NONE:
                return;
            case SIGMOID:
                eeForce *= 1. / (1. + TRAIT::exp(eePenetration(2) * alpha_));
                return;
            case TANH:
                // same as sigmoid, maybe cheaper / more expensive to compute?
                eeForce *= 0.5 * TRAIT::tanh(-0.5 * eePenetration(2) * alpha_) + 0.5;
                return;
            case ABS:
                eeForce *= 0.5 * -eePenetration(2) * alpha_ / (1. + TRAIT::fabs(-eePenetration(2) * alpha_)) + 0.5;
                return;
            default:
                throw std::runtime_error("undefined smoothing function");
        }
    }

    void computeDamperForce(EEForceLinear& force, const Vector3s& eePenetration, const Velocity3S& eeVelocity)
    {
        force = -d_ * eeVelocity.toImplementation();
    }

    void computeNormalSpring(EEForceLinear& force, const SCALAR& p_N, const SCALAR& p_dot_N)
    {
        if (alpha_n_ > SCALAR(0))
        {
            force(2) += k_ * TRAIT::exp(-alpha_n_ * p_N);
        }
        else if (p_N <= SCALAR(0))
        {
            force(2) -= k_ * p_N;
        }
    }


    std::shared_ptr<Kinematics> kinematics_;

    VELOCITY_SMOOTHING smoothing_;

    SCALAR k_;        
    SCALAR d_;        
    SCALAR alpha_;    
    SCALAR alpha_n_;  
    SCALAR zOffset_;  

    ActiveMap EEactive_;  
};
}  // namespace rbd
}  // namespace ct