iDynContact.h

Go to the documentation of this file.

/*
 * Copyright (C) 2010-2011 RobotCub Consortium
 * Author: Andrea Del Prete
 * CopyPolicy: Released under the terms of the GNU GPL v2.0.
 *
 */
 
#ifndef __IDYNCONT_H__
#define __IDYNCONT_H__
 
#include <iCub/iDyn/iDyn.h>
#include "iCub/skinDynLib/dynContactList.h"
 
 
namespace iCub
{
 
namespace iDyn
{
 
    // tollerance used to solve the linear system (all singular values < TOLLERANCE are considered zero)
    static double const TOLLERANCE = 10e-08;
 
class iDynContactSolver : public iDynSensor
{
protected:        
    iCub::skinDynLib::dynContactList contactList;
    // empty list
    const iCub::skinDynLib::dynContactList nullList;
    // body part related to this solver
    iCub::skinDynLib::BodyPart      bodyPart;
 
    void findContactSubChain(unsigned int &firstLink, unsigned int &lastLink);
    
    yarp::sig::Matrix buildA(unsigned int firstContactLink, unsigned int lastContactLink);
    yarp::sig::Vector buildB(unsigned int firstContactLink, unsigned int lastContactLink);
    
    //***************************************************************************************
    // UTILITY METHODS
    //***************************************************************************************
 
    yarp::sig::Matrix getHFromAtoB(unsigned int a, unsigned int b);
 
    yarp::sig::Vector projectContact2Root(const iCub::skinDynLib::dynContact &c);
 
public:
    
 
    iDynContactSolver(iDynChain *_c, const std::string &_info="", const NewEulMode _mode=DYNAMIC, 
        iCub::skinDynLib::BodyPart bodyPart = iCub::skinDynLib::BODY_PART_UNKNOWN, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
    
    iDynContactSolver(iDynChain *_c, unsigned int sensLink, SensorLinkNewtonEuler *sensor, 
        const std::string &_info="", const NewEulMode _mode=DYNAMIC, iCub::skinDynLib::BodyPart bodyPart = iCub::skinDynLib::BODY_PART_UNKNOWN, 
        unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
 
    iDynContactSolver(iDynChain *_c, unsigned int sensLink, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, 
        double _m, const yarp::sig::Matrix &_I, const std::string &_info="", const NewEulMode _mode=DYNAMIC, 
        iCub::skinDynLib::BodyPart bodyPart = iCub::skinDynLib::BODY_PART_UNKNOWN, unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
 
    ~iDynContactSolver();
 
    bool addContact(const iCub::skinDynLib::dynContact &contact);
 
    bool addContacts(const iCub::skinDynLib::dynContactList &contacts);
 
    void clearContactList();
 
    const iCub::skinDynLib::dynContactList& computeExternalContacts(const yarp::sig::Vector &FMsens);
 
    const iCub::skinDynLib::dynContactList& computeExternalContacts();
 
    void computeWrenchFromSensorNewtonEuler();
 
    //***************************************************************************************
    // GET METHODS
    //***************************************************************************************
 
    const iCub::skinDynLib::dynContactList& getContactList() const;
 
    yarp::sig::Vector getForceMomentEndEff() const;
 
    unsigned int getUnknownNumber() const;
 
};
 
 
}
 
}//end namespace
 
#endif