robot.hh

 
#pragma once
 
#include <memory>
#include <cstdint>
#include <string>
#include <vector>
#include <array>
#include <map>
 
namespace lebai {
namespace l_master {
std::string version();
using CartesianPose = std::map<std::string, double>;
using DoubleVector = std::vector<double>;
struct KinematicsForwardResp {
  CartesianPose
      pose;        
  bool ok = false; 
};
struct KinematicsInverseResp {
  DoubleVector
      joint_positions; 
  bool ok = false;     
};
 
struct ClawData {
  double force;     
  double amplitude; 
  bool hold_on;     
};
 
struct StorageItem {
  std::string key;   
  std::string value; 
};
 
struct PhysicalData {
  DoubleVector joint_temperature; 
  DoubleVector joint_voltage;     
  double flange_voltage;          
};
 
struct DhParamData {
  double a = 0.0;     
  double alpha = 0.0; 
  double d = 0.0;     
  double theta = 0.0; 
};
 
struct SystemInfoData {
  std::string name;            
  std::string kernel_version;  
  std::string os_version;      
  std::string host_name;       
  uint64_t used_memory = 0;    
  uint64_t total_memory = 0;   
};
 
struct RobotInfoData {
  std::string name;      
  std::string mac;       
  std::string box_model; 
  std::string box_sn;    
  std::string arm_model; 
  std::string arm_sn;    
};
 
struct DiscoveredRobotData {
  std::string name; 
  std::string mac;  
  std::string ip;   
  bool online = false; 
};
 
struct DeviceInfoData {
  bool invalid = false;      
  std::string sn;            
  std::string version;       
  std::string partition;     
  uint32_t di_num = 0;       
  uint32_t do_num = 0;       
  uint32_t dio_num = 0;      
  uint32_t ai_num = 0;       
  uint32_t ao_num = 0;       
};
 
struct HardwareInfoData {
  DeviceInfoData comboard;             
  std::vector<DeviceInfoData> joints;  
  DeviceInfoData flange;               
  DeviceInfoData led;                  
  DeviceInfoData extra_io;             
};
 
struct SoftwareItemInfoData {
  std::string version; 
  std::string branch;  
};
 
struct SoftwareInfoData {
  std::map<std::string, SoftwareItemInfoData> software; 
};
 
struct BackupOptionsData {
  bool tmp = false;
  bool syslog = false;
  bool arm = false;
  bool config = false;
  bool data = false;
  bool file = false;
  bool docker = false;
  bool ds = false;
};
 
struct BackupInfoData {
  SystemInfoData system;
  RobotInfoData robot;
  HardwareInfoData hardware;
  SoftwareInfoData software;
  BackupOptionsData option;
};
 
struct HttpRequestData {
  std::string method;                         
  std::string url;                            
  std::map<std::string, std::string> headers; 
  std::string body;                           
};
 
struct HttpResponseData {
  unsigned int status = 0;                    
  std::map<std::string, std::string> headers; 
  std::string body;                           
};
 
struct MessageData {
  std::string level;  
  std::string kind;   
  std::string detail; 
  std::string time;   
};
 
struct OtaStateData {
  std::string step;     
  uint32_t progress = 0; 
};
 
struct CheckUpgradeData {
  bool need_upgrade = false;  
  std::string introduction;   
};
 
struct CommandStdoutData {
  bool done = false;        
  std::string stdout_text;  
  std::string stderr_text;  
  int code = 0;             
};
 
struct DirData {
  std::string name; 
  uint32_t id = 0;  
};
 
struct ShortcutData {
  uint32_t id = 0;  
  std::string dir;  
  std::string name; 
};
 
struct ButtonIndexData {
  std::string device; 
  uint32_t pin = 0;   
};
 
struct ButtonStatusData {
  std::string state; 
  uint32_t time = 0; 
};
 
struct TriggerConditionData {
  std::vector<ButtonIndexData> pressed; 
  ButtonIndexData button;               
  ButtonStatusData status;              
};
 
struct TriggerData {
  TriggerConditionData condition; 
  std::string function;           
};
 
struct LedStyleData {
  std::string mode;                
  std::string speed;               
  std::vector<std::string> colors; 
  std::string voice;               
  std::string volume;              
};
 
struct ServoParamData {
  double position_kp = 0.0;       
  double speed_kp = 0.0;          
  double speed_it = 0.0;          
  double torque_cmd_filter = 0.0; 
};
 
struct ExtraServoParamData {
  double acc_position_kp = 0.0;
  double acc_speed_kp = 0.0;
  double acc_speed_it = 0.0;
  double uni_position_kp = 0.0;
  double uni_speed_kp = 0.0;
  double uni_speed_it = 0.0;
  double dec_position_kp = 0.0;
  double dec_speed_kp = 0.0;
  double dec_speed_it = 0.0;
};
 
struct WrenchData {
  DoubleVector force;  
  DoubleVector torque; 
};
 
struct PluginInfoData {
  std::string name;        
  std::string description; 
  std::string homepage;    
  bool auto_restart = false; 
  bool web = false;          
  bool daemon = false;       
  bool cmd = false;          
  bool enable = false;       
};
 
struct PluginStoreInfoData {
  std::string name; 
  std::string url;  
};
 
struct TaskData {
  unsigned int id = 0;         
  std::string state;           
  unsigned int loop_count = 0; 
  unsigned int loop_to = 0;    
  bool is_parallel = false;    
  bool is_simu = false;        
  std::string stdout_text;     
  std::string kind;            
  std::string dir;             
  std::string name;            
  std::vector<std::string> params; 
};
 
struct TaskStdoutData {
  unsigned int id = 0;     
  bool done = false;       
  std::string stdout_text; 
};
 
struct ModbusData {
  std::string kind;    
  std::string address; 
  unsigned int port = 0;     
  unsigned int slave_id = 0; 
};
 
struct ModbusRegisterData {
  std::string kind;        
  unsigned int address = 0; 
};
 
struct StructureData {
  bool active = false;      
  std::string name;         
  std::string kind;         
  unsigned int dof = 0;     
  std::string dh;           
  std::string dyn;          
  std::string servo;        
};
 
struct PoseData {
  std::string kind;                 
  CartesianPose cart;               
  std::vector<double> joint;        
};
 
struct JointMoveData {
  double pose = 0.0;     
  double velocity = 0.0; 
  double acc = 0.0;      
};
 
struct PvatPointData {
  double duration = 0.0;            
  std::vector<JointMoveData> joints; 
};
 
struct TrajectoryData {
  std::string kind;                
  std::vector<PvatPointData> data; 
};
 
struct FrameData {
  std::string position_kind; 
  CartesianPose position;    
  std::string rotation_kind; 
  CartesianPose rotation;    
};
 
struct JointMotionData {
  DoubleVector actual_joint_pose;   
  DoubleVector actual_joint_speed;  
  DoubleVector actual_joint_acc;    
  DoubleVector actual_joint_torque; 
  DoubleVector target_joint_pose;   
  DoubleVector target_joint_speed;  
  DoubleVector target_joint_acc;    
  DoubleVector target_joint_torque; 
  CartesianPose actual_tcp_pose;    
  CartesianPose target_tcp_pose;    
  CartesianPose actual_flange_pose; 
};
 
struct CollisionDetectorConfig {
  unsigned int action = 99; 
  unsigned int pause_time = 0;  
  unsigned int sensitivity = 0; 
};
 
struct JointLimitConfig {
  double min_position = 0.0; 
  double max_position = 0.0; 
  double max_a = 0.0;        
  double max_v = 0.0;        
};
 
struct CartesianLimitConfig {
  double max_a = 0.0;     
  double max_v = 0.0;     
  double eq_radius = 0.0; 
};
 
class Robot {
 public:
  class RobotImpl;
 
  explicit Robot(std::string ip, bool simulator = false);
  virtual ~Robot();
 
  // clang-format off
  // clang-format on
  std::tuple<int, std::string> call(const std::string &method,
                                    const std::string &params);
 
  std::string hello(const std::string &data);
 
  void set_auto(int name, bool value);
 
  bool get_auto(int name);
 
  bool is_network_connected();
 
  void start_sys();
  void stop_sys();
  void powerdown();
  void stop();
  void estop();
  void start_teach_mode();
  void teach_mode();
  void end_teach_mode();
  void pause_move();
  void pause();
  void resume_move();
  void resume();
  void reboot();
  int move_joint(const std::vector<double> &joint_positions, double a, double v,
                 double t, double r);
  int movej(const std::vector<double> &joint_positions, double a, double v,
            double t, double r);
  int move_joint(const CartesianPose &cart_pose, double a, double v, double t,
                 double r);
  int movej(const CartesianPose &cart_pose, double a, double v, double t,
            double r);
  int move_linear(const std::vector<double> &joint_positions, double a,
                  double v, double t, double r);
  int movel(const std::vector<double> &joint_positions, double a, double v,
            double t, double r);
  int move_linear(const CartesianPose &cart_pose, double a, double v, double t,
                  double r);
  int movel(const CartesianPose &cart_pose, double a, double v, double t,
            double r);
  int move_circular(const std::vector<double> &joint_via,
                    const std::vector<double> &joint, double rad, double a,
                    double v, double t, double r);
  int movec(const std::vector<double> &joint_via,
            const std::vector<double> &joint, double rad, double a, double v,
            double t, double r);
  int move_circular(const CartesianPose &cart_via, const CartesianPose &cart,
                    double rad, double a, double v, double t, double r);
  int movec(const CartesianPose &cart_via, const CartesianPose &cart,
            double rad, double a, double v, double t, double r);
  int speed_joint(double a, const std::vector<double> &v, double t = 0.0);
  int speedj(double a, const std::vector<double> &v, double t = 0.0);
  int speed_linear(double a, const CartesianPose &v, double t = 0.0,
                   const CartesianPose &reference = {{"x", 0.0},
                                                     {"y", 0.0},
                                                     {"z", 0.0},
                                                     {"rx", 0.0},
                                                     {"ry", 0.0},
                                                     {"rz", 0.0}});
  int speedl(double a, const CartesianPose &v, double t = 0.0,
             const CartesianPose &reference = {{"x", 0.0},
                                               {"y", 0.0},
                                               {"z", 0.0},
                                               {"rx", 0.0},
                                               {"ry", 0.0},
                                               {"rz", 0.0}});
  int toward_joint(const std::vector<double> &joint_positions, double a,
                   double v, double t, double r);
  int towardj(const std::vector<double> &joint_positions, double a, double v,
              double t, double r);
  void move_pvat(std::vector<double> p, std::vector<double> v,
                 std::vector<double> a, double t);
  void wait_move(unsigned int id);
  void wait_move();
  unsigned int get_running_motion();
  std::string get_motion_state(unsigned int id);
  void stop_move();
  void skip_move();
  int get_robot_state();
  int get_estop_reason();
  SystemInfoData get_system_info();
  RobotInfoData get_robot_info();
  HardwareInfoData get_hardware_info();
  SoftwareInfoData get_software_info();
  HttpResponseData http(HttpRequestData request);
  void clean(const BackupOptionsData &option);
  void backup(const std::string &file, const BackupOptionsData &option);
  BackupInfoData get_backup_info(const std::string &file);
  void restore(const std::string &file, const BackupOptionsData &option);
  void set_virtual_ip(const std::string &ifname, const std::string &ip);
  std::vector<std::string> get_box_devices(const std::string &prefix);
  std::vector<DirData> get_dirs();
  void create_dir(DirData dir);
  void update_dir(std::string from, std::string to);
  std::vector<ShortcutData> get_short_poses();
  void set_short_pose(ShortcutData shortcut_data);
  ShortcutData get_short_pose(unsigned int id);
  std::vector<ShortcutData> get_short_tasks();
  void set_short_task(ShortcutData shortcut_data);
  ShortcutData get_short_task(unsigned int id);
  std::vector<TriggerData> get_triggers();
  void set_trigger(TriggerData trigger);
  std::map<std::string, LedStyleData> get_led_styles();
  void set_led_styles(std::map<std::string, LedStyleData> styles);
  LedStyleData load_led_style(std::string name, std::string dir = "");
  void save_led_style(std::string name, LedStyleData style,
                      std::string dir = "");
  void set_led_style(std::string state, LedStyleData style);
  std::vector<std::string> load_led_style_list(std::string dir = "");
  std::vector<ServoParamData> get_servo_params();
  void set_servo_params(const std::vector<ServoParamData> &params);
  void find_zero();
  void set_zero(const std::vector<double> &pose,
                const std::vector<bool> &valids);
  void set_extra_servo_params(const std::vector<ExtraServoParamData> &params,
                              const std::vector<bool> &valids);
  void reset_extra_servo_params(const std::vector<bool> &valids);
  void set_flange_baud_rate(unsigned int baud_rate);
  WrenchData get_tcp_force();
  void set_tcp_force(const WrenchData &wrench);
  void set_force_mode_sensor(const std::string &sensor, unsigned int address);
  void set_force_mode_param(double damping, double gain,
                            const std::vector<double> &max_vel);
  void start_force_mode(const CartesianPose &limit, const WrenchData &wrench);
  void end_force_mode();
  std::vector<PluginInfoData> load_plugins();
  std::vector<PluginStoreInfoData> get_plugin_store();
  PluginInfoData load_plugin(const std::string &name);
  CommandStdoutData run_plugin_cmd(
      const std::string &name,
      const std::vector<std::string> &params = std::vector<std::string>());
  CommandStdoutData enable_plugin(const std::string &name);
  CommandStdoutData disable_plugin(const std::string &name);
  void restart_plugin_daemon(const std::string &name);
  std::vector<DiscoveredRobotData> discover_robots();
  CommandStdoutData get_plugin_daemon_stdout(const std::string &name);
  std::vector<MessageData> get_messages();
  OtaStateData get_ota_state();
  CheckUpgradeData check_upgrade();
  CommandStdoutData get_upgrade_stdout();
  void sub_buttons_status(uint64_t interval_min, uint64_t interval_max);
  void sub_kin_data(uint64_t interval_min, uint64_t interval_max);
  void sub_message(uint64_t interval_min, uint64_t interval_max);
  void sub_robot_state(uint64_t interval_min, uint64_t interval_max);
  void sub_phy_data(uint64_t interval_min, uint64_t interval_max);
  void sub_task_stdout(uint64_t interval_min, uint64_t interval_max);
  void start_ota(const std::string &address, const std::string &partition,
                 const std::string &file);
  void switch_partition(const std::string &address,
                        const std::string &partition);
  void start_upgrade();
  int box_test();
  std::string init_robot(const std::string &time, const std::string &auth,
                         const RobotInfoData &info);
  PhysicalData get_phy_data();
  JointMotionData get_kin_data();
 
  bool is_disconnected();
  bool is_down();
  std::vector<double> get_actual_joint_positions();
  std::vector<double> get_target_joint_positions();
  std::vector<double> get_actual_joint_speed();
  // get_target_joint_speed
  std::vector<double> get_target_joint_speed();
  CartesianPose get_actual_tcp_pose();
  CartesianPose get_target_tcp_pose();
 
  double get_joint_temp(unsigned int joint_index);
 
  std::vector<double> get_actual_joint_torques();
  std::vector<double> get_target_joint_torques();
 
  void set_do(std::string device, unsigned int pin, unsigned int value);
  void set_dos(std::string device, unsigned int pin,
               std::vector<unsigned int> values);
  unsigned int get_do(std::string device, unsigned int pin);
  std::vector<unsigned int> get_dos(std::string device, unsigned int pin,
                                    unsigned int num);
  unsigned int get_di(std::string device, unsigned int pin);
  std::vector<unsigned int> get_dis(std::string device, unsigned int pin,
                                    unsigned int num);
 
  void set_ao(std::string device, unsigned int pin, double value);
  void set_aos(std::string device, unsigned int pin, std::vector<double> values);
  double get_ao(std::string device, unsigned int pin);
  std::vector<double> get_aos(std::string device, unsigned int pin,
                              unsigned int num);
  double get_ai(std::string device, unsigned int pin);
  std::vector<double> get_ais(std::string device, unsigned int pin,
                              unsigned int num);
  void set_dio_mode(std::string device, unsigned int pin, bool value);
  bool get_dio_mode(std::string device, unsigned int pin);
  std::vector<bool> get_dios_mode(std::string device, unsigned int pin,
                                  unsigned int count);
  void enable_button(std::string device, unsigned int pin);
  void disable_button(std::string device, unsigned int pin);
  void init_claw(bool force_initilization);
  void set_claw(double force, double amplitude);
  void set_claw_ao(unsigned int address, double value);
 
  ClawData get_claw();
  ClawData get_claw_data();
  double get_claw_ai(unsigned int address);
  void wait_claw_ai(unsigned int address, double value,
                    std::string relation = "EQ");
  void set_led(unsigned int mode, unsigned int speed,
               const std::vector<unsigned int> &color);
  void set_voice(unsigned int voice, unsigned int volume);
  void set_fan(unsigned int status);
  void set_signal(unsigned int index, int value);
  void set_signals(unsigned int index, std::vector<int> values);
  int get_signal(unsigned int index);
  std::vector<int> get_signals(unsigned int index, unsigned int len);
  void wait_signal(unsigned int index, int value, std::string relation = "EQ");
  void add_signal(unsigned int index, int value);
  unsigned int start_task(const std::string &name,
                          const std::vector<std::string> &params,
                          const std::string &dir, bool is_parallel,
                          unsigned int loop_to);
  unsigned int start_task(const std::string &name);
  std::vector<unsigned int> load_task_list();
  std::vector<unsigned int> get_task_list();
  std::vector<TaskData> load_running_tasks();
  TaskStdoutData get_task_stdout(unsigned int id);
  std::string wait_task(unsigned int id);
  void pause_task(unsigned int id, unsigned long time, bool wait);
  void pause_task(unsigned int id);
  void resume_task(unsigned int id);
  void cancel_task(unsigned int id);
  unsigned int exec_hook(unsigned int id);
  std::string load_task();
  std::string get_task_state();
  std::string load_task(unsigned int id);
  std::string get_task_state(unsigned int id);
  KinematicsForwardResp get_forward_kin(
      const std::vector<double> &joint_positions);
  KinematicsForwardResp kinematics_forward(
      const std::vector<double> &joint_positions);
 
  KinematicsInverseResp get_inverse_kin(
      const CartesianPose &pose,
      const std::vector<double> &joint_init_positions = {});
  KinematicsInverseResp kinematics_inverse(
      const CartesianPose &pose,
      const std::vector<double> &joint_init_positions = {});
  double measure_manipulation(const std::vector<double> &joint_positions);
 
  CartesianPose get_pose_trans(const CartesianPose &a, const CartesianPose &b);
  CartesianPose pose_times(const CartesianPose &a, const CartesianPose &b);
  CartesianPose get_pose_add(const CartesianPose &pose,
                             const CartesianPose &delta);
  CartesianPose calc_frame(const CartesianPose &o, const CartesianPose &x,
                           const CartesianPose &xy);
  CartesianPose calc_tcp(const std::vector<CartesianPose> &poses);
 
  CartesianPose get_pose_inverse(const CartesianPose &in);
  CartesianPose pose_inverse(const CartesianPose &in);
  void save_file(const std::string &dir, const std::string &name, bool is_dir,
                 const std::string &data);
  void rename_file(const std::string &from_dir, const std::string &from_name,
                   const std::string &to_dir, const std::string &to_name);
 
  void download_file(const std::string &dir, const std::string &name,
                     const std::string &url);
 
  std::tuple<bool, std::string> load_file(const std::string &dir,
                                          const std::string &name);
 
  std::vector<std::tuple<bool, std::string>> load_file_list(
      const std::string &dir, const std::string &prefix,
      const std::string &suffix);
  void zip(const std::string &from_dir, std::vector<std::string> files,
           const std::string &to_dir, const std::string &name);
  void unzip(const std::string &from_dir, const std::string &name,
             std::vector<std::string> files, const std::string &to_dir);
  std::vector<std::tuple<bool, std::string>> load_zip_list(
      const std::string &zip, const std::string &dir,
      const std::string &prefix, const std::string &suffix);
 
  void set_tcp(std::array<double, 6> tcp);
  std::array<double, 6> get_tcp();
  std::vector<DhParamData> get_dh();
  void set_dh(const std::vector<DhParamData> &params);
  void set_kin_factor(int factor);
  void set_velocity_factor(int factor);
  int get_kin_factor();
  int get_velocity_factor();
  void set_payload(double mass, std::map<std::string, double> cog);
  void set_payload_mass(double mass);
  void set_payload_cog(std::map<std::string, double> cog);
  void save_payload(std::string name, std::map<std::string, double> payload,
                    std::string dir = "");
  std::map<std::string, double> get_payload();
  std::map<std::string, double> load_payload(std::string name,
                                             std::string dir = "");
  std::vector<std::string> load_payload_list(std::string dir = "");
  void set_gravity(std::map<std::string, double> gravity);
  std::map<std::string, double> get_gravity();
  void enable_collision_detector();
  void disable_collision_detector();
  void set_collision_torque_diff(const std::vector<double> &diffs);
  std::vector<double> get_collision_torque_diff();
  void set_collision_detector(const CollisionDetectorConfig &config);
  CollisionDetectorConfig get_collision_detector();
  void enable_limit();
  void disable_limit();
  void set_joints_limit(const std::vector<JointLimitConfig> &joints);
  std::vector<JointLimitConfig> get_joints_limit();
  void set_cart_limit(const CartesianLimitConfig &limit);
  CartesianLimitConfig get_cart_limit();
  CartesianPose load_tcp(std::string name, std::string dir = "");
  void save_tcp(std::string name, CartesianPose tcp, std::string dir = "");
  std::vector<std::string> load_tcp_list(std::string dir = "");
  std::vector<std::string> load_trajectory_list(std::string dir = "");
  TrajectoryData load_trajectory(std::string name, std::string dir = "");
  void save_trajectory(std::string name, TrajectoryData trajectory,
                       std::string dir = "");
  unsigned int move_trajectory(std::string name, std::string dir = "");
  void start_record_trajectory(std::string kind, double duration);
  void end_record_trajectory(std::string name, std::string dir = "");
  std::vector<std::string> load_pose_list(std::string dir = "");
  PoseData load_pose(std::string name, std::string dir = "");
  void save_pose(std::string name, PoseData pose, std::string dir = "");
  std::vector<std::string> load_frame_list(std::string dir = "");
  FrameData load_frame(std::string name, std::string dir = "");
  void save_frame(std::string name, FrameData frame, std::string dir = "");
  std::vector<std::string> load_structure_list(std::string dir = "");
  StructureData load_structure(std::string name, std::string dir = "");
  void save_structure(std::string name, StructureData structure,
                      std::string dir = "");
  std::vector<std::string> load_modbus_list(std::string dir = "");
  ModbusData load_modbus(std::string name, std::string dir = "");
  void save_modbus(std::string name, ModbusData modbus);
  void set_modbus_timeout(std::string device, unsigned int timeout);
  void set_modbus_retry(std::string device, unsigned int retry);
  void disconnect_modbus(std::string device);
  std::vector<std::string> load_modbus_register_list(std::string device);
  ModbusRegisterData load_modbus_register(std::string device,
                                          std::string name);
  void save_modbus_register(std::string device, std::string name,
                            ModbusRegisterData reg);
 
  void write_single_coil(std::string device, std::string addr, bool value);
 
  void write_multiple_coils(std::string device, std::string addr,
                             std::vector<bool> values);
  void wirte_multiple_coils(std::string device, std::string addr,
                            std::vector<bool> values);
 
  std::vector<bool> read_coils(std::string device, std::string addr,
                               unsigned int num);
  std::vector<bool> read_discrete_inputs(std::string device, std::string addr,
                                         unsigned int num);
 
  void write_single_register(std::string device, std::string addr,
                             unsigned int value);
  void write_multiple_registers(std::string device, std::string addr,
                                std::vector<unsigned int> values);
 
  std::vector<unsigned int> read_holding_registers(std::string device,
                                                   std::string addr,
                                                   unsigned int num);
  std::vector<unsigned int> read_input_registers(std::string device,
                                                 std::string addr,
                                                 unsigned int num);
  void set_serial_baud_rate(std::string device, unsigned int baud_rate);
  void set_serial_timeout(std::string device, unsigned int timeout);
  void set_serial_parity(std::string device, unsigned int parity);
  void write_serial(std::string device, std::vector<unsigned int> data);
  std::vector<unsigned int> read_serial(std::string device, unsigned int len);
  void clear_serial(std::string device);
  void set_item(StorageItem item);
  StorageItem get_item(std::string name);
  std::vector<StorageItem> get_items(std::string prefix);
 protected:
  std::unique_ptr<RobotImpl> impl_; 
};
 
}  // namespace l_master
 
}  // namespace lebai