Robot.h

Go to the documentation of this file.

/*
 * Robot.h
 *
 *  Created on: Jun 15, 2011
 *      Author: dgooding
 */

#ifndef ROBOT_H_
#define ROBOT_H_

//#include "RobotModel-types.hpp"

#include <cstdio>
#include <iostream>
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>

#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainjnttojacsolver.hpp>

#include <Eigen/Core>

#include "nasa_common_logging/Logger.h"
#include <stdexcept>

class Robot
{
public:
    struct JointData
    {
        double curPosition;
        double curVelocity;
        double curTorque;
    };

    // struct added here to remove dependency on RobotModel-types.hpp in rtt_r2_controllers_integration
    struct JointTraits
    {
        std::string name;
        std::string type;
        double maxPosition;
        double minPosition;
        double maxVelocity;
        double maxTorque;
        double springConstant;
    };

    typedef std::vector<JointTraits> JointTraitsVector;
    typedef std::map<std::string, JointData*> JointDataMap;
    typedef std::map<std::string, JointTraits*> JointTraitsMap;
    typedef std::map<std::string, KDL::Frame> FrameMap;

    Robot();
    virtual ~Robot();

    KDL::Chain getChain(const std::string& fromSegment, const std::string& toSegment, KDL::Chain& chain);
    KDL::Chain getChainToRoot(const std::string& fromSegment, KDL::Chain& chain);
    KDL::Chain getChainFromRoot(const std::string& toSegment, KDL::Chain& chain);
    static KDL::Chain getChainInReverse(const KDL::Chain& inChain, KDL::Chain& retChain);

    double getMass();
    KDL::Vector getCenterOfMass();

    JointTraits* addJoint(const std::string& jointName);
    JointTraits* getJointTraits(const std::string& segmentName);
    JointTraitsVector getJointTraits();

    JointData* getJointData(const std::string& segmentName);

    void zeroJoints();
    void setJointLimits(const std::string& segmentName, double min, double max);

    void setJointPosition(const std::string& segmentName, double position);
    double getJointPosition(const std::string& segmentName);

    void setJointVelocity(const std::string& segmentName, double velocity);
    double getJointVelocity(const std::string& segmentName);

    void setJointTorque(const std::string& segmentName, double velocity);
    double getJointTorque(const std::string& segmentName);

    KDL::JntArray getChainJointPositions(const KDL::Chain& chain);
    KDL::JntArray getChainJointVelocities(const KDL::Chain& chain);

    KDL::Frame getChainTransformBaseToTip(const std::string& segmentName);
    KDL::Frame getChainTransformTipToBase(const std::string& segmentName);
    KDL::Frame getStaticTransform(const std::string& transformName);
    void setStaticTransform(const std::string& transformName, const KDL::Frame& transform);

//        double getHeightOfRoot(const KDL::Chain &chain);

    KDL::Frame  getChainTransform(const KDL::Chain& chain);

    static KDL::Frame  getChainTransform(const KDL::Chain& chain, const KDL::JntArray& pos);

    static void        getChainJacobian(const KDL::Chain& chain, const KDL::JntArray& positions, KDL::Jacobian& jacobian);

    static void        printChain(const KDL::Chain& chain);
    static std::string segmentToString(const KDL::Segment& segment);
    static std::string vectorToString(const KDL::Vector& vector);
    static std::string rotationToString(const KDL::Rotation& rotation);
    static std::string rotationToRpyString(const KDL::Rotation& rotation);
    static std::string frameToString(const KDL::Frame& frame);
    static std::string rigidBodyInertiaToString(const KDL::RigidBodyInertia& rbi);
    static std::string jacobianToString(KDL::Jacobian& jacobian);

    KDL::Tree robotTree;

protected:
    JointDataMap jointDataMap;
    JointTraitsMap jointTraitsMap;

    FrameMap staticTransforms;
};

#endif /* ROBOT_H_ */