ocra-wbi-plugins/ocra-icub-clients/walking-client/include/walking-client/StepController.h

Go to the documentation of this file.

#ifndef _STEPCONTROLLER_H_
#define _STEPCONTROLLER_H_

#include <ocra-recipes/TrajectoryThread.h>
#include "walking-client/utils.h"

class StepController {
public:

    StepController(int periodms, ocra::Model::Ptr model);
    virtual ~StepController();

    bool initialize();

    void deactivateFeetContacts(FOOT foot);

    void activateFeetContacts(FOOT foot);

    
    bool doStepWithMaxVelocity(FOOT foot, Eigen::Vector3d target, double stepHeight);

    void step(FOOT foot, Eigen::Vector3d target, double stepDuration, double stepHeight);

    bool isStepFinished(FOOT foot);
    
    double getFootTrajError(FOOT foot, double &error);
    
    Eigen::Vector3d getLeftFootPosition();

    Eigen::Vector3d getRightFootPosition();

    void computeMidPoint(FOOT foot, Eigen::Vector3d target, double stepHeight, Eigen::Vector3d & midPoint);


    void doComputeMidPoint(Eigen::Vector3d currentFootPosition, Eigen::Vector3d target, double stepHeight, Eigen::Vector3d & midPoint);

    bool isTrajectoryFinished(FOOT foot);

    void stop();


    Eigen::MatrixXd getContact2DCoordinates();


private:
    std::vector<ocra_recipes::TaskConnection::Ptr> _contactsCollection;
    ocra_recipes::TaskConnection::Ptr _LeftFootContact_BackLeft;
    ocra_recipes::TaskConnection::Ptr _LeftFootContact_FrontLeft;
    ocra_recipes::TaskConnection::Ptr _LeftFootContact_BackRight;
    ocra_recipes::TaskConnection::Ptr _LeftFootContact_FrontRight;
    ocra_recipes::TaskConnection::Ptr _RightFootContact_BackLeft;
    ocra_recipes::TaskConnection::Ptr _RightFootContact_FrontLeft;
    ocra_recipes::TaskConnection::Ptr _RightFootContact_BackRight;
    ocra_recipes::TaskConnection::Ptr _RightFootContact_FrontRight;
    std::shared_ptr<ocra_recipes::TrajectoryThread> _leftFoot_TrajThread;
    std::shared_ptr<ocra_recipes::TrajectoryThread> _rightFoot_TrajThread;
    Eigen::Vector3d target;
    Eigen::Vector3d _leftFootPosition;
    Eigen::Vector3d _rightFootPosition;
    ocra::Model::Ptr _model;
    int _period;

};
#endif