171 lines
10 KiB
C++
171 lines
10 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 "common/types/arm/arm_types.h"
|
|
#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h"
|
|
#include "cmvr/common/geometry.pb.h"
|
|
#include "cmvr/msgs/motor.pb.h"
|
|
#include "common/base/logging/logger.h"
|
|
#include <Eigen/Dense>
|
|
|
|
namespace cmvr::device{
|
|
|
|
class AbstractRobot: public AbstractDevice {
|
|
public:
|
|
AbstractRobot() = default;
|
|
~AbstractRobot() override=default;
|
|
|
|
DeviceKind kind() const noexcept override { return DeviceKind::Robot; }
|
|
|
|
virtual int getDOF() { CMVR_LOG(ERROR) << "[AbstractRobot] getDOF is not implemented"; return 0; }
|
|
|
|
virtual std::vector<std::string> getJointNames() { CMVR_LOG(ERROR) << "[AbstractRobot] getJointNames is not implemented"; return {}; }
|
|
|
|
virtual std::unordered_map<std::string,double> getJointQ() const { CMVR_LOG(ERROR) << "[AbstractRobot] getJointQ is not implemented"; return {}; }
|
|
|
|
/**
|
|
*
|
|
* @param joint_qs 函数会根据 joint_qs 的 joint name 去获取值
|
|
*/
|
|
virtual void getJointQ(std::unordered_map<std::string,double> &joint_qs) const { CMVR_LOG(ERROR) << "[AbstractRobot] getJointQ(out) is not implemented"; joint_qs.clear(); }
|
|
virtual std::vector<std::string> getLinkNames() { CMVR_LOG(ERROR) << "[AbstractRobot] getLinkNames is not implemented"; return {}; }
|
|
|
|
virtual void getJointsState(std::vector<JointState>& states) = 0;
|
|
virtual void getState(RobotState &state) { CMVR_LOG(ERROR) << "[AbstractRobot] getState is not implemented"; state = RobotState{}; }
|
|
|
|
virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) { CMVR_LOG(ERROR) << "[AbstractRobot] getTransform is not implemented"; return {}; }
|
|
|
|
virtual void torqueOn() { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOn is not implemented"; }
|
|
virtual void torqueOn(const std::string &joint_name) { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOn(joint) is not implemented: " << joint_name; }
|
|
|
|
virtual void torqueOff() { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOff is not implemented"; }
|
|
virtual void torqueOff(const std::string &joint_name) { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOff(joint) is not implemented: " << joint_name; }
|
|
|
|
virtual void eStop() { CMVR_LOG(ERROR) << "[AbstractRobot] eStop is 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) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(vector<double>) is 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) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(vector<JointPoint>) is not implemented"; }
|
|
virtual void speedJ(std::vector<JointVelocityCommand> &cmd) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(commands) is not implemented"; }
|
|
virtual void speedJ(double vel) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(double) is 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, cmvr::common::Pose3d pose,double vel = 0.5, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(pose) is not implemented"; }
|
|
virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveDeltaJ is not implemented"; }
|
|
|
|
virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ_IK is not implemented"; }
|
|
|
|
virtual bool moveL(const std::vector<double> &pose,
|
|
double speed = 0.25,
|
|
double acceleration = 1.2,
|
|
double jerk = 5.0,
|
|
const std::vector<double> &qd_max = std::vector<double>(7, 2.5),
|
|
bool asynchronous = false) {
|
|
CMVR_LOG(ERROR) << "[AbstractRobot] moveL is not implemented";
|
|
return false;
|
|
}
|
|
virtual bool speedL(const std::vector<double> &xd,
|
|
double acceleration = 0.25,
|
|
double time = 0.0,
|
|
cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) {
|
|
CMVR_LOG(ERROR) << "[AbstractRobot] speedL is not implemented";
|
|
return false;
|
|
}
|
|
virtual Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase() {
|
|
return Eigen::Matrix<double, 6, 1>::Zero();
|
|
}
|
|
virtual void stopSpeedL() { CMVR_LOG(ERROR) << "[AbstractRobot] stopSpeedL is not implemented"; }
|
|
|
|
virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(joint) is not implemented: " << joint_name; }
|
|
|
|
virtual void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followJointTrajectory(double) is not implemented"; }
|
|
|
|
virtual void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followJointTrajectory(JointPoint) is not implemented"; }
|
|
|
|
virtual void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followPoseTrajectory is not implemented"; }
|
|
|
|
/**
|
|
* @brief 关节空间周期伺服（无整段轨迹规划）。
|
|
* @details 每次调用只在一个控制周期内跟踪当前目标，需外部循环持续下发；是否稳定由外部更新频率和目标序列决定。
|
|
* @param joints 当前周期目标关节角，单位 rad。
|
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
|
*/
|
|
virtual void servoJ(std::vector<double> &joints, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector<double>) is not implemented"; }
|
|
|
|
/**
|
|
* @brief 按关节命令执行周期伺服（无整段轨迹规划）。
|
|
* @param joints 当前周期关节目标命令。
|
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
|
*/
|
|
virtual void servoJ(std::vector<JointPoint> &joints, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector<JointPoint>) is not implemented"; }
|
|
|
|
/**
|
|
* @brief 带速度参数的关节空间周期伺服。
|
|
* @param joints 当前周期关节目标命令。
|
|
* @param vel 速度参数（语义由具体驱动实现决定）。
|
|
* @param dt 当前伺服命令生效时长（控制周期），单位 s。
|
|
*/
|
|
virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector<JointPoint>, vel) is 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, cmvr::common::Pose3d pose,double vel = 0.1, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(pose) is not implemented"; }
|
|
virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] servoDeltaJ is not implemented"; }
|
|
|
|
|
|
virtual void servoL(math::Pose3d &pose, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoL is not implemented"; }
|
|
|
|
virtual void calibrateZeroQ(const std::string &joint_name) = 0;
|
|
|
|
void setToolFrame(const std::string& toolFrame)
|
|
{
|
|
toolFrame_ = toolFrame;
|
|
}
|
|
virtual std::string getToolFrame() const { return toolFrame_; }
|
|
|
|
virtual cmvr::common::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
|
|
virtual cmvr::common::Pose3d fk(bool is_tcp = true) = 0;
|
|
virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose) = 0;
|
|
protected:
|
|
int dof_{};
|
|
RobotState state_{};
|
|
std::string toolFrame_;
|
|
};
|
|
}
|
|
|
|
#endif //CMVR_ES_ABSTRACT_ROBOT_H
|