JointIntegrator.hpp

Go to the documentation of this file.

#ifndef JOINT_INTEGRATOR_HPP
#define JOINT_INTEGRATOR_HPP
 
#include <base/commands/Joints.hpp>
 
enum IntegrationMethod{
    NONE = -1,
    RECTANGULAR = 0,
    TRAPEZOIDAL = 1
};
 
namespace wbc {
 
class JointIntegrator{
    bool initialized;
    base::commands::Joints prev_cmd;
    base::samples::Joints cur_joint_state;
public:
    JointIntegrator() : initialized(false){}
    void integrate(const base::samples::Joints& joint_state, base::commands::Joints &cmd, double cycle_time, IntegrationMethod method = RECTANGULAR, bool use_cur_state = false);
    void integrateRectangular(base::commands::Joints &cmd, double cycle_time, bool use_cur_state = false);
    void integrateTrapezoidal(base::commands::Joints &cmd, double cycle_time, bool use_cur_state = false);
    base::JointState::MODE cmdMode(const base::JointState &cmd);
    void reinit(){initialized = false;}
};
 
}
 
#endif