243 lines
10 KiB
C++
243 lines
10 KiB
C++
//
|
|
// Created by xtkuang on 2025/7/24.
|
|
//
|
|
|
|
#ifndef CMVR_ES_HUMANOID_ROBOT_H
|
|
#define CMVR_ES_HUMANOID_ROBOT_H
|
|
|
|
#include "../../abstract_robot.h"
|
|
#include "../../../../utils/controller/include/cartesian_controller.h"
|
|
#include "../../../../utils/base/include/os.h"
|
|
#include "../../../../utils/base/include/timer.h"
|
|
#include "../../../../utils/base/include/ring_buffer.h"
|
|
|
|
#include "../../../canbus/abstract_canbus.h"
|
|
#include "canbus/can_comm/can_sender.h"
|
|
#include "canbus/can_comm/can_receiver.h"
|
|
#include "canbus/can_comm/message_manager.h"
|
|
#include "cmvr/msgs/robot_detail.pb.h"
|
|
#include "motor/motor_manager.h"
|
|
#include <thread>
|
|
#include <queue>
|
|
#include <mutex>
|
|
#include <condition_variable>
|
|
#include <atomic>
|
|
#include <cstdint>
|
|
#include <vector>
|
|
#include <memory>
|
|
#include <chrono>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <iomanip>
|
|
#include <Eigen/Dense>
|
|
#include <Eigen/Geometry>
|
|
|
|
#include "planner/joint_space_planner/include/joint_space_planner_creator.h"
|
|
#include "planner/joint_space_planner/include/joint_space_planner.h"
|
|
#include "ik_solver/include/pinocchio_dls_ik_solver.h"
|
|
#include "cmvr/config/robot_config/robot_config.pb.h"
|
|
|
|
|
|
namespace cmvr::device{
|
|
|
|
template<int DOF>
|
|
class HumanoidRobot: public AbstractRobot {
|
|
typedef enum {
|
|
ROBOT_DISABLED,
|
|
ROBOT_READY,
|
|
ROBOT_RUNNING,
|
|
ROBOT_ESTOP,
|
|
ROBOT_TOROFF,
|
|
ROBOT_ERROR
|
|
} RobotStateMachine;
|
|
|
|
public:
|
|
explicit HumanoidRobot(const XmlNode& cfg);
|
|
HumanoidRobot(const cmvr::config::HumanRobotConfig& config);
|
|
HumanoidRobot(const cmvr::config::EthercatRobotConfig& config);
|
|
~HumanoidRobot();
|
|
|
|
void init() override;
|
|
|
|
/* get robot state and info*/
|
|
int getDOF() override;
|
|
std::vector<std::string> getJointNames() override;
|
|
std::vector<std::string> getLinkNames() override;
|
|
|
|
std::unordered_map<std::string,double> getJointQ() const override;
|
|
void getJointQ(std::unordered_map<std::string,double> &joint_qs) const override;
|
|
|
|
void getJointsState(std::vector<JointState>& states) override;
|
|
void getState(RobotState &state) override;
|
|
math::Pose3d getTransform(std::string &bask_link, std::string &target_link) override;
|
|
|
|
/* torque on and off */
|
|
void torqueOn() override;
|
|
void torqueOn(const std::string &joint_name) override;
|
|
void torqueOff() override;
|
|
void torqueOff(const std::string &joint_name) override;
|
|
void calibrateZeroQ(const std::string &joint_name) override;
|
|
|
|
/* robot basic command*/
|
|
void eStop() override;
|
|
void moveJ(std::vector<JointPoint> &cmd, double vel = 0.5, double acc = 0.1) override;
|
|
void speedJ(std::vector<JointVelocityCommand> &cmd) override;
|
|
void speedJ(double vel) override;
|
|
void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override;
|
|
void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override ;
|
|
void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;
|
|
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) override;
|
|
|
|
|
|
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
|
|
|
|
bool speedL(const std::vector<double> &xd,
|
|
double acceleration = 0.25,
|
|
double time = 0.0,
|
|
cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) override;
|
|
void stopSpeedL() override;
|
|
Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase() override;
|
|
|
|
|
|
|
|
|
|
void servoJ(std::vector<JointPoint> &joints, double dt) override;
|
|
void servoJ(std::vector<JointPoint> &joints, double vel, double dt) override;
|
|
void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) override;
|
|
void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;
|
|
void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) override;
|
|
|
|
protected:
|
|
bool check_joint_traj_(std::vector<std::vector<JointPoint>> &traj, double dt);
|
|
void update_state_();
|
|
int exec_PPM_cmd_(std::vector<int> &ids, std::vector<double> &pos, std::vector<double> &vel, std::vector<double> &acc);
|
|
int exec_PVM_cmd_(std::vector<int> &ids, std::vector<double> &vel, std::vector<double> &acc);
|
|
int exec_PTM_cmd_(std::vector<int> &ids, std::vector<double> &acc);
|
|
int exec_CSP_cmd_(std::vector<int> &ids, std::vector<double> &pos, int dt);
|
|
int exec_CSV_cmd_(std::vector<int> &ids, std::vector<double> &vel, int dt);
|
|
int exec_CSC_cmd_(std::vector<int> &ids, std::vector<double> &cur, int dt);
|
|
void sendJointVelocityCommand_(const std::vector<JointVelocityCommand>& cmd);
|
|
bool updateSpeedLAccelerationConfig_(double acceleration);
|
|
void ensureSpeedLWorkerStarted_();
|
|
void stopSpeedLWorker_();
|
|
void speedLWorkerLoop_();
|
|
bool executeMoveLScurve_(const Eigen::Matrix4d& target_pose_input,
|
|
double v_tcp_max,
|
|
double a_tcp_max,
|
|
double j_tcp_max,
|
|
const std::vector<double>& qd_max,
|
|
double dt_real,
|
|
bool is_tcp,
|
|
cmvr::CartesianFrame input_frame);
|
|
|
|
|
|
|
|
|
|
|
|
// 添加这三个函数的声明
|
|
bool computeJacobian(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
|
|
const std::string& base_link,
|
|
const std::string& ee_link,
|
|
Eigen::Matrix<double, 6, DOF>& J);
|
|
|
|
Eigen::Vector3d getEndEffectorPosition(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
|
|
const std::string& ee_link);
|
|
|
|
Eigen::Matrix3d getEndEffectorOrientation(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
|
|
const std::string& ee_link);
|
|
|
|
|
|
|
|
public:
|
|
// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z
|
|
static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz);
|
|
static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);
|
|
msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override;
|
|
msgs::Pose3d fk(bool is_tcp) override;
|
|
std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override;
|
|
|
|
|
|
private:
|
|
std::string id_;
|
|
int upd_freq_;
|
|
std::shared_ptr<FDTimer> upd_timer_;
|
|
std::atomic<RobotStateMachine> rsm_{ROBOT_DISABLED};
|
|
// std::atomic<RobotRunningMode> rmode_{PPM_MODE};
|
|
|
|
std::shared_ptr<cmvr::dyn::State<DOF>> m_state_;
|
|
std::shared_ptr<cmvr::dyn::Robot<DOF>> m_robot_;
|
|
std::shared_ptr<cmvr::ctrl::CartesianController<DOF>> m_cctrl_;
|
|
|
|
std::vector<std::string> joint_names_;
|
|
std::vector<std::string> link_names_;
|
|
|
|
std::atomic<bool> flash_cmd_{false};
|
|
std::mutex state_mtx_{};
|
|
std::mutex exec_mtx_{};
|
|
|
|
std::shared_ptr<SPMCRingBuffer<JointPoint>> CSP_buffer_;
|
|
std::shared_ptr<SPMCRingBuffer<JointVelocityCommand>> CSV_buffer_;
|
|
std::shared_ptr<SPMCRingBuffer<JointCurrentCommand>> CSC_buffer_;
|
|
|
|
|
|
struct ControlPoint {
|
|
Eigen::Vector<double, DOF> joint_positions;
|
|
Eigen::Vector<double, DOF> joint_velocities;
|
|
double timestamp;
|
|
ControlPoint() : timestamp(0.0) {}
|
|
ControlPoint(const Eigen::Vector<double, DOF>& pos, const Eigen::Vector<double, DOF>& vel, double time): joint_positions(pos), joint_velocities(vel), timestamp(time) {}
|
|
};
|
|
// 线程间通信的队列和同步变量
|
|
std::queue<ControlPoint> control_queue_;
|
|
std::mutex queue_mutex_;
|
|
std::condition_variable queue_cv_;
|
|
std::atomic<bool> trajectory_completed_{false};
|
|
std::atomic<bool> control_thread_ready_{false};
|
|
std::atomic<bool> stop_requested_{false};
|
|
|
|
// 线程对象
|
|
std::unique_ptr<std::thread> trajectory_thread_;
|
|
std::unique_ptr<std::thread> control_thread_;
|
|
|
|
|
|
|
|
|
|
private:
|
|
|
|
std::shared_ptr<MotorManager> motor_manager_{nullptr};
|
|
|
|
std::shared_ptr<JointSpacePlanner> joint_space_planner_{nullptr};
|
|
std::shared_ptr<PinocchioDlsIKSolver> ik_solver_{nullptr};
|
|
PinocchioDlsIKSolver::SpeedLConfig speedl_config_{};
|
|
std::unique_ptr<std::thread> speedl_thread_;
|
|
std::mutex speedl_mutex_;
|
|
std::condition_variable speedl_cv_;
|
|
std::atomic<bool> speedl_stop_requested_{false};
|
|
bool speedl_command_active_{false};
|
|
Eigen::Matrix<double, 6, 1> speedl_target_twist_{Eigen::Matrix<double, 6, 1>::Zero()};
|
|
Eigen::Matrix<double, 6, 1> speedl_last_command_twist_base_{Eigen::Matrix<double, 6, 1>::Zero()};
|
|
double speedl_target_acceleration_{0.25};
|
|
cmvr::CartesianFrame speedl_target_frame_{cmvr::CartesianFrame::Base};
|
|
double speedl_applied_acceleration_{0.25};
|
|
std::uint64_t speedl_command_version_{0};
|
|
|
|
// 每个电机组的锁和执行状态
|
|
std::mutex left_arm_mutex_, right_arm_mutex_, head_mutex_, waist_mutex_;
|
|
std::atomic<bool> is_left_arm_busy_{false}, is_right_arm_busy_{false};
|
|
std::atomic<bool> is_head_busy_{false}, is_waist_busy_{false};
|
|
|
|
|
|
std::string urdf_path_ = "";
|
|
};
|
|
|
|
|
|
}
|
|
|
|
|
|
#endif //CMVR_ES_HUMANOID_ROBOT_H
|