/build/rock-master-18.09-control-orogen-trajectory-generation-0.20180822/trajectory_generationTypes.hpp

Go to the documentation of this file.

#ifndef trajectory_generation_TYPES_HPP
#define trajectory_generation_TYPES_HPP

#include <base/commands/Joints.hpp>

namespace trajectory_generation {

enum PositionalLimitsBehavior{
    POSITIONAL_LIMITS_IGNORE,          
    POSITIONAL_LIMITS_ERROR_MSG_ONLY,  
    POSITIONAL_LIMITS_ACTIVELY_PREVENT 
};

struct MotionConstraint{

    struct upperLimit{
        double position;     
        double speed;        
        double acceleration; 
    };
    struct lowerLimit{
        double position;     
    };
    double max_jerk;         
    upperLimit max;
    lowerLimit min;

    MotionConstraint(){
        max.position      = base::unset<float>();
        min.position      = base::unset<float>();
        max.speed         = base::unset<float>();
        max.acceleration  = base::unset<float>();
        max_jerk          = base::unset<float>();
    }

    bool hasMaxPosition() const {return !base::isUnset(max.position);}
    bool hasMinPosition() const {return !base::isUnset(min.position);}
    bool hasMaxVelocity() const {return !base::isUnset(max.speed);}
    bool hasMaxAcceleration() const {return !base::isUnset(max.acceleration);}
    bool hasMaxJerk() const {return !base::isUnset(max_jerk);}

    void validate() const{
#ifdef USING_REFLEXXES_TYPE_IV
        if(!hasMaxPosition())
            throw std::invalid_argument("Motion Constraints: Maximum position is invalid");
        if(!hasMinPosition())
            throw std::invalid_argument("Motion Constraints: Minimum position is invalid");
        if(max.position <= min.position)
            throw std::invalid_argument("Motion Constraints: Max. position has to be > min. position");
#endif
        if(!hasMaxVelocity())
            throw std::invalid_argument("Motion Constraints: Maximum speed is invalid");
        if(!hasMaxAcceleration())
            throw std::invalid_argument("Motion Constraints: Maximum acceleration is invalid");
        if(!hasMaxJerk())
            throw std::invalid_argument("Motion Constraints: Maximum jerk is invalid");
        if(max.speed <= 0)
            throw std::invalid_argument("Motion Constraints: Max. speed has to be > 0");
        if(max.acceleration <= 0)
            throw std::invalid_argument("Motion Constraints: Max. acceleration has to be > 0");
        if(max_jerk <= 0)
            throw std::invalid_argument("Motion Constraints: Max. jerk has to be > 0");
    }
    void applyDefaultIfUnset(const MotionConstraint& default_constraints){
        if(!hasMaxPosition())
            max.position = default_constraints.max.position;
        if(!hasMinPosition())
            min.position = default_constraints.min.position;
        if(!hasMaxVelocity())
            max.speed = default_constraints.max.speed;
        if(!hasMaxAcceleration())
            max.acceleration = default_constraints.max.acceleration;
        if(!hasMaxJerk())
            max_jerk = default_constraints.max_jerk;
    }
};

typedef MotionConstraint JointMotionConstraints;

struct MotionConstraints : base::NamedVector<MotionConstraint>{
};

typedef MotionConstraints JointsMotionConstraints;

struct ConstrainedJointsCmd : public base::commands::Joints{
    std::vector<MotionConstraint> motion_constraints;
    void validate() const{
        if(names.size() != size())
            throw std::invalid_argument("Invalid target: Size of name vector should be same as element vector");
        if(!motion_constraints.empty() && motion_constraints.size() != size())
            throw std::invalid_argument("Invalid target: If not empty, motion constraints must have same size as target element vector");
    }
};

enum ReflexxesResultValue{
    RML_WORKING                             =  0,   
    RML_FINAL_STATE_REACHED                 =  1,   
    RML_NO_ERROR                            =  2,   
    RML_ERROR                               = -1,   
    RML_ERROR_INVALID_INPUT_VALUES          = -100, 
    RML_ERROR_EXECUTION_TIME_CALCULATION    = -101, 
    RML_ERROR_SYNCHRONIZATION               = -102, 
    RML_ERROR_NUMBER_OF_DOFS                = -103, 
    RML_ERROR_NO_PHASE_SYNCHRONIZATION      = -104, 
    RML_ERROR_NULL_POINTER                  = -105, 
    RML_ERROR_EXECUTION_TIME_TOO_BIG        = -106, 
    RML_ERROR_USER_TIME_OUT_OF_RANGE        = -107, 
    RML_ERROR_POSITIONAL_LIMITS             = -108, 
    RML_ERROR_OVERRIDE_OUT_OF_RANGE         = -109, 
    RML_NOT_INITIALIZED                     = -200  
};

struct ReflexxesInputParameters{
    ReflexxesInputParameters(){}
    ReflexxesInputParameters(const size_t dof){
        selection_vector.resize(dof, base::NaN<u_int8_t>());
        current_position_vector.resize(dof, base::NaN<double>());
        current_velocity_vector.resize(dof, base::NaN<double>());
        current_acceleration_vector.resize(dof, base::NaN<double>());
        max_position_vector.resize(dof, base::NaN<double>());
        min_position_vector.resize(dof, base::NaN<double>());
        max_velocity_vector.resize(dof, base::NaN<double>());
        max_acceleration_vector.resize(dof, base::NaN<double>());
        max_jerk_vector.resize(dof, base::NaN<double>());
        target_position_vector.resize(dof, base::NaN<double>());
        target_velocity_vector.resize(dof, base::NaN<double>());
        min_synchronization_time = base::NaN<double>();
        override_value = base::NaN<double>();
    }
    std::vector<u_int8_t> selection_vector;
    std::vector<double> current_position_vector;
    std::vector<double> current_velocity_vector;
    std::vector<double> current_acceleration_vector;
    std::vector<double> max_position_vector;
    std::vector<double> min_position_vector;
    std::vector<double> max_velocity_vector;     
    std::vector<double> max_acceleration_vector;
    std::vector<double> max_jerk_vector;
    std::vector<double> target_position_vector;  
    std::vector<double> target_velocity_vector;
    double min_synchronization_time;
    double override_value; 
};

struct ReflexxesOutputParameters{
    ReflexxesOutputParameters(){}
    ReflexxesOutputParameters(const size_t dof){
        new_position_vector.resize(dof, base::NaN<int>());
        new_velocity_vector.resize(dof, base::NaN<double>());
        new_acceleration_vector.resize(dof, base::NaN<double>());
        execution_times.resize(dof, base::NaN<double>());
        position_values_at_target_velocity.resize(dof, base::NaN<double>());
        dof_with_greatest_execution_time = base::NaN<int>();
        synchronization_time = base::NaN<double>();
        current_override_value = base::NaN<double>();
        a_new_calculation_was_performed = false;
        trajectory_is_phase_synchronized = false;
        override_filter_is_active = false;
        trajectory_exceeds_target_position = false;
    }
    std::vector<double> new_position_vector;
    std::vector<double> new_velocity_vector;
    std::vector<double> new_acceleration_vector;
    std::vector<double> execution_times;
    std::vector<double> position_values_at_target_velocity; 
    bool a_new_calculation_was_performed;
    bool trajectory_is_phase_synchronized;
    bool override_filter_is_active;   
    int dof_with_greatest_execution_time;
    double synchronization_time;
    double current_override_value;   
    bool trajectory_exceeds_target_position; 
};

}

#endif