reference/latest/Body_8h_source.html

#ifndef CNOID_BODY_BODY_H

#define CNOID_BODY_BODY_H


#include "LinkTraverse.h"

#include "ExtraJoint.h"

#include "DeviceList.h"

#include "exportdecl.h"


namespace cnoid {


class Body;

class BodyHandler;

class Link;

class Mapping;

class CloneMap;


struct BodyInterface;

struct BodyCustomizerInterface;

typedef void* BodyCustomizerHandle;


typedef ref_ptr<Body> BodyPtr;


class CNOID_EXPORT Body : public ClonableReferenced

{

public:

    Body();

    Body(const std::string& name);

    Body(const Body& org) = delete;

    virtual ~Body();


    // This function can only be used just after the construction of a new instance

    void copyFrom(const Body* org, CloneMap* cloneMap = nullptr);

    Body* clone() const { return static_cast<Body*>(doClone(nullptr)); }

    Body* clone(CloneMap& cloneMap) const { return static_cast<Body*>(doClone(&cloneMap)); }


    virtual Link* createLink(const Link* org = nullptr) const;


    const std::string& name() const;

    void setName(const std::string& name);

    const std::string& modelName() const;

    void setModelName(const std::string& name);


    void setRootLink(Link* link);


    [[deprecated("This func. does nothing")]]

    void expandLinkOffsetRotations() {}


    void updateLinkTree();


    void initializePosition();

    virtual void initializeState();


    Body* parentBody() const { return parentBodyLink_ ? parentBodyLink_->body() : nullptr; }

    Link* parentBodyLink() const { return parentBodyLink_; }

    void setParent(Link* parentBodyLink);

    void resetParent();

    void syncPositionWithParentBody(bool doForwardKinematics = true);


    int numLinks() const {

        return linkTraverse_.numLinks();

    }


    Link* link(int index) const {

        return linkTraverse_.link(index);

    }


    Link* link(const std::string& name) const;


    const LinkTraverse& linkTraverse() const {

        return linkTraverse_;

    }


    const std::vector<Link*>& links() const {

        return linkTraverse_.links();

    }


    Link* rootLink() const {

        return rootLink_;

    }


    Link* findUniqueEndLink() const;

    Link* lastSerialLink() const;


    int numJoints() const {

        return numActualJoints;

    }


    int numVirtualJoints() const {

        return static_cast<int>(jointIdToLinkArray.size()) - numActualJoints;

    }


    int numAllJoints() const {

        return static_cast<int>(jointIdToLinkArray.size());

    }


    Link* joint(int id) const {

        return jointIdToLinkArray[id];

    }


    Link* joint(const std::string& name) const;


    template<class Container> class ContainerWrapper {

    public:

        typedef typename Container::iterator iterator;

        ContainerWrapper(iterator begin, iterator end) : begin_(begin), end_(end) { }

        iterator begin() { return begin_; }

        iterator end() { return end_; }

    private:

        iterator begin_;

        iterator end_;

    };


    ContainerWrapper<std::vector<LinkPtr>> joints() {

        return ContainerWrapper<std::vector<LinkPtr>>(

            jointIdToLinkArray.begin(), jointIdToLinkArray.begin() + numActualJoints);

    }


    const std::vector<LinkPtr>& allJoints() const { return jointIdToLinkArray; }


    int numDevices() const {

        return static_cast<int>(devices_.size());

    }


    Device* device(int index) const { return devices_[index]; }


    const DeviceList<>& devices() const {

        return devices_;

    }


    template<class DeviceType> DeviceList<DeviceType> devices() const {

        return devices_;

    }


    template<class DeviceType> DeviceType* findDevice(const std::string& name) const {

        return dynamic_cast<DeviceType*>(findDeviceSub(name));

    }


    template<class DeviceType> DeviceType* findDevice() const {

        for(auto& device : devices_)

            if(auto found = dynamic_cast<DeviceType*>(device.get()))

                return found;

        return nullptr;

    }


    Device* findDevice(const std::string& name) const {

        return findDeviceSub(name);

    }


    void addDevice(Device* device, Link* link);

    [[deprecated("Use addDevice(Device* device, Link* link)")]]

    void addDevice(Device* device);

    void removeDevice(Device* device);

    void clearDevices();

    void sortDevicesByLinkOrder();

    void initializeDeviceStates();


    bool isStaticModel() const {

        return isStaticModel_;

    }

    bool isFixedRootModel() const {

        return rootLink_->isFixedJoint();

    }


    void resetDefaultPosition(const Isometry3& T);

    const Isometry3& defaultPosition() const { return rootLink_->Tb(); }


    double mass() const;


    const Vector3& calcCenterOfMass();

    const Vector3& centerOfMass() const;


    void calcTotalMomentum(Vector3& out_P, Vector3& out_L);


    void calcForwardKinematics(bool calcVelocity = false, bool calcAcceleration = false) {

        linkTraverse_.calcForwardKinematics(calcVelocity, calcAcceleration);

    }


    void clearExternalForces();


    int numExtraJoints() const { return static_cast<int>(extraJoints_.size()); }

    ExtraJoint& extraJoint(int index) { return extraJoints_[index]; }

    const ExtraJoint& extraJoint(int index) const {  return extraJoints_[index]; }

    void addExtraJoint(const ExtraJoint& extraJoint) { extraJoints_.push_back(extraJoint); }

    void clearExtraJoints() { extraJoints_.clear(); }


    const Mapping* info() const;

    Mapping* info();


    template<typename T> T info(const std::string& key) const;

    template<typename T> T info(const std::string& key, const T& defaultValue) const;

    template<typename T> void setInfo(const std::string& key, const T& value);


    void resetInfo(Mapping* info);


    void cloneShapes(CloneMap& cloneMap);


    template<class T> T* findCache(const std::string& name){

        return dynamic_cast<T*>(findCacheSub(name));

    }


    template<class T> const T* findCache(const std::string& name) const {

        return dynamic_cast<const T*>(findCacheSub(name));

    }


    template<class T> T* getOrCreateCache(const std::string& name){

        T* cache = findCache<T>(name);

        if(!cache){

            cache = new T();

            insertCache(name, cache);

        }

        return cache;

    }


    void setCache(const std::string& name, Referenced* cache){

        insertCache(name, cache);

    }


    bool getCaches(PolymorphicReferencedArrayBase<>& out_caches, std::vector<std::string>& out_names) const;


    void removeCache(const std::string& name);


    void setCurrentTimeFunction(std::function<double()> func);

    double currentTime() const { return currentTimeFunction(); }


    bool addHandler(BodyHandler* handler, bool isTopPriority = false);


    template<class BodyHandlerType> BodyHandlerType* findHandler(){

        return dynamic_cast<BodyHandlerType*>(

            findHandler([](BodyHandler* handler)->bool{ return dynamic_cast<BodyHandlerType*>(handler); }));

    }


    int numHandlers() const;

    BodyHandler* handler(int index);


    // The following functions for the body customizer are deprecated

    BodyCustomizerHandle customizerHandle() const;

    BodyCustomizerInterface* customizerInterface() const;

    bool installCustomizer();

    bool installCustomizer(BodyCustomizerInterface* customizerInterface);

    bool hasVirtualJointForces() const;

    void setVirtualJointForces(double timeStep = 0.0);

    static void addCustomizerDirectory(const std::string& path);

    static BodyInterface* bodyInterface();


    void resetLinkName(Link* link, const std::string& name);

    void resetJointSpecificName(Link* link);

    void resetJointSpecificName(Link* link, const std::string& name);


protected:

    Body(Link* rootLink);

    virtual Referenced* doClone(CloneMap* cloneMap) const override;


private:

    LinkTraverse linkTraverse_;

    LinkPtr rootLink_;

    LinkPtr parentBodyLink_;

    bool isStaticModel_;

    std::vector<LinkPtr> jointIdToLinkArray;

    int numActualJoints;

    DeviceList<> devices_;

    std::vector<ExtraJoint> extraJoints_;

    std::function<double()> currentTimeFunction;


    class Impl;

    Impl* impl;


    Link* cloneLinkTree(const Link* orgLink, CloneMap* cloneMap);

    Link* createEmptyJoint(int jointId);

    Device* findDeviceSub(const std::string& name) const;

    Referenced* findCacheSub(const std::string& name);

    const Referenced* findCacheSub(const std::string& name) const;

    void insertCache(const std::string& name, Referenced* cache);

    BodyHandler* findHandler(std::function<bool(BodyHandler*)> isTargetHandlerType);

};


template<> CNOID_EXPORT double Body::info(const std::string& key) const;

template<> CNOID_EXPORT double Body::info(const std::string& key, const double& defaultValue) const;

template<> CNOID_EXPORT bool Body::info(const std::string& key, const bool& defaultValue) const;

template<> CNOID_EXPORT void Body::setInfo(const std::string& key, const double& value);

template<> CNOID_EXPORT void Body::setInfo(const std::string& key, const bool& value);


}


#endif