init

include/armcontroller.hpp

@@ -0,0 +1,124 @@
+/*
+本程序用于机械臂的基本控制
+*/
+#include <unitree/robot/channel/channel_publisher.hpp>
+#include <unitree/common/time/time_tool.hpp>
+#include <string>
+#include "msg/ArmString_.hpp"
+
+#include <urdf_model/model.h>                 // 解析 URDF文本/文件
+#include <kdl_parser/kdl_parser.hpp>          //URDF -> KDL::Tree
+#include <kdl/tree.hpp>                       //结构树
+#include <kdl/chain.hpp>                      //让solver可识别
+#include <kdl/jntarray.hpp>                   //数据容器
+#include <kdl/frames.hpp>                     //位姿工具提供
+#include <kdl/chainfksolverpos_recursive.hpp> //前向运动学
+#include <kdl/chainiksolverpos_lma.hpp>       //逆向运动学
+
+#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<string> &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 goForward();
+
+    // 夹爪后退
+    void goBack();
+
+    // 夹爪向上
+    void goUp();
+
+    // 夹爪向下
+    void goDown();
+
+    // 夹爪左转
+    void goLeft();
+
+    // 夹爪右转
+    void goRight();
+
+    // 夹爪放开
+    void jawRelease();
+
+    // 夹爪夹住
+    void jawClaming();
+
+    ///////预设位置///////
+    // 回原点
+    void goHome();
+
+    // 捡垃圾预设位置
+    void trashActionDefault();
+
+    // 设置电机参数
+    void setAngle(msg_data msgData_);
+
+private:
+    // 发送消息
+    msg_data msgData_{};
+    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<unitree_arm::msg::dds_::ArmString_> publisher_{TOPIC}; // 机械臂dds控制句柄
+    KDL::Chain outchain_;                                                   // 保存机械臂结构
+    KDL::JntArray qSeed_;                                                   // 当前位姿
+    KDL::JntArray qLast_;                                                   // 保存上次的位姿
+    KDL::JntArray qSol_;                                                    // 输出位姿
+    vector<string> outJoinName_;                                            // 保存关节顺序
+};