184 lines
9.8 KiB
C++
184 lines
9.8 KiB
C++
//
|
|
// Created by xtkuang on 2025/5/8.
|
|
//
|
|
|
|
#ifndef CMVR_ES_ABSTRACT_ROBOT_H
|
|
#define CMVR_ES_ABSTRACT_ROBOT_H
|
|
#pragma once
|
|
|
|
#include "json/json.h"
|
|
#include "../abstract_device.h"
|
|
#include "../../utils/controller/include/cartesian_controller.h"
|
|
#include "cmvr/msgs/geometry.pb.h"
|
|
#include "cmvr/msgs/motor.pb.h"
|
|
|
|
namespace cmvr::device{
|
|
|
|
struct JointPoint{
|
|
std::string joint_name;
|
|
double rad; // rad
|
|
double vel; // rad / s
|
|
|
|
JointPoint(const std::string& name, double r, double v)
|
|
: joint_name(name), rad(r), vel(v) {}
|
|
JointPoint(const std::string& name, double r)
|
|
: joint_name(name), rad(r), vel(0) {}
|
|
JointPoint()
|
|
: joint_name(""), rad(0), vel(0) {}
|
|
} ;
|
|
|
|
typedef struct {
|
|
std::string joint_name;
|
|
double vel; // rad / s
|
|
} JointVelocityCommand;
|
|
|
|
typedef struct {
|
|
std::string joint_name;
|
|
double current; //
|
|
} JointCurrentCommand;
|
|
|
|
class AbstractRobot: public AbstractDevice {
|
|
public:
|
|
explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
|
|
~AbstractRobot() override=default;
|
|
|
|
virtual int getDOF() { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual std::vector<std::string> getJointNames() { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual std::unordered_map<std::string,double> getJointQ() const { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
*
|
|
* @param joint_qs 函数会根据 joint_qs 的 joint name 去获取值
|
|
*/
|
|
virtual void getJointQ(std::unordered_map<std::string,double> &joint_qs) const {throw std::runtime_error("Not implemented");}
|
|
virtual std::vector<std::string> getLinkNames() { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void getJointsState(std::vector<JointState>& states) = 0;
|
|
virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
|
|
|
|
virtual void torqueOn() { throw std::runtime_error("Not implemented"); }
|
|
virtual void torqueOn(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void torqueOff() { throw std::runtime_error("Not implemented"); }
|
|
virtual void torqueOff(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void eStop() { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
* @brief 关节空间 point-to-point 运动（内部轨迹规划）。
|
|
* @details 控制器会在内部生成满足速度/加速度约束的关节轨迹，并做多关节时间同步，使关节协调到达目标点。
|
|
* @param joints 目标关节角，单位 rad。
|
|
* @param max_vel 主导轴最大速度，单位 rad/s。
|
|
* @param max_acc 主导轴最大加速度，单位 rad/s^2。
|
|
*/
|
|
virtual void moveJ(std::vector<double> &joints, double max_vel=0.5, double max_acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
* @brief 按关节命令执行关节空间运动（内部轨迹规划）。
|
|
* @param cmd 目标关节命令（关节名 + 角度，`vel` 字段可由具体实现决定是否使用）。
|
|
* @param vel 主导轴最大速度，单位 rad/s。
|
|
* @param acc 主导轴最大加速度，单位 rad/s^2。
|
|
*/
|
|
virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
virtual void speedJ(std::vector<JointVelocityCommand> &cmd) { throw std::runtime_error("Not implemented"); }
|
|
virtual void speedJ(double vel) { throw std::runtime_error("Not implemented"); }
|
|
/**
|
|
* @brief 末端位姿目标的关节空间运动。
|
|
* @details 典型实现为先做 IK 求解关节目标，再按 `moveJ` 语义执行（关节空间规划与同步）。
|
|
* @param base_link 基坐标系 link 名称。
|
|
* @param ee_link 末端 link 名称。
|
|
* @param pose 目标末端位姿。
|
|
* @param vel 主导轴最大速度，单位 rad/s。
|
|
* @param acc 主导轴最大加速度，单位 rad/s^2。
|
|
*/
|
|
virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
|
|
virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
* @brief 关节空间周期伺服（无整段轨迹规划）。
|
|
* @details 每次调用只在一个控制周期内跟踪当前目标，需外部循环持续下发；是否稳定由外部更新频率和目标序列决定。
|
|
* @param joints 当前周期目标关节角，单位 rad。
|
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
|
*/
|
|
virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
* @brief 按关节命令执行周期伺服（无整段轨迹规划）。
|
|
* @param joints 当前周期关节目标命令。
|
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
|
*/
|
|
virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
* @brief 带速度参数的关节空间周期伺服。
|
|
* @param joints 当前周期关节目标命令。
|
|
* @param vel 速度参数（语义由具体驱动实现决定）。
|
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
|
*/
|
|
virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
/**
|
|
* @brief 基于末端位姿目标的周期伺服。
|
|
* @details 典型实现为每周期执行 IK 并下发关节伺服目标；属于实时闭环接口，不等价于一次性到位的 `moveJ`。
|
|
* @param base_link 基坐标系 link 名称。
|
|
* @param ee_link 末端 link 名称。
|
|
* @param pose 当前周期目标末端位姿。
|
|
* @param vel 速度参数（语义由具体驱动实现决定）。
|
|
* @param acc 加速度参数（语义由具体驱动实现决定）。
|
|
*/
|
|
virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
|
|
|
|
|
|
virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); }
|
|
|
|
virtual void calibrateZeroQ(const std::string &joint_name) = 0;
|
|
|
|
void setToolFrame(const std::string& toolFrame)
|
|
{
|
|
toolFrame_ = toolFrame;
|
|
}
|
|
|
|
virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
|
|
virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
|
|
|
|
virtual bool getPoseMatrix(std::string &bask_link, std::string &ee_link, Eigen::Matrix4d& cur_pose) = 0;
|
|
virtual bool fk(const std::vector<double>& joints_angle, Eigen::Matrix4d& cur_pose, bool is_tcp = true) = 0;
|
|
protected:
|
|
int dof_{};
|
|
RobotState state_{};
|
|
std::string toolFrame_;
|
|
};
|
|
}
|
|
|
|
#endif //CMVR_ES_ABSTRACT_ROBOT_H
|