CartesianForceController.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */
 
#ifndef CTRL_LIB_CARTESIANFORCECONTROLLER_TASK_HPP
#define CTRL_LIB_CARTESIANFORCECONTROLLER_TASK_HPP
 
#include "ctrl_lib/CartesianForceControllerBase.hpp"
#include <base/samples/Wrenches.hpp>
#include <base/samples/RigidBodyState.hpp>
#include <base/samples/RigidBodyStateSE3.hpp>
#include <wbc/controllers/CartesianForcePIDController.hpp>
 
namespace ctrl_lib{
 
class CartesianForcePIDController;
 
class CartesianForceController : public CartesianForceControllerBase
{
    friend class CartesianForceControllerBase;
 
public:
    CartesianForceController(std::string const& name = "ctrl_lib::CartesianForceController");
    CartesianForceController(std::string const& name, RTT::ExecutionEngine* engine);
    ~CartesianForceController(){}
    bool configureHook();
    bool startHook();
    void updateHook();
    void errorHook();
    void stopHook();
    void cleanupHook();
 
protected:
    virtual bool readFeedback();
    virtual bool readSetpoint();
    virtual void updateController();
    virtual const base::VectorXd& computeActivation(wbc::ActivationFunction& activation_function);
 
 
    bool isValid(const base::Wrench &w){
        return !base::isNaN(w.force(0)) && !base::isNaN(w.force(1)) && !base::isNaN(w.force(2)) &&
               !base::isNaN(w.torque(0)) && !base::isNaN(w.torque(1)) && !base::isNaN(w.torque(2));
    }
 
    const base::VectorXd wrenchToRaw(const base::samples::Wrench& wrench, base::VectorXd& raw){
        raw.resize(6);
        raw.segment(0,3) = wrench.force;
        raw.segment(3,3) = wrench.torque;
        return raw;
    }
 
    base::samples::Wrench setpoint, feedback;
    base::samples::Wrenches feedback_wrenches;
    std::string ft_sensor_name;
    base::VectorXd setpoint_raw, feedback_raw, ctrl_output_raw;
    base::samples::RigidBodyStateSE3 control_output;
    wbc::CartesianForcePIDController* controller;
 
};
}
 
#endif