robot.hh

 
#pragma once
 
#include <memory>
#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 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);
 
  bool is_network_connected();
 
  void start_sys();
  void stop_sys();
  void powerdown();
  void stop();
  void estop();
  void teach_mode();
  void end_teach_mode();
  void pause();
  void resume();
  void reboot();
  int movej(const std::vector<double> &joint_positions, double a, double v,
            double t, double r);
  int movej(const CartesianPose &cart_pose, 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 movel(const CartesianPose &cart_pose, 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 movec(const CartesianPose &cart_via, const CartesianPose &cart,
            double rad, double a, double v, double t, double r);
  int speedj(double a, const std::vector<double> &v, double t = 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 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();
  int get_robot_state();
  int get_estop_reason();
  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);
  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);
  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);
  std::vector<bool> get_dios_mode(std::string device, unsigned int pin,
                                  unsigned int count);
  void init_claw(bool force_initilization);
  void set_claw(double force, double amplitude);
 
  ClawData get_claw_data();
  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);
  int get_signal(unsigned int index);
  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> get_task_list();
  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 get_task_state();
  std::string get_task_state(unsigned int id);
  KinematicsForwardResp kinematics_forward(
      const std::vector<double> &joint_positions);
 
  KinematicsInverseResp kinematics_inverse(
      const CartesianPose &pose,
      const std::vector<double> &joint_init_positions = {});
 
  CartesianPose pose_times(const CartesianPose &a, const CartesianPose &b);
 
  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);
 
  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);
  // /**
  //  *  @brief 将zip文件解压到文件系统.
  //  *
  //  * @param from_dir zip文件的路径.
  //  * @param name zip文件的名称.
  //  * @param files zip文件内的文件名.
  //  * @param to_dir  解压到的路径.
  //  */
  // void unzip(const std::string &from_dir, const std::string &name,
  // std::vector<std::string> files, const std::string &to_dir);
  // /**
  //  * @brief 查询文件列表.
  //  *
  //  * @brief 目标zip文件名.
  //  * @param dir 文件的目录.
  //  * @param prefix 前缀.
  //  * @param suffix 后缀.
  //  *
  //  * @return 文件列表.
  //  */
  // //std::vector<std::tuple<bool,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();
  void set_velocity_factor(int 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);
  std::map<std::string, double> get_payload();
  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 write_single_coil(std::string device, std::string addr, bool value);
 
  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_parity(std::string device, unsigned int parity);
  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