/* 本程序用于机械臂的基本控制 */ #include #include #include #include "msg/ArmString_.hpp" #include // 解析 URDF文本/文件 #include //URDF -> KDL::Tree #include //结构树 #include //让solver可识别 #include //数据容器 #include //位姿工具提供 #include //前向运动学 #include //逆向运动学 #include #define TOPIC "rt/arm_Command" #define TOPICSTATUS "current_servo_angle" #define TOPICSTATUS1 "rt/arm_Feedback" using namespace std; using namespace unitree::robot; using namespace unitree::common; // 弧度互转工具,角度转弧度 static inline double rad2deg(double r) { return r * 180.0 / M_PI; } static inline double deg2rad(double d) { return d * M_PI / 180.0; } // 消息结构体 struct msg_data { // 电机编号 double angle0; // 底座 double angle1; double angle2; double angle3; double angle4; double angle5; double angle6; // 夹爪 }; // struct anglePos // { // double x; // double y; // double z; // double roll; // double pitch; // double yaw; // }; // 机械臂控制对象 class armController { public: armController(); ~armController(); void Init(); // 从urdf构建base->tip的kdl::chain,并返回关节顺序(后续角度填写顺序) bool buildChainFromUrdf(const string &urdfPath, const string &baseLink, const string &tipLink, KDL::Chain &outChain, vector &outJointNames); bool ikPoseRPY(double x, double y, double z, double roll, double pitch, double yaw); // 根据角度求坐标 bool getCurrentPoseFromMsg(double &x,double &y,double &z,double &roll,double &pitch,double &yaw); // 填充至JntArray void fillToJntArray(const msg_data &msgdata_, KDL::JntArray &qSol); // 填充至结构体中 void fillToMsgData(const KDL::JntArray &qSol, msg_data &msgdata); ///////基本控制/////// void goAction(const double x,const double y,const double z,const double roll,const double pitch,const double yaw); // 电机动作 void angleLRAction(double lr); // 夹爪后退 void angleUpDownAction(double j1,double j2,double j4); // 夹爪向上 void angleRotateAction(double j5); //扔垃圾 void throwTrash(); // 夹爪向下 void goDown(); // 夹爪左转 void goLeft(); // 夹爪右转 void goRight(); // 夹爪放开 void jawRelease(); // 夹爪夹住 void jawClaming(); ///////预设位置/////// // 回原点 void goHome(); // 捡垃圾预设位置 void trashActionDefault(); // 设置电机参数 void setAngle(msg_data msgData_); private: // 发送消息 msg_data msgData_{}; // anglePos anglePos_{}; uint seq = 0; // 唯一识别码 uint funcdoe = 0; // 指令功能码 uint address = 0; // 指令地址码 uint mode = 0; // 控制模式 unitree_arm::msg::dds_::ArmString_ msg_{}; // 传送消息 unitree::robot::ChannelFactory *arminit_ = ChannelFactory::Instance(); // 初始化句柄 ChannelPublisher publisher_{TOPIC}; // 机械臂dds控制句柄 KDL::Chain outchain_; // 保存机械臂结构 KDL::JntArray qSeed_; // 当前位姿 KDL::JntArray qLast_; // 保存上次的位姿 KDL::JntArray qSol_; // 输出位姿 vector outJoinName_; // 保存关节顺序 };