577 lines
20 KiB
C++
577 lines
20 KiB
C++
|
//
|
||
|
|
// Created by xtkuang on 2025/7/24.
|
||
|
|
//
|
||
|
|
|
||
|
|
#include "humanoid_robot.h"
|
||
|
|
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
|
||
|
|
#include "motor/ti5_motor/ti5_motor.h"
|
||
|
|
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
using namespace cmvr::device;
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
|
||
|
|
try {
|
||
|
|
id_ = cfg.getAttrString("id");
|
||
|
|
dof_ = DOF;
|
||
|
|
if (!pathExists(cfg.getAttrString("urdf"))) {
|
||
|
|
throw runtime_error("urdf file does not exist");
|
||
|
|
}
|
||
|
|
auto rcfg = cmvr::dyn::LoadRobotFromURDF(
|
||
|
|
cfg.getAttrString("urdf"), cfg.getAttrString("baseLink"));
|
||
|
|
m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
|
||
|
|
joint_names_ = splitString(cfg.getAttrString("jointNames"), ",");
|
||
|
|
link_names_ = splitString(cfg.getAttrString("linkNames"), ",");
|
||
|
|
if (joint_names_.size() != dof_) {
|
||
|
|
throw runtime_error("joint names size mismatched with dof");
|
||
|
|
}
|
||
|
|
|
||
|
|
m_state_ = m_robot_->MakeState(link_names_, joint_names_);
|
||
|
|
m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
|
||
|
|
upd_freq_ = cfg.getAttrDefault("updFreq", 500);
|
||
|
|
CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(cfg.getAttrDefault("bufferSize", 50));
|
||
|
|
CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(cfg.getAttrDefault("bufferSize", 50));
|
||
|
|
CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));
|
||
|
|
|
||
|
|
|
||
|
|
auto can_cfg = cfg.getChild("CanManger");
|
||
|
|
|
||
|
|
auto l_can_cfg = can_cfg.getChild("LeftArmCan");
|
||
|
|
l_motors_cfg_ = l_can_cfg.getChildren("Motor");
|
||
|
|
l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
|
||
|
|
l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||
|
|
l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||
|
|
l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||
|
|
|
||
|
|
|
||
|
|
auto r_can_cfg = can_cfg.getChild("RightArmCan");
|
||
|
|
r_motors_cfg_ = r_can_cfg.getChildren("Motor");
|
||
|
|
r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
|
||
|
|
r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||
|
|
r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||
|
|
r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||
|
|
|
||
|
|
|
||
|
|
auto waist_can_cfg = can_cfg.getChild("WaistCan");
|
||
|
|
waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
|
||
|
|
waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
|
||
|
|
waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
|
||
|
|
waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
||
|
|
waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();
|
||
|
|
|
||
|
|
upd_timer_ = make_shared<FDTimer>();
|
||
|
|
upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
|
||
|
|
[this] { update_state_(); });
|
||
|
|
rsm_.store(ROBOT_READY);
|
||
|
|
} catch (exception &e) {
|
||
|
|
LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::init() {
|
||
|
|
// 1 === 初始化公共组件 ===
|
||
|
|
l_can_client_->init();
|
||
|
|
r_can_client_->init();
|
||
|
|
waist_can_client_->init();
|
||
|
|
auto ret = l_can_sender_->Init(l_can_client_.get(), false);
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to init can sender.";
|
||
|
|
}
|
||
|
|
ret = r_can_sender_->Init(r_can_client_.get(), false);
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to init can sender.";
|
||
|
|
}
|
||
|
|
ret = waist_can_sender_->Init(waist_can_client_.get(), false);
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to init can sender.";
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
ret = l_can_receiver_->Init(l_can_client_.get(), l_message_manager_.get(), false);
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to init can receiver.";
|
||
|
|
}
|
||
|
|
ret = r_can_receiver_->Init(r_can_client_.get(), r_message_manager_.get(), false);
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to init can receiver.";
|
||
|
|
}
|
||
|
|
ret = waist_can_receiver_->Init(waist_can_client_.get(), waist_message_manager_.get(), false);
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to init can receiver.";
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// 2 === 启动通讯 ===
|
||
|
|
l_can_client_->start();
|
||
|
|
ret = l_can_sender_->Start();
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to start can sender.";
|
||
|
|
}
|
||
|
|
|
||
|
|
r_can_client_->start();
|
||
|
|
ret = r_can_sender_->Start();
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to start can sender.";
|
||
|
|
}
|
||
|
|
|
||
|
|
waist_can_client_->start();
|
||
|
|
ret = waist_can_sender_->Start();
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to start can sender.";
|
||
|
|
}
|
||
|
|
|
||
|
|
ret = l_can_receiver_->Start();
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to start can receiver.";
|
||
|
|
}
|
||
|
|
|
||
|
|
ret = r_can_receiver_->Start();
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to start can receiver.";
|
||
|
|
}
|
||
|
|
|
||
|
|
ret = waist_can_receiver_->Start();
|
||
|
|
if (ret != ErrorCode::OK) {
|
||
|
|
LOG(ERROR) << "Failed to start can receiver.";
|
||
|
|
}
|
||
|
|
|
||
|
|
// 3 == 创建协议 ===
|
||
|
|
auto l_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(l_can_sender_, l_message_manager_);
|
||
|
|
auto r_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(r_can_sender_, r_message_manager_);
|
||
|
|
auto waist_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(waist_can_sender_, waist_message_manager_);
|
||
|
|
|
||
|
|
// 4 === 创建 MotorManager ===
|
||
|
|
motor_manager_ = std::make_shared<MotorManager>();
|
||
|
|
|
||
|
|
// for (const auto& cfg : r_motors_cfg_) {
|
||
|
|
// auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
|
|
// motor->setProtocol(r_canopen_protocol);
|
||
|
|
// motor->init(); // 耗时操作
|
||
|
|
// motor_manager_->addMotor(motor);
|
||
|
|
// }
|
||
|
|
//
|
||
|
|
// for (const auto& cfg : l_motors_cfg_) {
|
||
|
|
// auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
|
|
// motor->setProtocol(l_canopen_protocol);
|
||
|
|
// motor->init(); // 耗时操作
|
||
|
|
// motor_manager_->addMotor(motor);
|
||
|
|
// }
|
||
|
|
|
||
|
|
// 5 === 并行创建电机 ===
|
||
|
|
auto left_task = std::async(std::launch::async, [&] {
|
||
|
|
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing LEFT motors...";
|
||
|
|
for (const auto &cfg: l_motors_cfg_) {
|
||
|
|
auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
|
|
motor->setProtocol(l_canopen_protocol);
|
||
|
|
motor->init();
|
||
|
|
motor_manager_->addMotor(motor);
|
||
|
|
}
|
||
|
|
});
|
||
|
|
|
||
|
|
auto right_task = std::async(std::launch::async, [&] {
|
||
|
|
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing RIGHT motors...";
|
||
|
|
for (const auto &cfg: r_motors_cfg_) {
|
||
|
|
auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
|
|
motor->setProtocol(r_canopen_protocol);
|
||
|
|
motor->init();
|
||
|
|
motor_manager_->addMotor(motor);
|
||
|
|
}
|
||
|
|
});
|
||
|
|
|
||
|
|
auto waist_task = std::async(std::launch::async, [&] {
|
||
|
|
LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing waist motors...";
|
||
|
|
for (const auto &cfg: waist_motors_cfg_) {
|
||
|
|
auto motor = std::make_shared<Ti5Motor>(cfg);
|
||
|
|
motor->setProtocol(waist_canopen_protocol);
|
||
|
|
motor->init();
|
||
|
|
motor_manager_->addMotor(motor);
|
||
|
|
}
|
||
|
|
});
|
||
|
|
|
||
|
|
// 等待两个线程完成
|
||
|
|
left_task.get();
|
||
|
|
right_task.get();
|
||
|
|
waist_task.get();
|
||
|
|
|
||
|
|
rsm_.store(ROBOT_ESTOP);
|
||
|
|
LOG(INFO) << "All motors initialized successfully.";
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::torqueOff() {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||
|
|
throw runtime_error("robot is running");
|
||
|
|
}
|
||
|
|
|
||
|
|
if (rsm_.load() != ROBOT_TOROFF) {
|
||
|
|
for (const auto &pair: motor_manager_->motorsMap()) {
|
||
|
|
if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P" )
|
||
|
|
pair.second->torqueOff();
|
||
|
|
}
|
||
|
|
rsm_.store(ROBOT_TOROFF);
|
||
|
|
}
|
||
|
|
} catch (std::exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
HumanoidRobot<DOF>::~HumanoidRobot() {
|
||
|
|
// TODO: close can interfaces
|
||
|
|
upd_timer_->stop();
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
int HumanoidRobot<DOF>::getDOF() {
|
||
|
|
return dof_;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
std::vector<std::string> HumanoidRobot<DOF>::getJointNames() {
|
||
|
|
return joint_names_;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
std::unordered_map<std::string, double> HumanoidRobot<DOF>::getJointQ() const{
|
||
|
|
std::unordered_map<std::string, double> joint_qs;
|
||
|
|
|
||
|
|
for (const auto &pair : motor_manager_->motorsMap()) {
|
||
|
|
auto motor = pair.second;
|
||
|
|
joint_qs[motor->jointName()] = motor->getQ();
|
||
|
|
}
|
||
|
|
return joint_qs;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::getJointQ(std::unordered_map<std::string, double> &joint_qs) const {
|
||
|
|
for (auto &pair : joint_qs) {
|
||
|
|
auto motor = motor_manager_->getMotor(pair.first);
|
||
|
|
if (motor) {
|
||
|
|
pair.second = motor->getQ();
|
||
|
|
} else {
|
||
|
|
pair.second = 0.0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
std::vector<std::string> HumanoidRobot<DOF>::getLinkNames() {
|
||
|
|
return link_names_;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::getState(RobotState &state) {
|
||
|
|
try {
|
||
|
|
lock_guard lock(exec_mtx_);
|
||
|
|
// TODO: copy m_state_ date into state
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::torqueOn() {
|
||
|
|
eStop();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::eStop() {
|
||
|
|
if (rsm_.load() != ROBOT_ESTOP) {
|
||
|
|
CSP_buffer_->clear();
|
||
|
|
CSV_buffer_->clear();
|
||
|
|
CSC_buffer_->clear();
|
||
|
|
for (const auto &pair: motor_manager_->motorsMap()) {
|
||
|
|
pair.second->brake();
|
||
|
|
}
|
||
|
|
rsm_.store(ROBOT_ESTOP);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||
|
|
flash_cmd_.store(true);
|
||
|
|
eStop();
|
||
|
|
}
|
||
|
|
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
|
||
|
|
for (const auto &j: cmd) {
|
||
|
|
auto motor = motor_manager_->getMotor(j.joint_name);
|
||
|
|
if (motor != nullptr) {
|
||
|
|
// PPM 模式下 这个实际速度会超30% 左右
|
||
|
|
motor->setQd(vel);
|
||
|
|
|
||
|
|
if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
|
||
|
|
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
|
||
|
|
}
|
||
|
|
motor->setQ(j.rad);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
//3. wait for completion
|
||
|
|
bool completion = true;
|
||
|
|
do {
|
||
|
|
completion = true;
|
||
|
|
for (const auto &j: cmd) {
|
||
|
|
auto motor = motor_manager_->getMotor(j.joint_name);
|
||
|
|
if (motor != nullptr) {
|
||
|
|
if (!motor->reachedTargetQ()) {
|
||
|
|
completion = false;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
// 4. while waiting, check flash_cmd_, if it is true, set it false then exit
|
||
|
|
if (flash_cmd_.load()) {
|
||
|
|
flash_cmd_.store(false);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(2));
|
||
|
|
} while (!completion);
|
||
|
|
|
||
|
|
rsm_.store(ROBOT_ESTOP);
|
||
|
|
} else {
|
||
|
|
throw runtime_error("rsm invalid");
|
||
|
|
}
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
|
||
|
|
auto motor = motor_manager_->getMotor(joint_name);
|
||
|
|
motor->calibrateZeroQ();
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::moveJ_IK(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
|
||
|
|
double acc) {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||
|
|
flash_cmd_.store(true);
|
||
|
|
eStop();
|
||
|
|
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
Eigen::Vector<double, DOF> q_cmd;
|
||
|
|
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position,
|
||
|
|
q_cmd, 60, 1e-4);
|
||
|
|
if (!ok) {
|
||
|
|
throw runtime_error("solve IK failed");
|
||
|
|
}
|
||
|
|
// TODO: execute joint position command from q_cmd
|
||
|
|
// 1. set motor vel and acc
|
||
|
|
// 2. call motor PROFILE POSITION MODE (PPM) function
|
||
|
|
// 3. wait for completion
|
||
|
|
// 4. while waiting, check flash_cmd_, if it is true, set it false then exit
|
||
|
|
rsm_.store(ROBOT_READY);
|
||
|
|
} else {
|
||
|
|
throw runtime_error("rsm invalid");
|
||
|
|
}
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
|
||
|
|
double acc) {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||
|
|
flash_cmd_.store(true);
|
||
|
|
eStop();
|
||
|
|
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
// TODO:
|
||
|
|
// 1. interpolate line waypoint by vel and acc
|
||
|
|
// 2. for each waypoint, call cartesian controller to solve joint positions
|
||
|
|
// 3. for each waypoint, call motor Cyclic Synchronous Position (CSP) command with Timer
|
||
|
|
// 4. in the loop, check flash_cmd_, if it is true, set it false then exit
|
||
|
|
// Eigen::Vector<double, DOF> q_cmd;
|
||
|
|
// m_state_->SetQ(state_.joint_positions);
|
||
|
|
// bool ok = m_cctrl_.compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position, q_cmd, 60, 1e-4);
|
||
|
|
// if (!ok) {
|
||
|
|
// throw runtime_error("solve IK failed");
|
||
|
|
// }
|
||
|
|
rsm_.store(ROBOT_READY);
|
||
|
|
} else {
|
||
|
|
throw runtime_error("rsm invalid");
|
||
|
|
}
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||
|
|
flash_cmd_.store(true);
|
||
|
|
eStop();
|
||
|
|
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
// TODO:
|
||
|
|
// 1. set joint speed and acc
|
||
|
|
// 2. set joint speed by PROFILE VELOCITY MODE (PVM)
|
||
|
|
// rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is running
|
||
|
|
} else {
|
||
|
|
throw runtime_error("rsm invalid");
|
||
|
|
}
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||
|
|
flash_cmd_.store(true);
|
||
|
|
eStop();
|
||
|
|
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
// TODO: ???
|
||
|
|
// rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is running
|
||
|
|
} else {
|
||
|
|
throw runtime_error("rsm invalid");
|
||
|
|
}
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
|
||
|
|
auto ok = check_joint_traj_(traj, dt);
|
||
|
|
if (!ok) { throw runtime_error("joint traj invalid"); }
|
||
|
|
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
// TODO: need to optimize callback loop
|
||
|
|
for (auto i = 0; i < traj.size(); i++) {
|
||
|
|
if (flash_cmd_.load()) {
|
||
|
|
flash_cmd_.store(false);
|
||
|
|
LOG(INFO) << "followJointTrajectory is canceled";
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
servoJ(traj[i], dt);
|
||
|
|
this_thread::sleep_for(chrono::milliseconds((int) dt));
|
||
|
|
}
|
||
|
|
rsm_.store(ROBOT_ESTOP);
|
||
|
|
} else {
|
||
|
|
throw runtime_error("rsm invalid");
|
||
|
|
}
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
|
||
|
|
std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) {
|
||
|
|
try {
|
||
|
|
if (rsm_.load() == ROBOT_RUNNING) {
|
||
|
|
flash_cmd_.store(true);
|
||
|
|
eStop();
|
||
|
|
} else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
// TODO:
|
||
|
|
// 1. set Timer(dt)
|
||
|
|
// 2. for each timestamp, use Cyclic Synchronous Position (CSP) Mode to set joint position
|
||
|
|
// 3. if flash_cmd_ is set, set it to false and exit
|
||
|
|
// 3. join timer
|
||
|
|
rsm_.store(ROBOT_READY);
|
||
|
|
} else {
|
||
|
|
throw runtime_error("rsm invalid");
|
||
|
|
}
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
|
||
|
|
for (const auto &j: joints) {
|
||
|
|
auto motor = motor_manager_->getMotor(j.joint_name);
|
||
|
|
if (motor != nullptr) {
|
||
|
|
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
|
|
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
|
|
}
|
||
|
|
motor->setQd(j.vel);
|
||
|
|
motor->setQ(j.rad);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
rsm_.store(ROBOT_READY);
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, double dt) {
|
||
|
|
if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
|
||
|
|
rsm_.store(ROBOT_RUNNING);
|
||
|
|
for (const auto &j: joints) {
|
||
|
|
auto motor = motor_manager_->getMotor(j.joint_name);
|
||
|
|
if (motor != nullptr) {
|
||
|
|
motor->setQd(vel);
|
||
|
|
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
|
||
|
|
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
|
||
|
|
}
|
||
|
|
motor->setQ(j.rad);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
rsm_.store(ROBOT_READY);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) {
|
||
|
|
try {
|
||
|
|
Eigen::Vector<double, DOF> q_cmd;
|
||
|
|
bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position,
|
||
|
|
q_cmd, 60, 1e-4);
|
||
|
|
if (!ok) {
|
||
|
|
LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_;
|
||
|
|
throw runtime_error("IK failed");
|
||
|
|
}
|
||
|
|
std::vector<JointPoint> joints(dof_);
|
||
|
|
for (size_t i = 0; i < dof_; i++) {
|
||
|
|
joints[i].joint_name = joint_names_[i];
|
||
|
|
joints[i].rad = q_cmd[i];
|
||
|
|
}
|
||
|
|
servoJ(joints, dt);
|
||
|
|
} catch (exception &e) {
|
||
|
|
throw runtime_error(e.what());
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
bool HumanoidRobot<DOF>::check_joint_traj_(std::vector<std::vector<JointPoint> > &traj, double dt) {
|
||
|
|
// TODO: to be implemented
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
template<int DOF>
|
||
|
|
void HumanoidRobot<DOF>::update_state_() {
|
||
|
|
std::lock_guard lock(state_mtx_);
|
||
|
|
// TODO: set m_state_
|
||
|
|
// m_state_->SetQ();
|
||
|
|
// m_state_->SetQdot();
|
||
|
|
// m_state_->SetQddot();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
template class cmvr::device::HumanoidRobot<7>;
|
||
|
|
template class cmvr::device::HumanoidRobot<14>;
|
||
|
|
template class cmvr::device::HumanoidRobot<20>;
|