refactor(devices): remove legacy robot stack and stale tests
This commit is contained in:
parent
7e803452e1
commit
32fe139015
@ -5,6 +5,5 @@ add_subdirectory(microphone)
|
|||||||
add_subdirectory(dexhand)
|
add_subdirectory(dexhand)
|
||||||
add_subdirectory(biohead)
|
add_subdirectory(biohead)
|
||||||
add_subdirectory(arm)
|
add_subdirectory(arm)
|
||||||
add_subdirectory(robot)
|
|
||||||
add_subdirectory(canbus)
|
add_subdirectory(canbus)
|
||||||
add_subdirectory(motor)
|
add_subdirectory(motor)
|
||||||
|
|||||||
@ -18,6 +18,8 @@ namespace cmvr::device {
|
|||||||
Microphone,
|
Microphone,
|
||||||
Motor,
|
Motor,
|
||||||
MotorSystem,
|
MotorSystem,
|
||||||
|
MujocoViewer,
|
||||||
|
MujocoWorld,
|
||||||
Robot,
|
Robot,
|
||||||
Speaker,
|
Speaker,
|
||||||
};
|
};
|
||||||
@ -46,6 +48,10 @@ namespace cmvr::device {
|
|||||||
return "Motor";
|
return "Motor";
|
||||||
case DeviceKind::MotorSystem:
|
case DeviceKind::MotorSystem:
|
||||||
return "MotorSystem";
|
return "MotorSystem";
|
||||||
|
case DeviceKind::MujocoViewer:
|
||||||
|
return "MujocoViewer";
|
||||||
|
case DeviceKind::MujocoWorld:
|
||||||
|
return "MujocoWorld";
|
||||||
case DeviceKind::Robot:
|
case DeviceKind::Robot:
|
||||||
return "Robot";
|
return "Robot";
|
||||||
case DeviceKind::Speaker:
|
case DeviceKind::Speaker:
|
||||||
|
|||||||
@ -12,17 +12,3 @@ target_link_libraries(px_6ax_gen3
|
|||||||
)
|
)
|
||||||
|
|
||||||
install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib)
|
install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib)
|
||||||
|
|
||||||
add_executable(px_6ax_gen3_test
|
|
||||||
src/px_6ax_gen3_test.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
target_link_libraries(px_6ax_gen3_test PRIVATE
|
|
||||||
cmvr_es::device::px_6ax_gen3
|
|
||||||
cmvr_es::common
|
|
||||||
cmvr_es::proto
|
|
||||||
glog
|
|
||||||
gtest
|
|
||||||
gtest_main
|
|
||||||
pthread
|
|
||||||
)
|
|
||||||
|
|||||||
@ -1,163 +0,0 @@
|
|||||||
#include "gtest/gtest.h"
|
|
||||||
|
|
||||||
#include "../include/px_6ax_gen3.h"
|
|
||||||
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
|
|
||||||
#include "common/config/config_files.h"
|
|
||||||
|
|
||||||
#include <chrono>
|
|
||||||
#include <cstdint>
|
|
||||||
#include <iostream>
|
|
||||||
#include <memory>
|
|
||||||
#include <stdexcept>
|
|
||||||
#include <string>
|
|
||||||
#include <thread>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
using DexHand = cmvr::device::AbstractDexHand;
|
|
||||||
using PX6AXGen3 = cmvr::device::PX6AXGen3;
|
|
||||||
|
|
||||||
DexHand::FingerType parseFingerType(const std::string& value) {
|
|
||||||
if (value == "PINKY") {
|
|
||||||
return DexHand::FingerType::PINKY;
|
|
||||||
}
|
|
||||||
if (value == "RING") {
|
|
||||||
return DexHand::FingerType::RING;
|
|
||||||
}
|
|
||||||
if (value == "MIDDLE" || value == "MIDDLE_FINGER") {
|
|
||||||
return DexHand::FingerType::MIDDLE;
|
|
||||||
}
|
|
||||||
if (value == "THUMB") {
|
|
||||||
return DexHand::FingerType::THUMB;
|
|
||||||
}
|
|
||||||
if (value == "PALM") {
|
|
||||||
return DexHand::FingerType::PALM;
|
|
||||||
}
|
|
||||||
return DexHand::FingerType::INDEX;
|
|
||||||
}
|
|
||||||
|
|
||||||
DexHand::TactileRegion parseTactileRegion(const std::string& value) {
|
|
||||||
if (value == "FINGER") {
|
|
||||||
return DexHand::TactileRegion::FINGER;
|
|
||||||
}
|
|
||||||
if (value == "PAD") {
|
|
||||||
return DexHand::TactileRegion::PAD;
|
|
||||||
}
|
|
||||||
if (value == "THUMB_MIDDLE") {
|
|
||||||
return DexHand::TactileRegion::THUMB_MIDDLE;
|
|
||||||
}
|
|
||||||
if (value == "PALM_PAD") {
|
|
||||||
return DexHand::TactileRegion::PALM_PAD;
|
|
||||||
}
|
|
||||||
return DexHand::TactileRegion::TIP;
|
|
||||||
}
|
|
||||||
|
|
||||||
struct StopGuard {
|
|
||||||
std::shared_ptr<PX6AXGen3> hand;
|
|
||||||
|
|
||||||
~StopGuard() {
|
|
||||||
if (!hand) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
try {
|
|
||||||
hand->stop();
|
|
||||||
} catch (...) {
|
|
||||||
}
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
DexHand::ResultantForce readFirstValidResultantForce(PX6AXGen3& hand,
|
|
||||||
const DexHand::FingerType finger,
|
|
||||||
const DexHand::TactileRegion region,
|
|
||||||
const int max_attempts,
|
|
||||||
const std::chrono::milliseconds retry_interval) {
|
|
||||||
std::string last_error;
|
|
||||||
|
|
||||||
for (int attempt = 0; attempt < max_attempts; ++attempt) {
|
|
||||||
try {
|
|
||||||
return hand.getResultantForce(finger, region);
|
|
||||||
} catch (const std::exception& ex) {
|
|
||||||
last_error = ex.what();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (attempt + 1 < max_attempts) {
|
|
||||||
std::this_thread::sleep_for(retry_interval);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
throw std::runtime_error("Failed to read PX6AXGen3 resultant force: " + last_error);
|
|
||||||
}
|
|
||||||
|
|
||||||
DexHand::ResultantForce readStateResultantForce(PX6AXGen3& hand) {
|
|
||||||
cmvr::device::DexHandState state;
|
|
||||||
hand.getState(state);
|
|
||||||
return DexHand::ResultantForce{0, 0, state.hands[0].force};
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
TEST(PX6AXGen3Test, PrintResultantForceOnly) {
|
|
||||||
cmvr::config::DexHandRootConfig root_config;
|
|
||||||
ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config));
|
|
||||||
const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr;
|
|
||||||
for (const auto& cfg : root_config.dexhand().dexhands()) {
|
|
||||||
if (cfg.id() == "paxini_tip_1" && cfg.has_px_6ax_gen3()) {
|
|
||||||
device_config_ptr = &cfg;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
ASSERT_NE(device_config_ptr, nullptr);
|
|
||||||
auto test_config = device_config_ptr->px_6ax_gen3();
|
|
||||||
test_config.set_id(device_config_ptr->id());
|
|
||||||
ASSERT_FALSE(test_config.serial_port().empty());
|
|
||||||
|
|
||||||
auto hand = std::make_shared<PX6AXGen3>(test_config);
|
|
||||||
ASSERT_NO_THROW(hand->init());
|
|
||||||
ASSERT_NO_THROW(hand->start());
|
|
||||||
StopGuard stop_guard{hand};
|
|
||||||
|
|
||||||
EXPECT_EQ(hand->state(), DexHand::Status::STREAMING);
|
|
||||||
|
|
||||||
const DexHand::FingerType finger = parseFingerType(test_config.tactile_finger());
|
|
||||||
const DexHand::TactileRegion region = parseTactileRegion(test_config.tactile_region());
|
|
||||||
const int warmup_ms = test_config.poll_interval_ms() > 0
|
|
||||||
? test_config.poll_interval_ms() * 5
|
|
||||||
: 200;
|
|
||||||
const int iterations = 1000000;
|
|
||||||
const int read_interval_ms = test_config.poll_interval_ms() > 0
|
|
||||||
? test_config.poll_interval_ms()
|
|
||||||
: 10;
|
|
||||||
if (warmup_ms > 0) {
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms));
|
|
||||||
}
|
|
||||||
|
|
||||||
bool read_once = false;
|
|
||||||
for (int iteration = 0; iteration < iterations; ++iteration) {
|
|
||||||
DexHand::ResultantForce resultant_force{};
|
|
||||||
ASSERT_NO_THROW(resultant_force = readFirstValidResultantForce(
|
|
||||||
*hand,
|
|
||||||
finger,
|
|
||||||
region,
|
|
||||||
10,
|
|
||||||
std::chrono::milliseconds(100)));
|
|
||||||
|
|
||||||
DexHand::ResultantForce state_force{};
|
|
||||||
ASSERT_NO_THROW(state_force = readStateResultantForce(*hand));
|
|
||||||
|
|
||||||
std::cout << "[PX6AXGen3Test] iter=" << (iteration + 1)
|
|
||||||
<< " resultant_fx=" << resultant_force.fx
|
|
||||||
<< " resultant_fy=" << resultant_force.fy
|
|
||||||
<< " resultant_fz=" << resultant_force.fz
|
|
||||||
<< " state_resultant_fz=" << state_force.fz
|
|
||||||
<< std::endl;
|
|
||||||
|
|
||||||
read_once = true;
|
|
||||||
|
|
||||||
if (read_interval_ms > 0 && iteration + 1 < iterations) {
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
EXPECT_TRUE(read_once);
|
|
||||||
}
|
|
||||||
@ -7,17 +7,3 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand)
|
|||||||
target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus)
|
target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus)
|
||||||
|
|
||||||
install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib)
|
install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib)
|
||||||
|
|
||||||
add_executable(rh56dftp_dexhand_test
|
|
||||||
src/rh56dftp_dexhand_test.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
target_link_libraries(rh56dftp_dexhand_test PRIVATE
|
|
||||||
cmvr_es::device::rh56dftp_dexhand
|
|
||||||
cmvr_es::common
|
|
||||||
cmvr_es::proto
|
|
||||||
glog
|
|
||||||
gtest
|
|
||||||
gtest_main
|
|
||||||
pthread
|
|
||||||
)
|
|
||||||
|
|||||||
@ -1,135 +0,0 @@
|
|||||||
#include "gtest/gtest.h"
|
|
||||||
|
|
||||||
#include "../include/rh56dftp_dexhand.h"
|
|
||||||
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
|
|
||||||
#include "common/config/config_files.h"
|
|
||||||
|
|
||||||
#include <chrono>
|
|
||||||
#include <iomanip>
|
|
||||||
#include <iostream>
|
|
||||||
#include <stdexcept>
|
|
||||||
#include <string>
|
|
||||||
#include <thread>
|
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
|
|
||||||
using DexHand = cmvr::device::AbstractDexHand;
|
|
||||||
using RH56DexHand = cmvr::device::RH56DFTPDexhand;
|
|
||||||
|
|
||||||
const char* tactileRegionToString(const DexHand::TactileRegion region) {
|
|
||||||
switch (region) {
|
|
||||||
case DexHand::TactileRegion::TIP: return "TIP";
|
|
||||||
case DexHand::TactileRegion::FINGER: return "FINGER";
|
|
||||||
case DexHand::TactileRegion::PAD: return "PAD";
|
|
||||||
case DexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE";
|
|
||||||
case DexHand::TactileRegion::PALM_PAD: return "PALM_PAD";
|
|
||||||
}
|
|
||||||
return "UNKNOWN";
|
|
||||||
}
|
|
||||||
|
|
||||||
struct StopGuard {
|
|
||||||
std::shared_ptr<RH56DexHand> hand;
|
|
||||||
|
|
||||||
~StopGuard() {
|
|
||||||
if (!hand) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
try {
|
|
||||||
hand->stop();
|
|
||||||
} catch (...) {
|
|
||||||
}
|
|
||||||
}
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) {
|
|
||||||
cmvr::config::DexHandRootConfig root_config;
|
|
||||||
ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config));
|
|
||||||
const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr;
|
|
||||||
for (const auto& cfg : root_config.dexhand().dexhands()) {
|
|
||||||
if (cfg.id() == "hand2" && cfg.has_rh56dftp()) {
|
|
||||||
device_config_ptr = &cfg;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
ASSERT_NE(device_config_ptr, nullptr);
|
|
||||||
auto hand_config = device_config_ptr->rh56dftp();
|
|
||||||
hand_config.set_id(device_config_ptr->id());
|
|
||||||
ASSERT_FALSE(hand_config.ip().empty());
|
|
||||||
|
|
||||||
const auto finger = DexHand::FingerType::RING;
|
|
||||||
const auto region = DexHand::TactileRegion::TIP;
|
|
||||||
const int iterations = 20000;
|
|
||||||
const int warmup_ms = 200;
|
|
||||||
const int read_interval_ms = 10;
|
|
||||||
|
|
||||||
auto hand = std::make_shared<RH56DexHand>(hand_config);
|
|
||||||
ASSERT_NO_THROW(hand->init());
|
|
||||||
ASSERT_NO_THROW(hand->start());
|
|
||||||
StopGuard stop_guard{hand};
|
|
||||||
|
|
||||||
EXPECT_EQ(hand->state(), DexHand::Status::STREAMING);
|
|
||||||
ASSERT_NO_THROW(hand->setTactilePollingRegion(finger, region));
|
|
||||||
|
|
||||||
if (warmup_ms > 0) {
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms));
|
|
||||||
}
|
|
||||||
|
|
||||||
double last_pressure_sum = 0.0;
|
|
||||||
double last_pressure_peak = 0.0;
|
|
||||||
int point_count = 0;
|
|
||||||
std::string sensor_name;
|
|
||||||
|
|
||||||
for (int i = 0; i < iterations; ++i) {
|
|
||||||
const auto region_data = hand->getSensorData(finger, region);
|
|
||||||
|
|
||||||
ASSERT_TRUE(region_data.valid());
|
|
||||||
EXPECT_EQ(region_data.finger, finger);
|
|
||||||
EXPECT_EQ(region_data.region, region);
|
|
||||||
|
|
||||||
if (point_count == 0) {
|
|
||||||
point_count = region_data.view.pointCount();
|
|
||||||
sensor_name = region_data.name == nullptr ? "" : region_data.name;
|
|
||||||
}
|
|
||||||
|
|
||||||
last_pressure_sum = 0.0;
|
|
||||||
last_pressure_peak = 0.0;
|
|
||||||
for (int index = 0; index < region_data.view.pointCount(); ++index) {
|
|
||||||
const double pressure = static_cast<double>(region_data.view.data[index].fz);
|
|
||||||
last_pressure_sum += pressure;
|
|
||||||
if (pressure > last_pressure_peak) {
|
|
||||||
last_pressure_peak = pressure;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::cout << std::fixed << std::setprecision(3)
|
|
||||||
<< "[RH56DFTPDexhandLatencyTest] iter=" << (i + 1)
|
|
||||||
<< "/" << iterations
|
|
||||||
<< " sensor=" << sensor_name
|
|
||||||
<< " pressure_sum=" << last_pressure_sum
|
|
||||||
<< " pressure_peak=" << last_pressure_peak
|
|
||||||
<< std::endl;
|
|
||||||
|
|
||||||
if (read_interval_ms > 0) {
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
std::cout << std::fixed << std::setprecision(3)
|
|
||||||
<< "[RH56DFTPDexhandLatencyTest] id=" << hand_config.id()
|
|
||||||
<< " ip=" << hand_config.ip()
|
|
||||||
<< " port=" << hand_config.port()
|
|
||||||
<< " finger=INDEX"
|
|
||||||
<< " region=" << tactileRegionToString(region)
|
|
||||||
<< " sensor=" << sensor_name
|
|
||||||
<< " iterations=" << iterations
|
|
||||||
<< " points=" << point_count
|
|
||||||
<< " last_pressure_sum=" << last_pressure_sum
|
|
||||||
<< " last_pressure_peak=" << last_pressure_peak
|
|
||||||
<< " read_interval_ms=" << read_interval_ms
|
|
||||||
<< "\n";
|
|
||||||
|
|
||||||
EXPECT_GT(point_count, 0);
|
|
||||||
}
|
|
||||||
@ -1,3 +0,0 @@
|
|||||||
#add_subdirectory(ti5_robot)
|
|
||||||
#add_subdirectory(c701)
|
|
||||||
#add_subdirectory(aubo_robot)
|
|
||||||
@ -1,170 +0,0 @@
|
|||||||
//
|
|
||||||
// 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
|
|
||||||
@ -1,35 +0,0 @@
|
|||||||
# 第一步：先定义 Aubo SDK 的路径（必须在 add_library 之前！）
|
|
||||||
if(CMAKE_SYSTEM_NAME STREQUAL "Linux")
|
|
||||||
set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include)
|
|
||||||
set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib)
|
|
||||||
elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows")
|
|
||||||
set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/win/include)
|
|
||||||
set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/win/lib)
|
|
||||||
endif()
|
|
||||||
|
|
||||||
# 第二步：创建 aubo_robot 库（此时 SDK 路径已经定义好了）
|
|
||||||
add_library(aubo_robot SHARED src/aubo_robot.cpp)
|
|
||||||
|
|
||||||
# 第三步：给 aubo_robot 配置头文件路径（合并你原来的 ${CMAKE_CURRENT_SOURCE_DIR} 和 SDK 的 include 目录）
|
|
||||||
target_include_directories(aubo_robot
|
|
||||||
PUBLIC
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR} # 你原来的路径（如果需要的话）
|
|
||||||
${AUBO_SDK_INCLUDE_DIR} # SDK 头文件路径（关键！）
|
|
||||||
)
|
|
||||||
|
|
||||||
# 第四步：给 aubo_robot 配置库文件路径（精准绑定到目标，比 link_directories 靠谱）
|
|
||||||
target_link_directories(aubo_robot
|
|
||||||
PRIVATE
|
|
||||||
${AUBO_SDK_LIB_DIR} # SDK 库文件路径（关键！）
|
|
||||||
)
|
|
||||||
|
|
||||||
# 第五步：链接 SDK 库（和你原来一致，但现在路径已经生效）
|
|
||||||
target_link_libraries(aubo_robot
|
|
||||||
PRIVATE
|
|
||||||
aubo_sdk
|
|
||||||
robot_proxy
|
|
||||||
cmvr_es::proto
|
|
||||||
)
|
|
||||||
|
|
||||||
# 别名（不变）
|
|
||||||
add_library(cmvr_es::device::aubo_robot ALIAS aubo_robot)
|
|
||||||
@ -1,51 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by linbo on 2025/11/7.
|
|
||||||
//
|
|
||||||
|
|
||||||
#ifndef CMVR_ES_AUBO_ROBOT_H
|
|
||||||
#define CMVR_ES_AUBO_ROBOT_H
|
|
||||||
#include "devices/robot/abstract_robot.h"
|
|
||||||
#include "aubo_sdk/rpc.h"
|
|
||||||
#include "cmvr/config/aubo_robot_config/aubo_robot_config.pb.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
using namespace arcs::common_interface;
|
|
||||||
using namespace arcs::aubo_sdk;
|
|
||||||
namespace cmvr::device
|
|
||||||
{
|
|
||||||
template<int DOF>
|
|
||||||
class AuboRobot: public AbstractRobot {
|
|
||||||
public:
|
|
||||||
explicit AuboRobot(const config::AuboRobotConfig& cfg);
|
|
||||||
~AuboRobot();
|
|
||||||
|
|
||||||
std::string typeName() const override { return "AuboRobot"; }
|
|
||||||
bool init () override;
|
|
||||||
|
|
||||||
void torqueOn() override;
|
|
||||||
void torqueOff() override;
|
|
||||||
|
|
||||||
void getJointsState(std::vector<JointState>& states) override;
|
|
||||||
|
|
||||||
void moveJ(std::vector<JointPoint>& cmd, double vel, double acc) override;
|
|
||||||
|
|
||||||
void moveL(math::Pose3d& pose, double vel, double acc) override;
|
|
||||||
|
|
||||||
void calibrateZeroQ(const std::string& joint_name) override;
|
|
||||||
cmvr::common::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,cmvr::common::Pose3d pose) override;
|
|
||||||
private:
|
|
||||||
static void waitForRobotMode(const RobotInterfacePtr& robot_interface,
|
|
||||||
const RobotModeType& target_mode);
|
|
||||||
static int waitArrival(const RobotInterfacePtr& impl);
|
|
||||||
private:
|
|
||||||
std::string ip_;
|
|
||||||
int port_;
|
|
||||||
std::string username_;
|
|
||||||
std::string password_;
|
|
||||||
std::shared_ptr<RpcClient> rpc_cli_;
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif //CMVR_ES_AUBO_ROBOT_H
|
|
||||||
@ -1,272 +0,0 @@
|
|||||||
#include "common/base/logging/logger.h"
|
|
||||||
//
|
|
||||||
// Created by linbo on 2025/11/7.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "devices/robot/aubo_robot/include/aubo_robot.h"
|
|
||||||
using namespace std;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
|
|
||||||
template<int DOF>
|
|
||||||
AuboRobot<DOF>::AuboRobot(const config::AuboRobotConfig &cfg)
|
|
||||||
{
|
|
||||||
try {
|
|
||||||
id_ = cfg.id();
|
|
||||||
ip_ = cfg.ip();
|
|
||||||
port_ = cfg.port() > 0 ? cfg.port() : 30004;
|
|
||||||
username_ = cfg.username();
|
|
||||||
password_ = cfg.password();
|
|
||||||
} catch (std::exception &e) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboRobot] Failed to parse config: " << e.what();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
template<int DOF>
|
|
||||||
AuboRobot<DOF>::~AuboRobot()
|
|
||||||
{
|
|
||||||
if (rpc_cli_)
|
|
||||||
{
|
|
||||||
// 接口调用: 退出登录
|
|
||||||
rpc_cli_->logout();
|
|
||||||
// 接口调用: 断开连接
|
|
||||||
rpc_cli_->disconnect();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
template<int DOF>
|
|
||||||
bool AuboRobot<DOF>::init ()
|
|
||||||
{
|
|
||||||
try {
|
|
||||||
//初始化AuboSDK
|
|
||||||
rpc_cli_ = std::make_shared<RpcClient>();
|
|
||||||
|
|
||||||
// 接口调用: 设置 RPC 超时
|
|
||||||
rpc_cli_->setRequestTimeout(1000);
|
|
||||||
// 接口调用: 连接到 RPC 服务
|
|
||||||
rpc_cli_->connect(ip_, port_);
|
|
||||||
// 接口调用: 登录
|
|
||||||
rpc_cli_->login(username_, password_);
|
|
||||||
return true;
|
|
||||||
} catch (const std::exception& e) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboRobot] init failed: " << e.what();
|
|
||||||
rpc_cli_.reset();
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
template<int DOF>
|
|
||||||
void AuboRobot<DOF>::waitForRobotMode(const RobotInterfacePtr& robot_interface,
|
|
||||||
const RobotModeType& target_mode)
|
|
||||||
{
|
|
||||||
// 接口调用: 获取当前机械臂的模式
|
|
||||||
auto current_mode = robot_interface->getRobotState()->getRobotModeType();
|
|
||||||
|
|
||||||
while (current_mode != target_mode) {
|
|
||||||
CMVR_LOG(INFO) << "机械臂当前模式:" << current_mode;
|
|
||||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
||||||
current_mode = robot_interface->getRobotState()->getRobotModeType();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
template<int DOF>
|
|
||||||
void AuboRobot<DOF>::torqueOn()
|
|
||||||
{
|
|
||||||
// 接口调用: 获取机器人的名字
|
|
||||||
auto robot_name = rpc_cli_->getRobotNames().front();
|
|
||||||
|
|
||||||
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
|
|
||||||
|
|
||||||
// 接口调用: 设置负载
|
|
||||||
double mass = 0.0;
|
|
||||||
std::vector<double> cog(3, 0.0);
|
|
||||||
std::vector<double> aom(3, 0.0);
|
|
||||||
std::vector<double> inertia(6, 0.0);
|
|
||||||
robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia);
|
|
||||||
|
|
||||||
// 接口调用: 获取机械臂当前模式
|
|
||||||
auto robot_mode = robot_interface->getRobotState()->getRobotModeType();
|
|
||||||
|
|
||||||
if (robot_mode == RobotModeType::Running) {
|
|
||||||
CMVR_LOG(INFO) << "机械臂已松刹车，处于运行模式";
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// 接口调用: 机械臂发起上电请求
|
|
||||||
robot_interface->getRobotManage()->poweron();
|
|
||||||
|
|
||||||
// 等待机械臂进入空闲模式
|
|
||||||
waitForRobotMode(robot_interface, RobotModeType::Idle);
|
|
||||||
|
|
||||||
CMVR_LOG(INFO) << "机械臂上电成功，当前模式:"
|
|
||||||
<< robot_interface->getRobotState()->getRobotModeType()
|
|
||||||
;
|
|
||||||
|
|
||||||
// 接口调用: 机械臂发起松刹车请求
|
|
||||||
rpc_cli_->getRobotInterface(robot_name)->getRobotManage()->startup();
|
|
||||||
|
|
||||||
// 等待机械臂进入运行模式
|
|
||||||
waitForRobotMode(robot_interface, RobotModeType::Running);
|
|
||||||
|
|
||||||
CMVR_LOG(INFO) << "机械臂松刹车成功，当前模式:"
|
|
||||||
<< robot_interface->getRobotState()->getRobotModeType()
|
|
||||||
;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
template<int DOF>
|
|
||||||
void AuboRobot<DOF>::getJointsState(std::vector<JointState>& states)
|
|
||||||
{
|
|
||||||
try {
|
|
||||||
// 接口调用: 获取机器人的名字
|
|
||||||
auto robot_name = rpc_cli_->getRobotNames().front();
|
|
||||||
|
|
||||||
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
|
|
||||||
auto rebotState = robot_interface->getRobotState();
|
|
||||||
states.clear();
|
|
||||||
JointState state;
|
|
||||||
//获取关节位置和速度
|
|
||||||
for (int i = 0; i < 6; i++) {
|
|
||||||
state.velocity = rebotState->getJointSpeeds()[i];
|
|
||||||
state.position = rebotState->getJointPositions()[i];
|
|
||||||
states.push_back(state);
|
|
||||||
}
|
|
||||||
} catch (exception &e) {
|
|
||||||
CMVR_LOG(ERROR) << "[AuboRobot] getJointsState failed: " << e.what();
|
|
||||||
states.clear();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
template<int DOF>
|
|
||||||
void AuboRobot<DOF>::torqueOff()
|
|
||||||
{
|
|
||||||
// 接口调用: 获取机器人的名字
|
|
||||||
auto robot_name = rpc_cli_->getRobotNames().front();
|
|
||||||
|
|
||||||
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
|
|
||||||
|
|
||||||
// 接口调用: 机械臂断电
|
|
||||||
robot_interface->getRobotManage()->poweroff();
|
|
||||||
|
|
||||||
// 等待机械臂进入断电模式
|
|
||||||
waitForRobotMode(robot_interface, RobotModeType::PowerOff);
|
|
||||||
|
|
||||||
CMVR_LOG(INFO) << "机械臂断电成功，当前模式:"
|
|
||||||
<< robot_interface->getRobotState()->getRobotModeType()
|
|
||||||
;
|
|
||||||
}
|
|
||||||
template<int DOF>
|
|
||||||
void AuboRobot<DOF>::moveJ(std::vector<JointPoint>& cmd, double vel, double acc)
|
|
||||||
{
|
|
||||||
try
|
|
||||||
{
|
|
||||||
//检查参数
|
|
||||||
if (cmd.size() != dof_)
|
|
||||||
{
|
|
||||||
CMVR_LOG(ERROR) << "[AuboRobot](moveJ)Wrong number of cmd";
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
//调用aubo sdk接口实现moveJ
|
|
||||||
|
|
||||||
// 接口调用: 获取机器人的名字
|
|
||||||
auto robot_name = rpc_cli_->getRobotNames().front();
|
|
||||||
|
|
||||||
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
|
|
||||||
|
|
||||||
// 接口调用: 设置机械臂的速度比率
|
|
||||||
robot_interface->getMotionControl()->setSpeedFraction(0.3);
|
|
||||||
// 接口调用: 关节运动
|
|
||||||
// 关节角，单位: 弧度 (此处为6关节)
|
|
||||||
std::vector<double> joint_angle;
|
|
||||||
for (int i = 0; i < cmd.size(); i++)
|
|
||||||
{
|
|
||||||
joint_angle.emplace_back(cmd[i].rad);
|
|
||||||
}
|
|
||||||
robot_interface->getMotionControl()->moveJoint(
|
|
||||||
joint_angle, acc, vel, 0, 0);
|
|
||||||
}
|
|
||||||
catch (std::exception& e)
|
|
||||||
{
|
|
||||||
CMVR_LOG(ERROR) << "[AuboRobot] moveJ failed: " << e.what();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
template<int DOF>
|
|
||||||
void AuboRobot<DOF>::calibrateZeroQ(const std::string& joint_name)
|
|
||||||
{
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
template<int DOF>
|
|
||||||
cmvr::common::Pose3d AuboRobot<DOF>::fk(const std::string &base_link, const std::string &ee_link)
|
|
||||||
{
|
|
||||||
cmvr::common::Pose3d pose;
|
|
||||||
|
|
||||||
|
|
||||||
return pose;
|
|
||||||
}
|
|
||||||
|
|
||||||
template<int DOF>
|
|
||||||
std::vector<double> AuboRobot<DOF>::ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose)
|
|
||||||
{
|
|
||||||
std::vector<double> ik;
|
|
||||||
|
|
||||||
|
|
||||||
return ik;
|
|
||||||
}
|
|
||||||
template<int DOF>
|
|
||||||
int AuboRobot<DOF>::waitArrival(const RobotInterfacePtr& impl) {
|
|
||||||
const int max_retry_count = 5;
|
|
||||||
int cnt = 0;
|
|
||||||
|
|
||||||
// 接口调用: 获取当前的运动指令 ID
|
|
||||||
int exec_id = impl->getMotionControl()->getExecId();
|
|
||||||
|
|
||||||
// 等待机械臂开始运动
|
|
||||||
while (exec_id == -1) {
|
|
||||||
if (cnt++ > max_retry_count) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
||||||
exec_id = impl->getMotionControl()->getExecId();
|
|
||||||
}
|
|
||||||
|
|
||||||
// 等待机械臂动作完成
|
|
||||||
while (impl->getMotionControl()->getExecId() != -1) {
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
template <int DOF>
|
|
||||||
void AuboRobot<DOF>::moveL(math::Pose3d& pose, double vel, double acc)
|
|
||||||
{
|
|
||||||
// 接口调用: 获取机器人的名字
|
|
||||||
auto robot_name = rpc_cli_->getRobotNames().front();
|
|
||||||
|
|
||||||
auto robot_interface = rpc_cli_->getRobotInterface(robot_name);
|
|
||||||
|
|
||||||
// 接口调用: 设置机械臂的速度比率
|
|
||||||
robot_interface->getMotionControl()->setSpeedFraction(0.75);
|
|
||||||
|
|
||||||
// 接口调用: 设置工具中心点（TCP相对于法兰盘中心的偏移）
|
|
||||||
std::vector<double> tcp_offset(6, 0.0);
|
|
||||||
robot_interface->getRobotConfig()->setTcpOffset(tcp_offset);
|
|
||||||
|
|
||||||
// 接口调用: 直线运动到位置
|
|
||||||
std::vector<double> pose1;
|
|
||||||
pose1.emplace_back(pose.position.x);
|
|
||||||
pose1.emplace_back(pose.position.y);
|
|
||||||
pose1.emplace_back(pose.position.z);
|
|
||||||
pose1.emplace_back(pose.euler.rx);
|
|
||||||
pose1.emplace_back(pose.euler.ry);
|
|
||||||
pose1.emplace_back(pose.euler.rz);
|
|
||||||
robot_interface->getMotionControl()->moveLine(pose1, acc, vel, 0.025, 0);
|
|
||||||
// 阻塞
|
|
||||||
auto ret = waitArrival(robot_interface);
|
|
||||||
if (ret == 0) {
|
|
||||||
CMVR_LOG(INFO)<<"直线运动到位置成功！";
|
|
||||||
} else {
|
|
||||||
CMVR_LOG(INFO)<<"直线运动到位置失败！";
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
template class cmvr::device::AuboRobot<6>;
|
|
||||||
@ -1,50 +0,0 @@
|
|||||||
add_library(c701 SHARED
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_sdo_response.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo1.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo2.cpp
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_rpdo1.cpp)
|
|
||||||
|
|
||||||
|
|
||||||
target_include_directories(c701 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
|
||||||
|
|
||||||
add_library(cmvr_es::robot::c701 ALIAS c701)
|
|
||||||
|
|
||||||
target_link_libraries(c701
|
|
||||||
PRIVATE
|
|
||||||
cmvr_es::device::canbus
|
|
||||||
protobuf
|
|
||||||
glog
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
# --------------------------------------------------------
|
|
||||||
# Unit test
|
|
||||||
# --------------------------------------------------------
|
|
||||||
include_directories(
|
|
||||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include
|
|
||||||
)
|
|
||||||
|
|
||||||
link_directories(
|
|
||||||
${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
add_executable(motor_controller_test
|
|
||||||
${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller_test.cpp
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
target_link_libraries(motor_controller_test
|
|
||||||
PRIVATE
|
|
||||||
cmvr_es::device::canbus
|
|
||||||
cmvr_es::robot::c701
|
|
||||||
c701
|
|
||||||
gtest
|
|
||||||
gtest_main
|
|
||||||
pthread
|
|
||||||
glog
|
|
||||||
cmvr_es::proto
|
|
||||||
)
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,406 +0,0 @@
|
|||||||
#include "common/base/logging/logger.h"
|
|
||||||
//
|
|
||||||
// Created by lgv on 2025/7/17.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "robot/c701/motor/motor_controller.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_sdo_response.h"
|
|
||||||
#include "canbus/canopen/register.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_tpdo1.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_tpdo2.h"
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
using namespace cmvr::robot::c701;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
|
|
||||||
ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool enable_log) {
|
|
||||||
if (!can_client->init()) {
|
|
||||||
CMVR_LOG(ERROR) << "[MotorController] CAN client init failed";
|
|
||||||
return ErrorCode::CAN_CLIENT_ERROR_BASE;
|
|
||||||
}
|
|
||||||
if (!can_client->start()) {
|
|
||||||
CMVR_LOG(ERROR) << "[MotorController] CAN client start failed";
|
|
||||||
return ErrorCode::CAN_CLIENT_ERROR_BASE;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
auto ret = ErrorCode::OK;
|
|
||||||
// 初始化 message_manager_
|
|
||||||
message_manager_ = std::make_shared<device::MessageManager<msgs::RobotDetail> >();
|
|
||||||
|
|
||||||
// NMT
|
|
||||||
message_manager_->AddSendProtocolData<NmtRequestProtocol<RobotDetail>, false>();
|
|
||||||
|
|
||||||
//sync
|
|
||||||
message_manager_->AddSendProtocolData<SyncProtocol<RobotDetail>, false>();
|
|
||||||
for (const auto node_id: node_ids_) {
|
|
||||||
//nmt
|
|
||||||
message_manager_->AddRecvProtocolData<NmtResponseProtocol<RobotDetail>, false>(node_id);
|
|
||||||
|
|
||||||
//sdo
|
|
||||||
message_manager_->AddSendProtocolData<SdoRequestProtocol<RobotDetail>, false>(node_id);
|
|
||||||
message_manager_->AddRecvProtocolData<Ti5MotorSdoResponse, false>(node_id);
|
|
||||||
|
|
||||||
//TPDO
|
|
||||||
message_manager_->AddRecvProtocolData<Ti5MotorTPDO1,false>(node_id);
|
|
||||||
message_manager_->AddRecvProtocolData<Ti5MotorTPDO2,false>(node_id);
|
|
||||||
|
|
||||||
//RPDO
|
|
||||||
message_manager_->AddSendProtocolData<Ti5MotorRPDO1,false>(node_id);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 初始化 sender
|
|
||||||
can_sender_ = std::make_shared<device::CanSender<msgs::RobotDetail> >();
|
|
||||||
ret = can_sender_->Init(can_client, enable_log);
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to init can sender.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// 初始化 receiver
|
|
||||||
can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
|
|
||||||
ret = can_receiver_->Init(can_client, message_manager_.get(), enable_log);
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to init can receiver.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// nmt
|
|
||||||
nmt_command_ = dynamic_cast<NmtRequestProtocol<RobotDetail> *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(NmtRequestProtocol<RobotDetail>::ID));
|
|
||||||
|
|
||||||
|
|
||||||
if (nmt_command_ == nullptr) {
|
|
||||||
CMVR_LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true);
|
|
||||||
|
|
||||||
// sync
|
|
||||||
sync_command_ = dynamic_cast<SyncProtocol<RobotDetail> *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(SyncProtocol<RobotDetail>::ID));
|
|
||||||
|
|
||||||
|
|
||||||
if (sync_command_ == nullptr) {
|
|
||||||
CMVR_LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
// can_sender_->AddMessage(sync_command_->ID, sync_command_, false);
|
|
||||||
|
|
||||||
|
|
||||||
// sdo
|
|
||||||
for (auto node_id: node_ids_) {
|
|
||||||
sdo_commands_[node_id] = dynamic_cast<SdoRequestProtocol<RobotDetail> *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(SdoRequestProtocol<RobotDetail>::ID(node_id)));
|
|
||||||
|
|
||||||
if (sdo_commands_[node_id] == nullptr) {
|
|
||||||
CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true);
|
|
||||||
}
|
|
||||||
|
|
||||||
// pdo1
|
|
||||||
for (auto node_id: node_ids_) {
|
|
||||||
rpdo1_commands_[node_id] = dynamic_cast<Ti5MotorRPDO1 *>(
|
|
||||||
message_manager_->GetMutableProtocolDataById(Ti5MotorRPDO1::ID(node_id)));
|
|
||||||
|
|
||||||
if (rpdo1_commands_[node_id] == nullptr) {
|
|
||||||
CMVR_LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
|
|
||||||
return ErrorCode::CANBUS_ERROR;
|
|
||||||
}
|
|
||||||
can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// need sleep to ensure all messages received
|
|
||||||
CMVR_LOG(INFO) << "Motor Controller is initialized.";
|
|
||||||
|
|
||||||
|
|
||||||
return ErrorCode::OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
ErrorCode MotorController::Start() {
|
|
||||||
auto ret = ErrorCode::OK;
|
|
||||||
ret = can_sender_->Start();
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to start can sender.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
ret = can_receiver_->Start();
|
|
||||||
if (ret != ErrorCode::OK) {
|
|
||||||
CMVR_LOG(ERROR) << "Failed to start can receiver.";
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::SetMode(uint8_t node_id, msgs::RunMode mode) {
|
|
||||||
|
|
||||||
cur_mode_ = mode;
|
|
||||||
// 1 : 先设置模式
|
|
||||||
auto data = static_cast<uint32_t>(mode);
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,OPERATION_MODE_6060,SUB_INDEX_0,data);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 2 : 状态机步进 —— Shutdown（0x06）
|
|
||||||
controlword_t cw = {};
|
|
||||||
cw.quick_stop = 1;
|
|
||||||
cw.enable_voltage = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20);
|
|
||||||
|
|
||||||
|
|
||||||
// 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
|
|
||||||
cw.switch_on = 1;
|
|
||||||
cw.enable_operation = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
switch (mode) {
|
|
||||||
case RUN_MODE_PROFILE_POSITION: {
|
|
||||||
|
|
||||||
// 4 : 设置目标位置（为当前位置）
|
|
||||||
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos);
|
|
||||||
|
|
||||||
|
|
||||||
// 5 : 触发位置运动（new_set_point 翻转）
|
|
||||||
cw.new_set_point = 1;
|
|
||||||
cw.change_set_immediately = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
|
|
||||||
// 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
|
|
||||||
cw.new_set_point = 0;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case RUN_MODE_CYCLIC_SYNC_POSITION: {
|
|
||||||
// 设置目标位置为当前位置
|
|
||||||
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos);
|
|
||||||
|
|
||||||
//3 : 使能 15
|
|
||||||
cw.enable_operation = 1;
|
|
||||||
cw.switch_on = 1;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CONTROL_WORD_6040,SUB_INDEX_0,cw.value);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
default:
|
|
||||||
// TODO: Handle unspecified or unknown mode
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::ConfigTPDO1(uint8_t node_id) {
|
|
||||||
//TDPO1 配置 状态字 和 控制字
|
|
||||||
// 1: 失能 pdo
|
|
||||||
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (1U << 31));
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
// 2: 配置为异步
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_COMM_1800,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC);
|
|
||||||
|
|
||||||
// 3：配置约束时间 unit:0.1ms
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_3,1);
|
|
||||||
|
|
||||||
// 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_5,1000);
|
|
||||||
|
|
||||||
// 5 ：映射控制字
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_1,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
//6 : 映射状态字
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_2,STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
//7 : 映射模式
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_3,MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
|
|
||||||
|
|
||||||
//8 映射错误码
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_4,ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
//9 写入该PDO映射对象总个数
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,4);
|
|
||||||
|
|
||||||
//10 使能
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (0U << 31));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::ConfigTPDO2(uint8_t node_id) {
|
|
||||||
// 1: 失能 pdo
|
|
||||||
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (1U << 31));
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
// 2: 配置为异步
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_COMM_1801,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC);
|
|
||||||
|
|
||||||
// 3：配置约束时间 unit:0.1ms
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_3,10000);
|
|
||||||
|
|
||||||
// 4 : 配置周期发送时间 unit : ms
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_5,1000);
|
|
||||||
|
|
||||||
// 5 ：映射当前位置
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_1,ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
|
|
||||||
|
|
||||||
//6 : 映射当前速度
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_2,ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
|
|
||||||
|
|
||||||
//9 写入该PDO映射对象总个数
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,2);
|
|
||||||
|
|
||||||
//10 使能
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (0U << 31));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::ConfigRPDO1(uint8_t node_id,bool start) {
|
|
||||||
|
|
||||||
// 1: 失能 pdo
|
|
||||||
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (1U << 31));
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
// 2: 配置为
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_COMM_1400,SUB_INDEX_2,SYNC_EVENT_DRIVEN);
|
|
||||||
|
|
||||||
// // 3：配置约束时间 unit:0.1ms
|
|
||||||
// SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
|
|
||||||
//
|
|
||||||
// // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
|
|
||||||
// SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
|
|
||||||
|
|
||||||
// 5 ：映射位置
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_1,TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
|
|
||||||
|
|
||||||
//6 : 映射控制字
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_2,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
|
|
||||||
|
|
||||||
|
|
||||||
if (start) {
|
|
||||||
//7 写入该PDO映射对象总个数
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,2);
|
|
||||||
|
|
||||||
//8 使能
|
|
||||||
SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (0U << 31));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SetPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
|
|
||||||
|
|
||||||
controlword_t cw = {};
|
|
||||||
cw.switch_on = 1;
|
|
||||||
cw.enable_voltage = 1;
|
|
||||||
cw.enable_operation = 1;
|
|
||||||
cw.quick_stop = 1;
|
|
||||||
cw.change_set_immediately = 1;
|
|
||||||
|
|
||||||
// 1. 设置目标位置
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
|
|
||||||
|
|
||||||
|
|
||||||
// 2. 设置触发位（bit4 = 1）
|
|
||||||
cw.new_set_point = 1;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
|
|
||||||
|
|
||||||
// 3. 清除触发位（bit4 = 0），准备下一次触发
|
|
||||||
cw.new_set_point = 0;
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SetPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
|
|
||||||
|
|
||||||
// 触发目标位置运动
|
|
||||||
controlword_t cw;
|
|
||||||
cw.value = 0x0F;
|
|
||||||
cw.new_set_point = 1;
|
|
||||||
cw.change_set_immediately = 1;
|
|
||||||
|
|
||||||
rpdo1_commands_[node_id]->SetTargetPos(pos);
|
|
||||||
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
|
|
||||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
||||||
|
|
||||||
cw.new_set_point = 0;
|
|
||||||
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
|
|
||||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::SetTargetPosition(uint8_t node_id, double angle_rad) {
|
|
||||||
|
|
||||||
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
|
|
||||||
|
|
||||||
switch (cur_mode_) {
|
|
||||||
case RUN_MODE_CYCLIC_SYNC_POSITION:
|
|
||||||
SetCSPTargetPosByPdo(node_id,static_cast<int32_t>(cmd));
|
|
||||||
break;
|
|
||||||
case RUN_MODE_PROFILE_POSITION:
|
|
||||||
SetPPTargetPosByPdo(node_id,static_cast<int32_t>(cmd));
|
|
||||||
// SetPPTargetPosBySdo(node_id,static_cast<int32_t>(cmd));
|
|
||||||
break;
|
|
||||||
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
|
|
||||||
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
|
|
||||||
SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SetCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
|
|
||||||
rpdo1_commands_[node_id]->SetTargetPos(pos);
|
|
||||||
rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
|
|
||||||
can_sender_->Update(rpdo1_commands_[node_id]->ID());
|
|
||||||
}
|
|
||||||
|
|
||||||
void MotorController::SeedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
|
|
||||||
nmt_command_->RequestService(node_id,command);
|
|
||||||
can_sender_->Update(nmt_command_->ID);
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void MotorController::SeedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms) {
|
|
||||||
sdo_commands_[node_id]->SetFrameData(cs,index,sub_index,data);
|
|
||||||
can_sender_->Update(sdo_commands_[node_id]->ID());
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,101 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/17.
|
|
||||||
//
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <utility>
|
|
||||||
|
|
||||||
#include "canbus/can_client/socket/socket_can_client_raw.h"
|
|
||||||
#include "can_comm/can_sender.h"
|
|
||||||
#include "canbus/can_comm/can_receiver.h"
|
|
||||||
#include "canbus/can_comm/can_sender.h"
|
|
||||||
#include "canbus/can_comm/message_manager.h"
|
|
||||||
#include "cmvr/msgs/error_code.pb.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
#include "canbus/canopen/sdo_request_protocol.h"
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h"
|
|
||||||
#include "canbus/canopen/sync_protocol.h"
|
|
||||||
|
|
||||||
#include "canbus/canopen/nmt_response_protocol.h"
|
|
||||||
#include "canopen/nmt_request_protocol.h"
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace c701 {
|
|
||||||
class MotorController {
|
|
||||||
public:
|
|
||||||
MotorController(std::vector<uint8_t> node_ids):node_ids_(node_ids) {}
|
|
||||||
|
|
||||||
MotorController(const MotorController &) = delete;
|
|
||||||
|
|
||||||
MotorController &operator=(const MotorController &) = delete;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Destructor.
|
|
||||||
*/
|
|
||||||
virtual ~MotorController() = default;
|
|
||||||
|
|
||||||
msgs::ErrorCode Init(cmvr::device::AbstractCanbus *can_client,bool enable_log);
|
|
||||||
msgs::ErrorCode Start();
|
|
||||||
|
|
||||||
std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
|
|
||||||
auto data_ptr = std::make_unique<msgs::RobotDetail>();
|
|
||||||
message_manager_->GetSensorData(data_ptr.get());
|
|
||||||
return data_ptr;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void SeedNmtRequest(uint8_t node_id,msgs::NmtCommand command,uint32_t delay_ms = 10);
|
|
||||||
|
|
||||||
void SeedSdoRequest(uint8_t node_id,msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data,uint32_t delay_ms = 10);
|
|
||||||
|
|
||||||
|
|
||||||
// 设置软件限位（上限和下限）
|
|
||||||
void SetPositionLimits(uint8_t node_id, int32_t lower_limit, int32_t upper_limit);
|
|
||||||
void ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
|
|
||||||
void ConfigTPDO1(uint8_t node_id);
|
|
||||||
void ConfigTPDO2(uint8_t node_id);
|
|
||||||
|
|
||||||
// 目标位置 607A + 控制字 6040
|
|
||||||
void ConfigRPDO1(uint8_t node_id,bool start);
|
|
||||||
void SetTargetPosition(uint8_t node_id,double angle_rad);
|
|
||||||
void SetMode(uint8_t node_id,msgs::RunMode mode);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
private:
|
|
||||||
static constexpr double GearRatio = 101.0; // 电机减速比
|
|
||||||
static constexpr double RADTODEG = 180.0 / 3.1415926;
|
|
||||||
std::vector<uint8_t> node_ids_{}; // 要控制的电机 id
|
|
||||||
// std::map<uint8_t,motor::Ti5MotorSdoRequestProtocol*> control_commands_{};
|
|
||||||
|
|
||||||
|
|
||||||
// nmt
|
|
||||||
device::NmtRequestProtocol<msgs::RobotDetail>* nmt_command_{nullptr};
|
|
||||||
|
|
||||||
//sync
|
|
||||||
device::SyncProtocol<msgs::RobotDetail>* sync_command_{nullptr};
|
|
||||||
|
|
||||||
// sdo
|
|
||||||
std::map<uint8_t,device::SdoRequestProtocol<msgs::RobotDetail>*> sdo_commands_{};
|
|
||||||
|
|
||||||
// rpdo1
|
|
||||||
std::map<uint8_t,robot::motor::Ti5MotorRPDO1*> rpdo1_commands_{};
|
|
||||||
|
|
||||||
|
|
||||||
std::shared_ptr<device::CanReceiver<msgs::RobotDetail>> can_receiver_{nullptr};
|
|
||||||
std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_{nullptr};
|
|
||||||
std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_{nullptr};
|
|
||||||
|
|
||||||
msgs::RunMode cur_mode_{msgs::RunMode::RUN_MODE_UNSPECIFIED};
|
|
||||||
|
|
||||||
|
|
||||||
void SetPPTargetPosBySdo(uint8_t node_id,int32_t pos);
|
|
||||||
void SetPPTargetPosByPdo(uint8_t node_id,int32_t pos);
|
|
||||||
void SetCSPTargetPosByPdo(uint8_t node_id,int32_t pos);
|
|
||||||
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,87 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/18.
|
|
||||||
//
|
|
||||||
|
|
||||||
|
|
||||||
#include <gtest/gtest.h>
|
|
||||||
|
|
||||||
#include "robot/c701/motor/motor_controller.h"
|
|
||||||
#include "canbus/can_comm/can_sender.h"
|
|
||||||
#include "canbus/can_comm/message_manager.h"
|
|
||||||
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
#include "canbus/canopen/sdo_request_protocol.h"
|
|
||||||
#include "canbus/can_client/socket/socket_can_client_raw.h"
|
|
||||||
// #include "cmvr/msgs/ti5_motor.pb.h"
|
|
||||||
|
|
||||||
using cmvr::msgs::RobotDetail;
|
|
||||||
using cmvr::msgs::ErrorCode;
|
|
||||||
using cmvr::device::CanSender;
|
|
||||||
using cmvr::device::MessageManager;
|
|
||||||
|
|
||||||
using cmvr::robot::c701::MotorController;
|
|
||||||
using cmvr::device::SdoRequestProtocol;
|
|
||||||
using cmvr::device::SocketCanClientRaw;
|
|
||||||
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
|
|
||||||
|
|
||||||
TEST(MotorControllerTest, MotorControllerCmdTest) {
|
|
||||||
|
|
||||||
|
|
||||||
std::vector<uint8_t> node_ids = {0x03};
|
|
||||||
MotorController controller(node_ids);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
cmvr::config::SocketCanConfig can_config;
|
|
||||||
can_config.set_channel_id(0);
|
|
||||||
SocketCanClientRaw can_client(can_config);
|
|
||||||
can_client.start();
|
|
||||||
|
|
||||||
|
|
||||||
auto result = controller.Init(&can_client, false);
|
|
||||||
controller.Start();
|
|
||||||
|
|
||||||
for (uint8_t node_id: node_ids) {
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
controller.ConfigTPDO1(node_id);
|
|
||||||
controller.ConfigTPDO2(node_id);
|
|
||||||
controller.ConfigRPDO1(node_id,true);
|
|
||||||
controller.ConfigProfile(node_id,1000,1000,1000);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
controller.SeedNmtRequest(node_id,NMT_ENTER_PRE_OPERATIONAL);
|
|
||||||
|
|
||||||
controller.SeedNmtRequest(node_id,NMT_START_REMOTE_NODE);
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
|
||||||
}
|
|
||||||
|
|
||||||
for (uint8_t node_id: node_ids) {
|
|
||||||
controller.SetMode(node_id,RUN_MODE_CYCLIC_SYNC_POSITION);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
controller.SetTargetPosition(node_id,3.14);
|
|
||||||
// controller.SyncStart();
|
|
||||||
// controller.SetTarget(node_id,3.14,1000);
|
|
||||||
}
|
|
||||||
|
|
||||||
while (true) {
|
|
||||||
|
|
||||||
|
|
||||||
// controller.SetNmtRequest(test_id,NMT_RESET_NODE);
|
|
||||||
// controller.SeedSdoRequest(test_id,CS_READ_REQUEST,ACTUAL_POSITION_6064,SUB_INDEX_0,0);
|
|
||||||
|
|
||||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
EXPECT_EQ(result, ErrorCode::CANBUS_ERROR);
|
|
||||||
}
|
|
||||||
@ -1,27 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/28.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h"
|
|
||||||
#include "common/base/logging/logger.h"
|
|
||||||
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
|
|
||||||
|
|
||||||
void Ti5MotorRPDO1::UpdateData(uint8_t *data) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
data[0] = target_pos_ & 0xFF;
|
|
||||||
data[1] = target_pos_ >> 8 & 0xFF;
|
|
||||||
data[2] = target_pos_ >> 16 & 0xFF;
|
|
||||||
data[3] = target_pos_ >> 24 & 0xFF;
|
|
||||||
|
|
||||||
data[4] = ctrl_word_ & 0xFF;
|
|
||||||
data[5] = ctrl_word_ >> 8 & 0xFF;
|
|
||||||
|
|
||||||
// data[4] = target_vel_ & 0xFF;
|
|
||||||
// data[5] = target_vel_ >> 8 & 0xFF;
|
|
||||||
// data[6] = target_vel_ >> 16 & 0xFF;
|
|
||||||
// data[7] = target_vel_ >> 24 & 0xFF;
|
|
||||||
}
|
|
||||||
@ -1,60 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/28.
|
|
||||||
//
|
|
||||||
#pragma once
|
|
||||||
#include "canbus/can_comm/protocol_data.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
#include <mutex>
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorRPDO1 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
static constexpr uint32_t BASE_ID = msgs::RPDO1_BASE_ID_200;
|
|
||||||
|
|
||||||
static uint32_t ID(uint8_t node_id) {
|
|
||||||
return BASE_ID + node_id;
|
|
||||||
}
|
|
||||||
uint32_t ID() const{
|
|
||||||
return BASE_ID + node_id_;
|
|
||||||
}
|
|
||||||
|
|
||||||
explicit Ti5MotorRPDO1(uint8_t node_id) : node_id_(node_id) {}
|
|
||||||
|
|
||||||
void UpdateData(uint8_t *data) override;
|
|
||||||
|
|
||||||
int32_t GetLength() const override {
|
|
||||||
return 0x06;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t GetPeriod() const override {
|
|
||||||
return 1000 * 1; // 5 ms
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetTargetPos(int32_t position) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
target_pos_ = position;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetTargetVel(int32_t velocity) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
target_vel_ = velocity;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetCtrlWord(uint16_t ctrl_word) {
|
|
||||||
std::lock_guard<std::mutex> lock(mutex_);
|
|
||||||
ctrl_word_ = ctrl_word;
|
|
||||||
}
|
|
||||||
|
|
||||||
private:
|
|
||||||
mutable std::mutex mutex_;
|
|
||||||
uint8_t node_id_{0};
|
|
||||||
int32_t target_pos_{0};
|
|
||||||
int32_t target_vel_{0};
|
|
||||||
|
|
||||||
uint16_t ctrl_word_{0};
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,40 +0,0 @@
|
|||||||
#include "common/base/logging/logger.h"
|
|
||||||
//
|
|
||||||
// Created by lgv on 2025/7/24.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "ti5_motor_sdo_response.h"
|
|
||||||
|
|
||||||
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
|
|
||||||
void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
|
|
||||||
cmvr::msgs::RobotDetail *sensor_data) const {
|
|
||||||
// 通过 node_id 获取对应的电机状态（Ti5_MotorStatus）
|
|
||||||
auto &motors_map = *sensor_data->mutable_motors();
|
|
||||||
auto *motor_status = &motors_map[this->node_id_];
|
|
||||||
|
|
||||||
// 先把基础的 sdo_response 拷贝进去
|
|
||||||
motor_status->mutable_sdo_response()->CopyFrom(sdo_response);
|
|
||||||
|
|
||||||
|
|
||||||
switch (sdo_response.index()) {
|
|
||||||
case msgs::CONTROL_WORD_6040:
|
|
||||||
motor_status->set_ctrl_word(sdo_response.data());
|
|
||||||
break;
|
|
||||||
case msgs::STATUS_WORD_6041:
|
|
||||||
motor_status->set_status_word(sdo_response.data());
|
|
||||||
break;
|
|
||||||
case msgs::ACTUAL_POSITION_6064:
|
|
||||||
motor_status->set_position(static_cast<int32_t>(sdo_response.data()));
|
|
||||||
CMVR_LOG(INFO) << "pos = " << motor_status->position();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// CMVR_LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_)
|
|
||||||
// << ": command=" << int(sdo_response.cs())
|
|
||||||
// << ", index=" << sdo_response.index()
|
|
||||||
// << ", subindex=" << int(sdo_response.sub_index())
|
|
||||||
// << ", data=" << sdo_response.data();
|
|
||||||
}
|
|
||||||
@ -1,26 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/24.
|
|
||||||
//
|
|
||||||
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "canbus/canopen/sdo_response_protocol.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorSdoResponse : public device::SdoResponseProtocol<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
explicit Ti5MotorSdoResponse(uint8_t node_id)
|
|
||||||
: SdoResponseProtocol<cmvr::msgs::RobotDetail>(node_id) {
|
|
||||||
}
|
|
||||||
|
|
||||||
protected:
|
|
||||||
void ParseSdoData(const msgs::SdoFrame &sdo_response,
|
|
||||||
cmvr::msgs::RobotDetail *sensor_data) const override;
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,36 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "ti5_motor_tpdo1.h"
|
|
||||||
#include "common/base/logging/logger.h"
|
|
||||||
#include "canbus/canopen/register.h"
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
|
|
||||||
|
|
||||||
void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const {
|
|
||||||
if (length < 7) {
|
|
||||||
CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto &motors_map = *sensor_data->mutable_motors();
|
|
||||||
auto *motor_status = &motors_map[this->node_id_];
|
|
||||||
|
|
||||||
motor_status->set_ctrl_word(bytes[1] << 8 | bytes[0]);
|
|
||||||
motor_status->set_status_word(bytes[3] << 8 | bytes[2]);
|
|
||||||
motor_status->set_run_mode(static_cast<RunMode>(bytes[4]));
|
|
||||||
motor_status->set_error_state(bytes[6] << 8 | bytes[5]);
|
|
||||||
|
|
||||||
statusword_t st{};
|
|
||||||
st.value = motor_status->status_word();
|
|
||||||
|
|
||||||
if (st.op_mode_specific > 0) {
|
|
||||||
CMVR_LOG(INFO) << st.op_mode_specific ;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,32 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "canbus/can_comm/protocol_data.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorTPDO1 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
static constexpr uint32_t BASE_ID = msgs::TPDO1_BASE_ID_180;
|
|
||||||
|
|
||||||
static uint32_t ID(uint8_t node_id) {
|
|
||||||
return BASE_ID + node_id;
|
|
||||||
}
|
|
||||||
uint32_t ID() const{
|
|
||||||
return BASE_ID + node_id_;
|
|
||||||
}
|
|
||||||
|
|
||||||
explicit Ti5MotorTPDO1(uint8_t node_id) : node_id_(node_id) {}
|
|
||||||
|
|
||||||
void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
uint8_t node_id_{0};
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@ -1,26 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
|
|
||||||
#include "ti5_motor_tpdo2.h"
|
|
||||||
#include "common/base/logging/logger.h"
|
|
||||||
|
|
||||||
using namespace cmvr::msgs;
|
|
||||||
using namespace cmvr::device;
|
|
||||||
using namespace cmvr::robot::motor;
|
|
||||||
|
|
||||||
|
|
||||||
void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const {
|
|
||||||
if (length < 8) {
|
|
||||||
CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length;
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
auto &motors_map = *sensor_data->mutable_motors();
|
|
||||||
auto *motor_status = &motors_map[this->node_id_];
|
|
||||||
|
|
||||||
motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]);
|
|
||||||
motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]);
|
|
||||||
|
|
||||||
CMVR_LOG(INFO) << "Motor ID " << this->node_id_ << "pos = " << motor_status->position() << " speed = " << motor_status->speed();
|
|
||||||
}
|
|
||||||
@ -1,32 +0,0 @@
|
|||||||
//
|
|
||||||
// Created by lgv on 2025/7/25.
|
|
||||||
//
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include "canbus/can_comm/protocol_data.h"
|
|
||||||
#include "cmvr/msgs/robot_detail.pb.h"
|
|
||||||
|
|
||||||
namespace cmvr {
|
|
||||||
namespace robot {
|
|
||||||
namespace motor {
|
|
||||||
class Ti5MotorTPDO2 : public device::ProtocolData<cmvr::msgs::RobotDetail> {
|
|
||||||
public:
|
|
||||||
static constexpr uint32_t BASE_ID = msgs::TPDO2_BASE_ID_280;
|
|
||||||
|
|
||||||
static uint32_t ID(uint8_t node_id) {
|
|
||||||
return BASE_ID + node_id;
|
|
||||||
}
|
|
||||||
uint32_t ID() const{
|
|
||||||
return BASE_ID + node_id_;
|
|
||||||
}
|
|
||||||
|
|
||||||
explicit Ti5MotorTPDO2(uint8_t node_id) : node_id_(node_id) {}
|
|
||||||
|
|
||||||
void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override;
|
|
||||||
|
|
||||||
private:
|
|
||||||
uint8_t node_id_{0};
|
|
||||||
};
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Loading…
Reference in New Issue
Block a user