data.hpp

Go to the documentation of this file.

 
#ifndef FLEXIV_RDK_DATA_HPP_
#define FLEXIV_RDK_DATA_HPP_
 
#include <array>
#include <vector>
#include <string>
#include <ostream>
#include <variant>
#include <chrono>
#include <map>
 
namespace flexiv::rdk {
constexpr size_t kCartDoF = 6;
 
constexpr size_t kSerialJointDoF = 7;
 
constexpr size_t kPoseSize = 7;
 
constexpr size_t kIOPorts = 16 + 2 * 2;
 
constexpr size_t kMaxExtAxes = 6;
 
enum class ProductModel
{
    UNKNOWN = 0,
    Enlight_L,  
    Enlight_LL, 
    MICO_Core,  
    MICO_Plus,  
    MICO_Ultra, 
};
 
inline const std::map<ProductModel, std::string> kProductModelNames
    = {{ProductModel::UNKNOWN, "UNKNOWN"}, {ProductModel::Enlight_L, "Enlight-L"},
        {ProductModel::Enlight_LL, "Enlight-LL"}, {ProductModel::MICO_Core, "MICO-Core"},
        {ProductModel::MICO_Plus, "MICO-Plus"}, {ProductModel::MICO_Ultra, "MICO-Ultra"}};
 
enum class JointGroup
{
    UNKNOWN = -1, 
    ALL = 0,      
    ARMS = 2,     
    ARM_1 = 3,    
    ARM_2 = 4,    
    EXT_AXIS = 5, 
 
    FIRST = ALL,
    LAST = EXT_AXIS,
};
 
inline const std::map<JointGroup, std::string> kJointGroupNames {
    {JointGroup::UNKNOWN, "UNKNOWN"},
    {JointGroup::ALL, "ALL"},
    {JointGroup::ARMS, "ARMS"},
    {JointGroup::ARM_1, "ARM_1"},
    {JointGroup::ARM_2, "ARM_2"},
    {JointGroup::EXT_AXIS, "EXT_AXIS"},
};
 
enum class OperationalStatus
{
    UNKNOWN = 0,        
    READY,              
    BOOTING,            
    ESTOP_NOT_RELEASED, 
    NOT_SERVO_ON,       
    RELEASING_BRAKE,    
    MINOR_FAULT,        
    CRITICAL_FAULT,     
    IN_REDUCED_STATE,   
    IN_RECOVERY_STATE,  
    IN_MANUAL_MODE,     
    IN_AUTO_MODE,       
};
 
inline const std::map<OperationalStatus, std::string> kOpStatusNames {
    {OperationalStatus::UNKNOWN, "Unknown status"},
    {OperationalStatus::READY, "Ready"},
    {OperationalStatus::BOOTING, "System booting"},
    {OperationalStatus::ESTOP_NOT_RELEASED, "E-Stop not released"},
    {OperationalStatus::NOT_SERVO_ON, "Not servo on"},
    {OperationalStatus::RELEASING_BRAKE, "Releasing brakes"},
    {OperationalStatus::MINOR_FAULT, "Minor fault occurred"},
    {OperationalStatus::CRITICAL_FAULT, "Critical fault occurred"},
    {OperationalStatus::IN_REDUCED_STATE, "In reduced state"},
    {OperationalStatus::IN_RECOVERY_STATE, "In recovery state"},
    {OperationalStatus::IN_MANUAL_MODE, "In Manual mode"},
    {OperationalStatus::IN_AUTO_MODE, "In regular Auto mode"},
};
 
enum class CoordType
{
    WORLD, 
    TCP,   
};
 
struct RobotEvent
{
    enum Level
    {
        UNKNOWN = 0, 
        INFO,        
        WARNING,     
        ERROR,       
        CRITICAL,    
    };
 
    Level level = UNKNOWN;
 
    int id = 0;
 
    std::string description = "";
 
    std::string consequences = "";
 
    std::string probable_causes = "";
 
    std::string recommended_actions = "";
 
    std::chrono::time_point<std::chrono::system_clock> timestamp;
};
 
struct RobotInfo
{
    ProductModel product_model = {};
 
    std::string serial_num = {};
 
    std::string software_ver = {};
 
    std::string license_type = {};
 
    std::map<JointGroup, std::string> all_groups = {};
 
    std::map<JointGroup, std::string> single_arm_groups = {};
 
    std::map<JointGroup, size_t> DoF = {};
 
    std::map<JointGroup, std::array<double, kCartDoF>> K_x_nom = {};
 
    std::map<JointGroup, std::vector<double>> K_q_nom = {};
 
    std::map<JointGroup, std::vector<double>> q_min = {};
 
    std::map<JointGroup, std::vector<double>> q_max = {};
 
    std::map<JointGroup, std::vector<double>> dq_max = {};
 
    std::map<JointGroup, std::vector<double>> tau_max = {};
};
 
struct RobotStates
{
    std::pair<int, int> timestamp = {};
 
    std::vector<double> q = {};
 
    std::vector<double> theta = {};
 
    std::vector<double> dq = {};
 
    std::vector<double> dtheta = {};
 
    std::vector<double> tau = {};
 
    std::vector<double> tau_dot = {};
 
    std::vector<double> tau_ext = {};
 
    std::vector<double> tau_interact = {};
 
    std::vector<double> temperature = {};
 
    std::array<double, kPoseSize> flange_pose = {};
 
    std::array<double, kPoseSize> tcp_pose = {};
 
    std::array<double, kCartDoF> tcp_twist = {};
 
    std::array<double, kCartDoF> tcp_wrench = {};
 
    std::array<double, kCartDoF> tcp_wrench_local = {};
 
    std::array<double, kCartDoF> raw_tcp_wrench = {};
 
    std::array<double, kCartDoF> raw_tcp_wrench_local = {};
 
    std::array<double, kCartDoF> raw_ft_sensor = {};
};
 
struct RobotActions
{
    std::pair<int, int> timestamp = {};
 
    std::vector<double> q_d = {};
 
    std::vector<double> dq_d = {};
 
    std::vector<double> tau_d = {};
 
    std::array<double, kPoseSize> tcp_pose_d = {};
 
    std::array<double, kCartDoF> tcp_twist_d = {};
 
    std::array<double, kCartDoF> tcp_wrench_d = {};
};
 
struct PlanInfo
{
    std::string pt_name = {};
 
    std::string node_name = {};
 
    std::string node_path = {};
 
    std::string node_path_time_period = {};
 
    std::string node_path_number = {};
 
    std::string assigned_plan_name = {};
 
    double velocity_scale = {};
 
    bool waiting_for_step = {};
};
 
struct JPos
{
    JPos() = default;
 
    JPos(const std::array<double, kSerialJointDoF>& q_m,
        const std::array<double, kMaxExtAxes>& q_e = {})
    : q_m(q_m)
    , q_e(q_e)
    {
    }
 
    std::array<double, kSerialJointDoF> q_m = {};
 
    std::array<double, kMaxExtAxes> q_e = {};
 
    std::string str() const;
};
 
struct Coord
{
    Coord() = default;
 
    Coord(const std::array<double, kCartDoF / 2>& position,
        const std::array<double, kCartDoF / 2>& orientation,
        const std::array<std::string, 2>& ref_frame,
        const std::array<double, kSerialJointDoF>& ref_q_m = {},
        const std::array<double, kMaxExtAxes>& ref_q_e = {})
    : position(position)
    , orientation(orientation)
    , ref_frame(ref_frame)
    , ref_q_m(ref_q_m)
    , ref_q_e(ref_q_e)
    {
    }
 
    std::array<double, kCartDoF / 2> position = {};
 
    std::array<double, kCartDoF / 2> orientation = {};
 
    std::array<std::string, 2> ref_frame = {};
 
    std::array<double, kSerialJointDoF> ref_q_m = {};
 
    std::array<double, kMaxExtAxes> ref_q_e = {};
 
    std::string str() const;
};
 
enum class SyncMotionMode
{
    DISABLE = 0,    
    ARM1_TCP = 1,   
    ARM2_TCP = 2,   
    POSITIONER = 3, 
};
 
using FlexivDataTypes = std::variant<int, double, std::string, rdk::JPos, rdk::Coord,
    std::vector<int>, std::vector<double>, std::vector<std::string>, std::vector<rdk::JPos>,
    std::vector<rdk::Coord>>;
 
struct PrimitiveArgs
{
    PrimitiveArgs() = default;
 
    PrimitiveArgs(
        const std::string& pt_name, const std::map<std::string, FlexivDataTypes>& input_params)
    : pt_name(pt_name)
    , input_params(input_params)
    {
    }
 
    std::string pt_name = {};
 
    std::map<std::string, FlexivDataTypes> input_params = {};
 
    bool external_axis_control = true;
 
    SyncMotionMode sync_motion_mode = SyncMotionMode::DISABLE;
};
 
struct PrimitiveStates
{
    std::string pt_name = {};
 
    std::map<std::string, FlexivDataTypes> names_and_values = {};
};
 
struct RtJointTorqueCmd
{
    RtJointTorqueCmd() = default;
 
    RtJointTorqueCmd(const std::vector<double>& tau_d, bool enable_gravity_comp = true,
        bool enable_soft_limits = true, double friction_comp_scale = 100.0)
    : tau_d(tau_d)
    , enable_gravity_comp(enable_gravity_comp)
    , enable_soft_limits(enable_soft_limits)
    , friction_comp_scale(friction_comp_scale)
    {
    }
 
    std::vector<double> tau_d = {};
 
    bool enable_gravity_comp = true;
 
    bool enable_soft_limits = true;
 
    double friction_comp_scale = 100.0;
};
 
struct RtJointPositionCmd
{
    RtJointPositionCmd() = default;
 
    RtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
        const std::vector<double>& ddq_d)
    : q_d(q_d)
    , dq_d(dq_d)
    , ddq_d(ddq_d)
    {
    }
 
    std::vector<double> q_d = {};
 
    std::vector<double> dq_d = {};
 
    std::vector<double> ddq_d = {};
};
 
struct NrtJointPositionCmd
{
    NrtJointPositionCmd() = default;
 
    NrtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
        const std::vector<double>& dq_max, const std::vector<double>& ddq_max)
    : q_d(q_d)
    , dq_d(dq_d)
    , dq_max(dq_max)
    , ddq_max(ddq_max)
    {
    }
 
    std::vector<double> q_d = {};
 
    std::vector<double> dq_d = {};
 
    std::vector<double> dq_max = {};
 
    std::vector<double> ddq_max = {};
};
 
struct RtCartesianCmd
{
    RtCartesianCmd() = default;
 
    RtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
        const std::array<double, kCartDoF>& wrench_d = {},
        const std::array<double, kCartDoF>& twist_d = {},
        const std::array<double, kCartDoF>& acc_d = {})
    : pose_d(pose_d)
    , wrench_d(wrench_d)
    , twist_d(twist_d)
    , acc_d(acc_d)
    {
    }
 
    std::array<double, kPoseSize> pose_d = {};
 
    std::array<double, kCartDoF> wrench_d = {};
 
    std::array<double, kCartDoF> twist_d = {};
 
    std::array<double, kCartDoF> acc_d = {};
};
 
struct NrtCartesianCmd
{
    NrtCartesianCmd() = default;
 
    NrtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
        const std::array<double, kCartDoF>& wrench_d = {},
        const std::array<double, kCartDoF>& twist_d = {}, double max_linear_vel = 0.5,
        double max_angular_vel = 1.0, double max_linear_acc = 2.0, double max_angular_acc = 5.0)
    : pose_d(pose_d)
    , wrench_d(wrench_d)
    , twist_d(twist_d)
    , max_linear_vel(max_linear_vel)
    , max_angular_vel(max_angular_vel)
    , max_linear_acc(max_linear_acc)
    , max_angular_acc(max_angular_acc)
    {
    }
 
    std::array<double, kPoseSize> pose_d = {};
 
    std::array<double, kCartDoF> wrench_d = {};
 
    std::array<double, kCartDoF> twist_d = {};
 
    double max_linear_vel = 0.5;
 
    double max_angular_vel = 1.0;
 
    double max_linear_acc = 2.0;
 
    double max_angular_acc = 5.0;
};
 
struct NrtCartesianMultiWaypointCmd
{
    NrtCartesianMultiWaypointCmd() = default;
 
    NrtCartesianMultiWaypointCmd(const std::vector<NrtCartesianCmd>& waypoints)
    : waypoints(waypoints)
    {
    }
 
    std::vector<NrtCartesianCmd> waypoints = {};
};
 
std::ostream& operator<<(std::ostream& ostream, const RobotEvent& robot_event);
 
std::ostream& operator<<(std::ostream& ostream, const RobotInfo& robot_info);
 
std::ostream& operator<<(std::ostream& ostream, const RobotStates& robot_states);
 
std::ostream& operator<<(std::ostream& ostream, const RobotActions& robot_actions);
 
std::ostream& operator<<(std::ostream& ostream, const PlanInfo& plan_info);
 
} /* namespace flexiv::rdk */
 
#endif /* FLEXIV_RDK_DATA_HPP_ */