132 lines
4.2 KiB
C++
132 lines
4.2 KiB
C++
/*
|
|
本程序用于机械臂的基本控制
|
|
*/
|
|
#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> //逆向运动学
|
|
#include <thread>
|
|
|
|
#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 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<unitree_arm::msg::dds_::ArmString_> publisher_{TOPIC}; // 机械臂dds控制句柄
|
|
KDL::Chain outchain_; // 保存机械臂结构
|
|
KDL::JntArray qSeed_; // 当前位姿
|
|
KDL::JntArray qLast_; // 保存上次的位姿
|
|
KDL::JntArray qSol_; // 输出位姿
|
|
vector<string> outJoinName_; // 保存关节顺序
|
|
}; |