SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
tankaitao 2025-09-19 15:06:32 +08:00
parent 823e235ee0
commit 9a4f2ee6d4
7 changed files with 753 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
config/cabin_robot.xml
View File

@ -16,13 +16,13 @@
<!-- <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
<!-- <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" codec="H265"/>-->
</Camera>
<DexHand>
<RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000">
<Freedom order="01" default_force="500" default_speed="500" />
</RH56DFTP>
<!-- <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
<!-- <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="10.148.108.113" port="6000">-->
<!-- <Freedom order="01" default_force="500" default_speed="500" />-->
<!-- </RH56DFTP>-->
@ -37,8 +37,7 @@
baseLink="PELVIS_S"
jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S"
bufferSize="50"
verbose="false">
bufferSize="50">
<CanManger id="" devId="">
<LeftArmCan id = " " devId = " " channelId ="0">
<Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@ -47,7 +46,7 @@
<Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<!-- <Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<Motor id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1">
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@ -69,23 +68,31 @@
<BioHead>
<!-- <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">-->
<!-- &lt;!&ndash; 配置左眉毛，舵机通道 0~3 &ndash;&gt;-->
<!-- <EyeBrow serial="64:0~3" offest="90 90 90 90"-->
<!-- jLmtUp="170 170 170 170" jLmtLow="10 10 10 10"/>-->
<!-- &lt;!&ndash; 配置眼睛，舵机通道 4~9 &ndash;&gt;-->
<!-- <Eye serial="64:4~9" offest="90 90 90 90 90 90"-->
<!-- jLmtUp="170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10"/>-->
<!-- &lt;!&ndash; 眉毛 &ndash;&gt;-->
<!-- <EyeBrow serial="64:0~3"-->
<!-- offest="90 90 90 90"-->
<!-- jLmtUp="120 120 120 120"-->
<!-- jLmtLow="60 60 60 60"/>-->
<!-- &lt;!&ndash; 眼睛 &ndash;&gt;-->
<!-- <Eye serial="64:4~9"-->
<!-- offest="90 90 90 90 90 90"-->
<!-- jLmtUp="170 90 90 122 170 170"-->
<!-- jLmtLow="90 58 58 90 10 10"/>-->
<!-- &lt;!&ndash; 嘴巴 &ndash;&gt;-->
<!-- <Mouth serial="65:0~9"-->
<!-- offest="90 90 90 90 90 90 90 90 90 90"-->
<!-- jLmtUp="110 130 130 130 110 130 130 130 100 100"-->
<!-- jLmtLow="70 90 70 70 70 90 70 70 80 80"/>-->
<!-- &lt;!&ndash; 配置嘴巴，舵机通道 0~8 &ndash;&gt;-->
<!-- <Mouth serial="65:0~8" offest="90 90 90 90 90 90 90 90 90"-->
<!-- jLmtUp="170 170 170 170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10 10 10 10"/>-->
<!-- </esp32>-->
</BioHead >
<Microphone>
<!-- <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
<ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>
<!-- <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
</Microphone>
<Speaker>
@ -132,7 +139,7 @@
</MonitorManager>
<gRPCServer port="50055">
<gRPCServer port="50051">
</gRPCServer>

14
include/devices/state_define.h
View File

@ -41,9 +41,21 @@ namespace cmvr::device{
// ------------------------------------- robot -------------------------------------
typedef enum {
FORWARD, BACKWARD
FORWARD, BACKWARD,
X_POSITIVE, // X轴正向
X_NEGATIVE, // X轴负向
Y_POSITIVE, // Y轴正向
Y_NEGATIVE, // Y轴负向
Z_POSITIVE, // Z轴正向
Z_NEGATIVE, // Z轴负向
// 角度方向也可以添加
ROTATE_X, // 绕X轴旋转
ROTATE_Y, // 绕Y轴旋转
ROTATE_Z // 绕Z轴旋转
} RobotJointIndexDirection;
/// robot cartesian index
typedef enum {
X, Y, Z, RX, RY, RZ

17
src/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp
View File

@ -206,7 +206,12 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
break;
}
case RUN_MODE_PROFILE_VELOCITY: {
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;
@ -419,18 +424,22 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
}
void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) {
uint32_t speed = ((std::abs(qd) * RADTODEG) * GearRatio * 100.0) / 360.0;
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
switch (cur_mode_[node_id]) {
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
case msgs::RUN_MODE_PROFILE_POSITION: {
auto it = last_Qd_.find(node_id);
if (it == last_Qd_.end() || it->second != speed) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed,0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),0);
last_Qd_[node_id] = speed;
}
break;
}
case msgs::RUN_MODE_PROFILE_VELOCITY: {
// 在速度模式下，直接设置目标速度
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
break;
}
default:
break;
}

566
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -1006,38 +1006,546 @@ void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirectio
if (rsm_.load() == ROBOT_RUNNING) {
flash_cmd_.store(true);
eStop();
LOG(ERROR) << "机器人正在运行，停止当前运动";
} 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
// 获取电机控制对象
// 这里的控制函数需要根据你的实际实现来进行填充
// 获取目标关节的电机
auto motor = motor_manager_->getMotor(joint_name);
if (motor == nullptr) {
throw runtime_error("Motor not found for joint: " + joint_name);
}
// 设置电机的运行模式为速度模式
if (motor->getMode() != msgs::RUN_MODE_PROFILE_VELOCITY) {
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
}
// 设置加速度和目标速度
motor->setQd(vel);
LOG(INFO) << "开始在关节 " << joint_name << " 上进行速度控制，速度：" << vel << " rad/s，加速度：" << acc << " rad/s²";
std::this_thread::sleep_for(std::chrono::seconds(10));
motor->setQd(0);
LOG(INFO) << "速度控制完成，电机已停止。";
// 运动完成后不立即将 rsm_ 置为 READY，防止误操作
} else {
throw runtime_error("rsm invalid");
throw runtime_error("无效的机器人状态，无法进行速度控制");
}
} catch (exception &e) {
throw runtime_error(e.what());
rsm_.store(ROBOT_ESTOP); // 出错时，设置为紧急停止状态
LOG(ERROR) << "speedJ 控制失败: " << e.what();
throw runtime_error("speedJ 控制失败: " + string(e.what()));
}
}
// template<int DOF>
// void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) {
// if (vel <= 0 || acc <= 0) {
// throw std::runtime_error("speedL: vel and acc must be positive");
// }
// rsm_.store(ROBOT_RUNNING);
// try {
// const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变
//
// // 定义基座和末端执行器链接
// std::string base_link = "PELVIS_S";
// std::string ee_link = "R_WRIST_R_S";
//
// // 获取当前末端执行器位姿
// msgs::Pose3d current_pose = fk(base_link, ee_link);
//
// // 初始化当前位姿矩阵
// Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity();
// T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
// current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz()
// );
// T_current(0, 3) = current_pose.position().x();
// T_current(1, 3) = current_pose.position().y();
// T_current(2, 3) = current_pose.position().z();
//
// // 获取目标笛卡尔速度方向
// Eigen::Vector3d direction = Eigen::Vector3d::Zero();
//
// // 根据方向设置笛卡尔速度
// if (cart == RobotCartesian::X) {
// direction.x() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0;
// } else if (cart == RobotCartesian::Y) {
// direction.y() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0;
// } else if (cart == RobotCartesian::Z) {
// direction.z() = (dir == RobotJointIndexDirection::FORWARD) ? 1.0 : -1.0;
// }
//
// // 规范化方向向量
// direction.normalize();
//
// // 计算目标位姿（基于当前位姿和方向）
// Eigen::Matrix4d T_target = T_current;
// T_target(0, 3) += direction.x() * vel * CONTROL_PERIOD; // 小位移
// T_target(1, 3) += direction.y() * vel * CONTROL_PERIOD;
// T_target(2, 3) += direction.z() * vel * CONTROL_PERIOD;
//
// // 创建目标位姿
// cmvr::ctrl::PoseTarget target;
// target.T_target = T_target;
// target.link_name = ee_link;
// target.w_posrot = 0.5; // 位置和旋转权重
// target.weight = 1.0;
//
// // 创建队列用于存储关节速度
// std::queue<Eigen::Vector<double, DOF>> velocity_queue;
// std::mutex queue_mutex;
// std::condition_variable queue_cv;
// std::atomic<bool> ik_completed{false};
// std::atomic<bool> stop_requested{false};
//
// // IK计算线程（使用CartesianController的Velocity模式）
// std::thread ik_thread([&]() {
// try {
// // 获取当前关节位置
// auto q_map_current = getJointQ();
// Eigen::Vector<double, DOF> current_joint_positions;
// for (int i = 0; i < DOF; ++i) {
// current_joint_positions[i] = q_map_current[joint_names_[i]];
// }
//
// // 设置初始状态
// m_state_->SetQ(current_joint_positions);
// m_robot_->ComputeForwardKinematics(m_state_);
//
// LOG(INFO) << "Current robot state: " << rsm_.load();
// LOG(INFO) << "stop_requested: " << stop_requested.load();
//
// // IK计算循环
// while (rsm_.load() == ROBOT_RUNNING ) {
// // 使用CartesianController计算关节速度
// Eigen::Vector<double, DOF> joint_velocities;
// bool ok = m_cctrl_->compute(m_state_, base_link, {target}, CONTROL_PERIOD,
// ctrl::CartesianController<DOF>::Mode::Velocity,
// joint_velocities, 10000, 1e-6);
//
// if (!ok) {
// LOG(WARNING) << "IK计算失败，使用上一个有效速度";
// // 可以使用上一个有效的速度或者零速度
// continue;
// }
//
// LOG(INFO) << "IK计算得到的关节速度：";
// for (int i = 0; i < DOF; ++i) {
// LOG(INFO) << "关节 " << joint_names_[i] << " 速度: " << joint_velocities[i];
// }
//
// // 将关节速度加入队列
// {
// std::lock_guard<std::mutex> lock(queue_mutex);
// velocity_queue.push(joint_velocities);
// }
// queue_cv.notify_one();
//
// // 更新目标位姿（基于当前位姿和方向）
// msgs::Pose3d current_pose = fk(base_link, ee_link);
// T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
// current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz()
// );
// T_current(0, 3) = current_pose.position().x();
// T_current(1, 3) = current_pose.position().y();
// T_current(2, 3) = current_pose.position().z();
//
// T_target = T_current;
// T_target(0, 3) += direction.x() * vel * CONTROL_PERIOD;
// T_target(1, 3) += direction.y() * vel * CONTROL_PERIOD;
// T_target(2, 3) += direction.z() * vel * CONTROL_PERIOD;
//
// target.T_target = T_target;
//
// // 等待控制周期
// std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(CONTROL_PERIOD * 1000)));
// }
//
// ik_completed.store(true);
// queue_cv.notify_one();
//
// } catch (const std::exception& e) {
// LOG(ERROR) << "IK thread error: " << e.what();
// ik_completed.store(true);
// queue_cv.notify_one();
// }
// });
//
// // 控制执行线程（周期同步速度模式）
// std::thread control_thread([&]() {
// try {
// // 设置所有电机为周期同步速度模式
// for (int i = 0; i < DOF; ++i) {
// auto motor = motor_manager_->getMotor(joint_names_[i]);
// if (motor != nullptr) {
// if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
// motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
// }
// }
// }
//
// // 控制循环
// while (rsm_.load() == ROBOT_RUNNING && !stop_requested.load()) {
// Eigen::Vector<double, DOF> joint_velocities;
// bool has_velocity = false;
//
// // 从队列中获取关节速度
// {
// std::unique_lock<std::mutex> lock(queue_mutex);
// if (queue_cv.wait_for(lock, std::chrono::milliseconds(100),
// [&] { return !velocity_queue.empty() || ik_completed.load(); })) {
// if (!velocity_queue.empty()) {
// joint_velocities = velocity_queue.front();
// velocity_queue.pop();
// has_velocity = true;
// }
// }
// }
//
// if (has_velocity) {
// // 发送关节速度命令
// for (int j = 0; j < DOF; ++j) {
// auto motor = motor_manager_->getMotor(joint_names_[j]);
// // if (motor != nullptr) {
// // motor->setQd(joint_velocities[j]);
// // }
// }
// }
//
// // 检查是否完成
// {
// std::lock_guard<std::mutex> lock(queue_mutex);
// if (ik_completed.load() && velocity_queue.empty()) {
// break;
// }
// }
//
// // 控制时间节奏
// std::this_thread::sleep_for(std::chrono::milliseconds(10));
// }
//
// // 停止所有关节
// for (int i = 0; i < DOF; ++i) {
// auto motor = motor_manager_->getMotor(joint_names_[i]);
// if (motor != nullptr) {
// motor->setQd(0);
// }
// }
//
// } catch (const std::exception& e) {
// LOG(ERROR) << "Control thread error: " << e.what();
// }
// });
//
// LOG(INFO) << "开始在笛卡尔空间进行速度控制，方向：" << dir << "，速度：" << vel;
//
// // 等待停止命令
// while (rsm_.load() == ROBOT_RUNNING && !flash_cmd_.load()) {
// std::this_thread::sleep_for(std::chrono::milliseconds(100));
// }
//
// // 通知线程停止
// stop_requested.store(true);
// {
// std::lock_guard<std::mutex> lock(queue_mutex);
// ik_completed.store(true);
// }
// queue_cv.notify_all();
//
// // 等待线程结束
// if (ik_thread.joinable()) {
// ik_thread.join();
// }
// if (control_thread.joinable()) {
// control_thread.join();
// }
//
// LOG(INFO) << "speedL速度控制完成，所有电机已停止。";
//
// } catch (const std::exception &e) {
// LOG(ERROR) << "speedL failed: " << e.what();
// rsm_.store(ROBOT_ERROR);
// throw std::runtime_error(std::string("speedL 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");
if (vel <= 0 || acc <= 0) {
throw std::runtime_error("speedL: vel and acc must be positive");
}
} catch (exception &e) {
throw runtime_error(e.what());
try {
const double CONTROL_PERIOD = 1.0 / 100; // 控制周期保持不变
// 定义基座和末端执行器链接
std::string base_link = "PELVIS_S";
std::string ee_link = "R_WRIST_R_S";
msgs::Pose3d current_pose = fk(base_link, ee_link);
// 初始化当前位姿矩阵
Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity();
T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz()
);
T_current(0, 3) = current_pose.position().x();
T_current(1, 3) = current_pose.position().y();
T_current(2, 3) = current_pose.position().z();
// 获取目标笛卡尔速度方向
Eigen::Vector3d direction = Eigen::Vector3d::Zero();
bool is_rotation = false;
// 根据方向设置笛卡尔速度
switch (dir) {
case RobotJointIndexDirection::X_POSITIVE:
direction.x() = 1.0;
break;
case RobotJointIndexDirection::X_NEGATIVE:
direction.x() = -1.0;
break;
case RobotJointIndexDirection::Y_POSITIVE:
direction.y() = 1.0;
break;
case RobotJointIndexDirection::Y_NEGATIVE:
direction.y() = -1.0;
break;
case RobotJointIndexDirection::Z_POSITIVE:
direction.z() = 1.0;
break;
case RobotJointIndexDirection::Z_NEGATIVE:
direction.z() = -1.0;
break;
case RobotJointIndexDirection::ROTATE_X:
direction.x() = 1.0;
is_rotation = true;
break;
case RobotJointIndexDirection::ROTATE_Y:
direction.y() = 1.0;
is_rotation = true;
break;
case RobotJointIndexDirection::ROTATE_Z:
direction.z() = 1.0;
is_rotation = true;
break;
case RobotJointIndexDirection::FORWARD:
// 保持向后兼容
if (cart == RobotCartesian::X) direction.x() = 1.0;
else if (cart == RobotCartesian::Y) direction.y() = 1.0;
else if (cart == RobotCartesian::Z) direction.z() = 1.0;
break;
case RobotJointIndexDirection::BACKWARD:
// 保持向后兼容
if (cart == RobotCartesian::X) direction.x() = -1.0;
else if (cart == RobotCartesian::Y) direction.y() = -1.0;
else if (cart == RobotCartesian::Z) direction.z() = -1.0;
break;
default:
throw std::runtime_error("speedL: unknown direction");
}
// 计算末端执行器的总运动时间
double move_time = calculateMoveTime(direction.norm(), vel, acc);
// 使用S曲线速度规划
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
std::vector<double> time_points;
std::vector<double> distance_ratios;
generateSTrapezoidalProfile(direction.norm(), vel, acc, move_time, num_points,
time_points, distance_ratios);
LOG(INFO) << "speedL: Planning trajectory - points=" << num_points
<< ", total distance=" << direction.norm() << "m, move time=" << move_time << "s";
// 生成轨迹点
std::vector<Eigen::Matrix4d> cartesian_trajectory;
for (size_t i = 0; i <= num_points; ++i) {
double s = distance_ratios[i]; // 使用S曲线规划的距离比例
Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity();
if (is_rotation) {
// 旋转运动：保持位置不变，旋转姿态
T_interp.block<3, 1>(0, 3) = T_current.block<3, 1>(0, 3); // 位置不变
// 计算旋转矩阵
double angle = s * direction.norm(); // 旋转角度
Eigen::AngleAxisd rotation(angle, direction.normalized());
Eigen::Matrix3d R_current = T_current.block<3, 3>(0, 0);
Eigen::Matrix3d R_interp = rotation * R_current;
T_interp.block<3, 3>(0, 0) = R_interp;
} else {
// 平移运动：保持姿态不变，移动位置
T_interp.block<3, 3>(0, 0) = T_current.block<3, 3>(0, 0); // 保持姿态不变
T_interp(0, 3) = T_current(0, 3) + s * direction.x();
T_interp(1, 3) = T_current(1, 3) + s * direction.y();
T_interp(2, 3) = T_current(2, 3) + s * direction.z();
}
cartesian_trajectory.push_back(T_interp);
}
// 预先计算所有关节位置和速度
std::vector<Eigen::Vector<double, DOF>> joint_positions;
std::vector<Eigen::Vector<double, DOF>> joint_velocities;
// 获取当前关节位置
auto q_map_current = getJointQ();
Eigen::Vector<double, DOF> q_current;
// 根据你的关节名称填充q_current
for (int i = 0; i < DOF; ++i) {
q_current[i] = q_map_current[joint_names_[i]];
}
joint_positions.push_back(q_current);
joint_velocities.push_back(Eigen::Vector<double, DOF>::Zero()); // 起始速度为零
// 预先计算所有关节位置
for (size_t i = 1; i < cartesian_trajectory.size(); ++i) {
const auto& T_interp = cartesian_trajectory[i];
// 构造当前目标
cmvr::ctrl::PoseTarget current_target;
current_target.T_target = T_interp;
current_target.link_name = ee_link;
current_target.w_posrot = 0.5;
current_target.weight = 1.0;
// 使用前一点的位置作为初始值求解IK
Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_next, 10000, 1e-6);
if (!ok) {
LOG(WARNING) << "IK计算失败，使用上一个有效点";
q_next = joint_positions.back();
}
LOG(INFO) << "IK计算结果 (q_next): " << q_next.transpose();
joint_positions.push_back(q_next);
}
// 计算每个点的关节速度
for (size_t i = 1; i < joint_positions.size(); ++i) {
double dt = time_points[i] - time_points[i-1];
Eigen::Vector<double, DOF> vel = (joint_positions[i] - joint_positions[i-1]) / dt;
joint_velocities.push_back(vel);
}
// 创建队列用于存储轨迹点
std::queue<std::pair<Eigen::Vector<double, DOF>, Eigen::Vector<double, DOF>>> trajectory_queue;
std::mutex queue_mutex;
std::condition_variable queue_cv;
std::atomic<bool> trajectory_completed{false};
// 轨迹点计算线程
std::thread trajectory_thread([&]() {
try {
// 计算并将轨迹点放入队列
for (size_t i = 0; i < joint_positions.size(); ++i) {
{
std::lock_guard<std::mutex> lock(queue_mutex);
trajectory_queue.push({joint_positions[i], joint_velocities[i]});
}
queue_cv.notify_one(); // 通知控制线程
// 控制节奏
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(CONTROL_PERIOD * 1000)));
}
trajectory_completed.store(true);
queue_cv.notify_one();
} catch (const std::exception &e) {
LOG(ERROR) << "Trajectory thread error: " << e.what();
trajectory_completed.store(true);
queue_cv.notify_one();
}
});
// 控制执行线程
std::thread control_thread([&]() {
try {
while (!trajectory_completed.load() || !trajectory_queue.empty()) {
// 从队列中获取轨迹点
std::pair<Eigen::Vector<double, DOF>, Eigen::Vector<double, DOF>> point;
bool has_point = false;
{
std::unique_lock<std::mutex> lock(queue_mutex);
if (queue_cv.wait_for(lock, std::chrono::milliseconds(100),
[&] { return !trajectory_queue.empty() || trajectory_completed.load(); })) {
if (!trajectory_queue.empty()) {
point = trajectory_queue.front();
trajectory_queue.pop();
has_point = true;
}
}
}
if (has_point) {
// 更新关节命令
m_state_->SetQ(point.first);
m_robot_->ComputeForwardKinematics(m_state_);
// 发送关节命令 - 为每个电机单独设置位置和速度
for (int j = 0; j < DOF; ++j) {
auto motor = motor_manager_->getMotor(joint_names_[j]);
if (motor != nullptr) {
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
motor->setQd(point.second[j]);
motor->setQ(point.first[j]);
// 打印发送的关节命令
LOG(INFO) << "Sending setQ: joint[" << joint_names_[j] << "] = "
<< point.first[j] << ", joint_velocity = "
<< point.second[j];
}
}
}
// 控制时间节奏
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 控制周期
}
rsm_.store(ROBOT_READY); // 运动完成
} catch (const std::exception &e) {
LOG(ERROR) << "Control thread error: " << e.what();
rsm_.store(ROBOT_ERROR);
}
});
// 等待线程完成
if (trajectory_thread.joinable()) {
trajectory_thread.join();
}
if (control_thread.joinable()) {
control_thread.join();
}
LOG(INFO) << "speedL trajectory execution completed successfully.";
} catch (const std::exception &e) {
LOG(ERROR) << "speedL failed: " << e.what();
rsm_.store(ROBOT_ERROR);
throw std::runtime_error(std::string("speedL error: ") + e.what());
}
}
template<int DOF>
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
try {
@ -1436,7 +1944,7 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
}
try {
const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变
const double CONTROL_PERIOD = 1.0 / 100.0; // 控制周期保持不变
msgs::Pose3d current_pose = fk(base_link, ee_link);
// 1. 初始化当前和目标位姿矩阵
@ -1524,6 +2032,26 @@ void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &
cartesian_trajectory.push_back(T_interp);
}
// 在这里保存轨迹点到文件
std::ofstream trajectory_file("/home/tankaitao/cmvr-es/trajectory_points.csv");
if (!trajectory_file.is_open()) {
throw std::runtime_error("Failed to open trajectory file");
}
// 写入 CSV 文件头
trajectory_file << "x,y,z\n";
// 写入轨迹点位置
for (size_t i = 0; i < cartesian_trajectory.size(); ++i) {
Eigen::Matrix4d T = cartesian_trajectory[i];
Eigen::Vector3d position = T.block<3, 1>(0, 3); // 获取位置
trajectory_file << position.x() << "," << position.y() << "," << position.z() << "\n";
}
trajectory_file.close();
LOG(INFO) << "Trajectory points saved to /home/tankaitao/cmvr-es/trajectory_points.csv";
// 6. 预先计算所有轨迹点的关节位置
std::vector<Eigen::Vector<double, DOF>> joint_positions;

54
src/devices/robot/humanoid_robot/humanoid_robot.h
View File

@ -17,6 +17,19 @@
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include "motor/motor_manager.h"
#include <thread>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <vector>
#include <memory>
#include <chrono>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <Eigen/Dense>
#include <Eigen/Geometry>
namespace cmvr::device{
@ -93,10 +106,26 @@ namespace cmvr::device{
// 添加这三个函数的声明
bool computeJacobian(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::string& base_link,
const std::string& ee_link,
Eigen::Matrix<double, 6, DOF>& J);
Eigen::Vector3d getEndEffectorPosition(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::string& ee_link);
Eigen::Matrix3d getEndEffectorOrientation(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
const std::string& ee_link);
public:
// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z
static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz);
static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);\
static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);
double calculateMoveTime(double distance, double vel, double acc);
@ -128,6 +157,29 @@ namespace cmvr::device{
std::shared_ptr<SPMCRingBuffer<JointVelocityCommand>> CSV_buffer_;
std::shared_ptr<SPMCRingBuffer<JointCurrentCommand>> CSC_buffer_;
struct ControlPoint {
Eigen::Vector<double, DOF> joint_positions;
Eigen::Vector<double, DOF> joint_velocities;
double timestamp;
ControlPoint() : timestamp(0.0) {}
ControlPoint(const Eigen::Vector<double, DOF>& pos, const Eigen::Vector<double, DOF>& vel, double time): joint_positions(pos), joint_velocities(vel), timestamp(time) {}
};
// 线程间通信的队列和同步变量
std::queue<ControlPoint> control_queue_;
std::mutex queue_mutex_;
std::condition_variable queue_cv_;
std::atomic<bool> trajectory_completed_{false};
std::atomic<bool> control_thread_ready_{false};
std::atomic<bool> stop_requested_{false};
// 线程对象
std::unique_ptr<std::thread> trajectory_thread_;
std::unique_ptr<std::thread> control_thread_;
private:
std::vector<XmlNode> l_motors_cfg_;
std::vector<XmlNode> r_motors_cfg_;

91
src/devices/robot/humanoid_robot/humanoid_robot_test.cpp
View File

@ -13,6 +13,8 @@
#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
#include "cmvr/msgs/geometry.pb.h"
#include "robot/humanoid_robot/humanoid_robot.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "utils/base/abstract_interpolation.h"
// 定义一个命令行参数 --config_path
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
@ -432,6 +434,7 @@ TEST(HumanoidRobotTest,MoveIKTest) {
}
TEST(HumanoidRobotTest,MoveLTest) {
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
@ -452,15 +455,19 @@ TEST(HumanoidRobotTest,MoveLTest) {
// 定义目标位姿（直线运动的终点与当前的差值）
cmvr::msgs::Pose3d delta_pose;
delta_pose.mutable_position()->set_x(0.05);
delta_pose.mutable_position()->set_y(0.05);
delta_pose.mutable_position()->set_z(0.05);
delta_pose.mutable_position()->set_x(0.2);
delta_pose.mutable_position()->set_y(0);
delta_pose.mutable_position()->set_z(0);
delta_pose.mutable_euler()->set_rx(0);
delta_pose.mutable_euler()->set_ry(0);
delta_pose.mutable_euler()->set_rz(0);
// 4. 设置运动参数（速度单位：m/s，加速度单位：m/s²）
double vel = 0.1; // 最大线速度 0.1m/s
double acc = 0.05; // 加速度 0.05m/s²
// 调用moveL执行直线运动
@ -472,3 +479,81 @@ TEST(HumanoidRobotTest,MoveLTest) {
LOG(ERROR) << "moveL调用失败：" << e.what();
}
}
TEST(HumanoidRobotTest, SpeedJTest) {
// 配置文件路径
std::string config_path = "/home/tankaitao/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
// 检查配置文件是否包含 DeviceManager 节点
if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found";
}
// 获取 DeviceManager 配置并初始化设备管理器
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
// 获取机器人实例
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
// 设置目标关节和相关参数
std::string joint_name = "L_WRIST_R"; // 目标关节名
RobotJointIndexDirection dir = RobotJointIndexDirection::Y_NEGATIVE; // 方向
double vel = -0.1; // 速度 (rad/s)
double acc = 1; // 加速度 (rad/s²)
try {
// 调用 speedJ 函数进行关节运动
robot->speedJ(joint_name, dir, vel, acc);
} catch (const std::exception& e) {
LOG(ERROR) << "speedJ test failed: " << e.what();
FAIL() << "Exception thrown during speedJ test: " << e.what();
}
}
TEST(HumanoidRobotTest, SpeedLTest) {
std::string config_path = "/home/linbo/newProject/cmvr-es/config/cabin_robot.xml";
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) {
LOG(ERROR) << "Device Manager node not found";
}
auto dmgr_cfg = config.getChild("DeviceManager");
auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
try {
// 定义运动方向（笛卡尔坐标系）
RobotCartesian cart_direction = RobotCartesian::Y; // 沿X轴移动
// 定义运动方向（正向或反向）
RobotJointIndexDirection move_direction = RobotJointIndexDirection::Y_NEGATIVE; // 正向
// 设置运动参数（速度单位：m/s，加速度单位：m/s²）
double vel = 0.02; // 速度 0.1m/s
double acc = 0.05; // 加速度 0.05m/s²
LOG(INFO) << "开始执行速度控制，方向：X轴正向，速度：" << vel << "m/s，加速度：" << acc << "m/s²";
// 调用speedL执行速度控制
robot->speedL(cart_direction, move_direction, vel, acc);
// 等待一段时间让机器人运动
std::this_thread::sleep_for(std::chrono::seconds(5));
// 停止运动（假设有停止函数）
// robot->stopSpeedL();
LOG(INFO) << "速度控制完成！";
} catch (const std::exception& e) {
// 捕获异常（如IK解算失败、状态非法等）
LOG(ERROR) << "speedL调用失败：" << e.what();
}
}

16
src/utils/controller/cartesian_controller.cpp
View File

@ -79,9 +79,23 @@ bool CartesianController<DOF>::compute(
case Mode::Velocity: {
const double Kp = 1.0; // [rad/s] per rad error
out_cmd = Kp * (q_des - q_init);
std::cout << "=================== q_des = " << q_des
<< "q_init = " << q_init << std::endl;
const auto qd_lim = robot_->GetLimitQdotUpper(state);
for (int i = 0; i < DOF; ++i)
for (int i = 0; i < DOF; ++i) {
// 打印原始的速度命令和限制值
std::cout << "Before clamping - out_cmd[" << i << "] = " << out_cmd[i]
<< ", qd_lim(" << i << ") = " << qd_lim(i)
<< ", -qd_lim(" << i << ") = " << -qd_lim(i) << std::endl;
// 对 out_cmd 进行限速
out_cmd[i] = std::clamp(out_cmd[i], -qd_lim(i), qd_lim(i));
// 打印被clamp之后的值
std::cout << "After clamping - out_cmd[" << i << "] = " << out_cmd[i]
<< ", qd_lim(" << i << ") = " << qd_lim(i)
<< ", -qd_lim(" << i << ") = " << -qd_lim(i) << std::endl;
}
return ik_ok;
}
case Mode::Torque: {