234 lines
9.8 KiB
C++
234 lines
9.8 KiB
C++
//
|
|
// Created by xtkuang on 2025/7/24.
|
|
//
|
|
|
|
#ifndef CMVR_ES_HUMANOID_ROBOT_H
|
|
#define CMVR_ES_HUMANOID_ROBOT_H
|
|
|
|
#include "devices/abstract_robot.h"
|
|
#include "utils/solver/qp_solver.h"
|
|
#include "utils/base/os.h"
|
|
#include "utils/base/timer.h"
|
|
#include "utils/base/ring_buffer.h"
|
|
|
|
#include "devices/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 <vector>
|
|
#include <memory>
|
|
#include <chrono>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
#include <iomanip>
|
|
#include <Eigen/Dense>
|
|
#include <Eigen/Geometry>
|
|
|
|
#include "../controller/controller_manager.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;
|
|
|
|
// typedef enum {
|
|
// PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE
|
|
// } RobotRunningMode;
|
|
|
|
public:
|
|
explicit HumanoidRobot(const XmlNode& cfg);
|
|
~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 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;
|
|
|
|
void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override;
|
|
void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) override;
|
|
void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) override;
|
|
|
|
|
|
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
|
|
void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override;
|
|
|
|
void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) override;
|
|
void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) 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);
|
|
|
|
|
|
|
|
|
|
|
|
// 添加这三个函数的声明
|
|
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;
|
|
std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override;
|
|
|
|
|
|
double calculateMoveTime(double distance, double vel, double acc);
|
|
void generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc,
|
|
double total_time, size_t num_points,
|
|
std::vector<double>& time_points,
|
|
std::vector<double>& distance_ratios);
|
|
|
|
|
|
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::QPSolver<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::vector<XmlNode> l_motors_cfg_{};
|
|
std::vector<XmlNode> r_motors_cfg_{};
|
|
std::vector<XmlNode> waist_motors_cfg_{};
|
|
std::vector<XmlNode> head_motors_cfg_{};
|
|
|
|
|
|
std::shared_ptr<AbstractCanbus> l_can_client_{nullptr};
|
|
std::shared_ptr<AbstractCanbus> r_can_client_{nullptr};
|
|
std::shared_ptr<AbstractCanbus> waist_can_client_{nullptr};
|
|
std::shared_ptr<AbstractCanbus> head_can_client_{nullptr};
|
|
|
|
std::shared_ptr<CanReceiver<msgs::RobotDetail>> l_can_receiver_{nullptr};
|
|
std::shared_ptr<CanReceiver<msgs::RobotDetail>> r_can_receiver_{nullptr};
|
|
std::shared_ptr<CanReceiver<msgs::RobotDetail>> waist_can_receiver_{nullptr};
|
|
std::shared_ptr<CanReceiver<msgs::RobotDetail>> head_can_receiver_{nullptr};
|
|
|
|
std::shared_ptr<CanSender<msgs::RobotDetail>> l_can_sender_{nullptr};
|
|
std::shared_ptr<CanSender<msgs::RobotDetail>> r_can_sender_{nullptr};
|
|
std::shared_ptr<CanSender<msgs::RobotDetail>> waist_can_sender_{nullptr};
|
|
std::shared_ptr<CanSender<msgs::RobotDetail>> head_can_sender_{nullptr};
|
|
|
|
std::shared_ptr<MessageManager<msgs::RobotDetail>> l_message_manager_{nullptr};
|
|
std::shared_ptr<MessageManager<msgs::RobotDetail>> r_message_manager_{nullptr};
|
|
std::shared_ptr<MessageManager<msgs::RobotDetail>> waist_message_manager_{nullptr};
|
|
std::shared_ptr<MessageManager<msgs::RobotDetail>> head_message_manager_{nullptr};
|
|
|
|
bool waist_enabled_{false};
|
|
bool right_arm_enabled_{false};
|
|
bool left_arm_enabled_{false};
|
|
bool head_enabled_{false};
|
|
|
|
std::shared_ptr<MotorManager> motor_manager_{nullptr};
|
|
|
|
|
|
private:
|
|
std::shared_ptr<ControllerManager> controller_manager_{nullptr};
|
|
};
|
|
|
|
|
|
}
|
|
|
|
|
|
#endif //CMVR_ES_HUMANOID_ROBOT_H
|