TaskConnection.h

Go to the documentation of this file.

#ifndef TASK_CONNECTION_H
#define TASK_CONNECTION_H

#include <iostream>
#include <string>
#include <ocra/control/TaskYarpInterfaceVocab.h>
#include <ocra/control/Task.h>

#include <yarp/os/RpcClient.h>
#include <yarp/os/Network.h>
#include <yarp/os/PortReader.h>
#include <yarp/os/Log.h>
#include <yarp/os/LogStream.h>

#include <ocra-recipes/ClientCommunications.h>

#include <Eigen/Dense>
#include <Eigen/Lgsm>
#include <ocra/util/Macros.h>
#include <ocra/util/EigenUtilities.h>
#include <ocra/util/YarpUtilities.h>


namespace ocra_recipes {

class TaskConnection {
DEFINE_CLASS_POINTER_TYPEDEFS(TaskConnection)

public:
    TaskConnection ();
    TaskConnection (const std::string& destinationTaskName);
    virtual ~TaskConnection ();

    void reconnect();
    void disconnect();



private:
    yarp::os::Network yarp;             
    yarp::os::Log yLog;                 
    yarp::os::RpcClient taskRpcClient;  
    std::string taskName;               
    std::string taskRpcServerName;      
    std::string taskRpcClientName;      
    std::string taskOutputPortName;
    std::string taskInputPortName;
    std::string inputPortName;
    std::string outputPortName;

    std::string taskDesiredStateOutputPortName;
    std::string desiredStateInputPortName;

    yarp::os::Port desiredStateInputPort;
    yarp::os::Port inputPort;
    yarp::os::Port outputPort;

    ocra::TaskState currentState;
    ocra::TaskState currentDesiredState;
    bool firstUpdateOfTaskStateHasOccured;
    bool firstUpdateOfTaskDesiredStateHasOccured;
    bool controlPortsAreOpen;

    static int TASK_CONNECTION_COUNT;
    int taskConnectionNumber;

private:
    void parseInput(yarp::os::Bottle& input);
    void parseDesiredStateInput(yarp::os::Bottle& input);


public:

    bool activate();

    bool deactivate();

    std::string getPortName();

    bool isActivated();

    Eigen::VectorXd getTaskError();

    double getTaskErrorNorm();

    void setStiffness(double K);

    void setStiffness(const Eigen::VectorXd& K);


    void setStiffness(const Eigen::MatrixXd& K);

    double getStiffness();

    Eigen::MatrixXd getStiffnessMatrix();

    void setDamping(double B);

    void setDamping(const Eigen::VectorXd& B);


    void setDamping(const Eigen::MatrixXd& B);

    double getDamping();

    Eigen::MatrixXd getDampingMatrix();


    void setWeight(double weight);

    void setWeight(Eigen::VectorXd& weights);

    Eigen::VectorXd getWeight();

    bool openControlPorts(bool connect=true);

    std::string getTaskName(){return taskName;}


    ocra::Task::META_TASK_TYPE getTaskType();
    std::string getTaskTypeAsString();

    int getTaskDimension();

    ocra::TaskState getTaskState();
    ocra::TaskState getDesiredTaskState();
    void setDesiredTaskState(const ocra::TaskState& newDesiredTaskState);
    void setDesiredTaskStateDirect(const ocra::TaskState& newDesiredTaskState);

    bool closeControlPorts();

    std::string getTaskOutputPortName();
    std::string getTaskInputPortName();

    Eigen::Displacementd getTaskFrameDisplacement();
    Eigen::Twistd getTaskFrameVelocity();
    Eigen::Twistd getTaskFrameAcceleration();
    Eigen::Vector3d getTaskFramePosition();
    Eigen::Rotation3d getTaskFrameOrientation();
    Eigen::Vector3d getTaskFrameLinearVelocity();
    Eigen::Vector3d getTaskFrameAngularVelocity();
    Eigen::Vector3d getTaskFrameLinearAcceleration();
    Eigen::Vector3d getTaskFrameAngularAcceleration();

    void queryTask(ocra::TASK_MESSAGE tag, yarp::os::Bottle& bottle);

    /************** controlInputCallback *************/
    class InputCallback : public yarp::os::PortReader {
        private:
            TaskConnection& tcRef;

        public:
            InputCallback(TaskConnection& _tcRef);

            virtual bool read(yarp::os::ConnectionReader& connection);
    };
    /************** controlInputCallback *************/
    /************** desStateInputCallback *************/
    class DesiredStateInputCallback : public yarp::os::PortReader {
        private:
            TaskConnection& tcRef;

        public:
            DesiredStateInputCallback(TaskConnection& _tcRef);

            virtual bool read(yarp::os::ConnectionReader& connection);
    };
    /************** desStateInputCallback *************/

private:
    std::shared_ptr<InputCallback> inpCallback;
    std::shared_ptr<DesiredStateInputCallback> desiredStateInputCallback;



};
} /* ocra_recipes */

#endif // TASK_CONNECTION_H