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

feat: css and csp test pass && add speedJ

Browse Source
This commit is contained in:
lgv 2026-03-04 10:50:58 +08:00
parent 61699a09df
commit 34e349ce2a
10 changed files with 108 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cmvr-es/common/config/cabin_robot.xml
View File

@ -50,12 +50,12 @@
<Motor id="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
</LeftArmCan>
<RightArmCan id = " " devId = " " channelId ="1" enable="true" toolFrame="R_FINGER_TIP">
<!-- <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>-->
<!-- <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>-->
<Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan>
<HeadCan id = " " devId = " " channelId ="2" enable="false">

9
cmvr-es/devices/motor/motor_manager_test.cpp
View File

@ -73,11 +73,11 @@ TEST(MotorMangerTest,MyTest) {
motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY);
motor_3->setTarget(-0.5);
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
// motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY);
// motor_3->setTarget(-0.5);
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION);
motor_3->setTarget(-3.14,1.6);
// motor_3->setTarget(0,1.6);
// std::this_thread::sleep_for(std::chrono::milliseconds(5000));
//
@ -100,6 +100,7 @@ TEST(MotorMangerTest,MyTest) {
while (true) {
auto mode = motor_3->getMode();
auto q = motor_3->getQ();
auto qd = motor_3->getQd();
std::cout << q << ", " << qd << std::endl;

3
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp
View File

@ -31,5 +31,8 @@ void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot
}
LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ;
}

20
cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp
View File

@ -104,10 +104,10 @@ void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier c
void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) {
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
switch (cur_mode_[node_id]) {
case RUN_MODE_CYCLIC_SYNC_POSITION:
setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
break;
switch (getMode(node_id)) {
// 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));
@ -179,10 +179,8 @@ void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos)
void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
cur_mode_[node_id] = mode;
// 1 : 先设置模式
auto data = static_cast<uint32_t>(mode);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data);
// cur_mode_[node_id] = mode;
// 2 : 状态机步进 —— Shutdown（0x06）
@ -193,6 +191,10 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
// configRPDO1(node_id, false);
// configRPDO2(node_id, false);
// 1 : 先设置模式
auto data = static_cast<uint32_t>(mode);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data);
// 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
cw.switch_on = 1;
@ -482,7 +484,7 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) {
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
switch (cur_mode_[node_id]) {
switch (getMode(node_id)) {
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
case msgs::RUN_MODE_PROFILE_POSITION: {
auto it = last_Qd_.find(node_id);

6
cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h
View File

@ -62,10 +62,10 @@ namespace cmvr {
}
msgs::RunMode getMode(uint8_t node_id) override {
// auto feed_mode = GetRobotDetail()->motors().at(node_id).run_mode();
return GetRobotDetail()->motors().at(node_id).run_mode();
// cur_mode_[node_id] = feed_mode;
// return feed_mode;
return cur_mode_[node_id];
// return cur_mode_[node_id];
}
@ -75,7 +75,7 @@ namespace cmvr {
std::shared_ptr<AbstractCanbus> can_client_{nullptr};
// key node_id
std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
// std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
std::unordered_map<uint8_t,uint32_t> last_Qd_{};
std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};

11
cmvr-es/devices/motor/ti5_motor/ti5_motor.h
View File

@ -35,12 +35,15 @@ namespace cmvr {
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION);
// canopen_protocol->torqueOff(node_id_);
canopen_protocol->configPdo(node_id_);
canopen_protocol->configProfile(node_id_,4000,8000,8000);
// canopen_protocol->configProfile(node_id_,4000,8000,8000);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE);
canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06);
canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F);
canopen_protocol->setLimitQd(node_id_,6.0);
canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
// canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
// canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
canopen_protocol->setLimitQd(node_id_,5.0);
canopen_protocol->setLimitQdd(node_id_,10.0,-10.0);
// canopen_protocol->torqueOff(node_id_);
}
}

3
cmvr-es/devices/robot/abstract_robot.h
View File

@ -84,7 +84,8 @@ namespace cmvr::device{
* @param acc 主导轴最大加速度，单位 rad/s^2。
*/
virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void speedJ(std::vector<JointVelocityCommand> &cmd) { throw std::runtime_error("Not implemented"); }
virtual void speedJ(double vel) { throw std::runtime_error("Not implemented"); }
/**
* @brief 末端位姿目标的关节空间运动。
* @details 典型实现为先做 IK 求解关节目标，再按 `moveJ` 语义执行（关节空间规划与同步）。

2
cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h
View File

@ -79,6 +79,8 @@ namespace cmvr::device{
/* robot basic command*/
void eStop() override;
void moveJ(std::vector<JointPoint> &cmd, double vel = 0.5, double acc = 0.1) override;
void speedJ(std::vector<JointVelocityCommand> &cmd) override;
void speedJ(double vel) override;
void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override;
void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override ;
void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;

27
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
View File

@ -216,6 +216,28 @@ void HumanoidRobot<DOF>::eStop() {
// }
template<int DOF>
void HumanoidRobot<DOF>::speedJ(std::vector<JointVelocityCommand> &cmd) {
for (const auto &j: cmd) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
}
motor->setTarget(j.vel);
}
}
}
template<int DOF>
void HumanoidRobot<DOF>::speedJ(double vel) {
for (const auto &motor: motor_manager_->motorsMap()) {
motor.second->setTarget(vel);
}
}
template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
if (cmd.empty()) return;
@ -341,7 +363,7 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
motor->setQd(vel);
// motor->setQd(vel);
motors.push_back(motor);
}
@ -353,7 +375,8 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double
while (k < K) {
const auto &s = samples[k];
for (size_t i = 0; i < motors.size(); ++i) {
motors[i]->setQ(s.q[i]);
// motors[i]->setQ(s.q[i]);
motors[i]->setTarget(s.q[i],s.qd[i]);
}
++k;

81
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
View File

@ -220,6 +220,43 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
}
TEST(HumanoidRobotTest,speedJTest) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
auto dmgr_cfg = config.getChild("DeviceManager");
auto& dmgr = DeviceManager::getInstance(dmgr_cfg);
auto robot = dmgr.getDevice<AbstractRobot>("hc01");
std::vector<JointVelocityCommand> cmd{};
cmd = {
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
{"R_WRIST_R", 0.2},
};
robot->speedJ(cmd);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// robot->speedJ(0);
//
std::vector<JointPoint> cmd1{};
cmd1 = {
// {"R_SHOULDER_P", 0.0},
// {"R_SHOULDER_R", 0.0},
// {"R_SHOULDER_Y", 0.0},
// {"R_ELBOW_R", 0.0},
// {"R_WRIST_P", 0.0},
// {"R_WRIST_Y", 0.0},
{"R_WRIST_R", 0},
};
robot->moveJ(cmd1,2.0,10.0);
// robot->eStop();
//
}
TEST(HumanoidRobotTest,IBVSWithRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");
ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found";
@ -319,19 +356,6 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
<< "Failed to extract right-arm 7 joints from robot state";
ibvs_controller.reset(q_now);
auto motor_manager = cmvr::MotorsInfo::getInstance()->getMotorManager();
ASSERT_TRUE(motor_manager != nullptr) << "MotorManager is null";
std::vector<std::shared_ptr<AbstractMotor>> right_arm_motors;
right_arm_motors.reserve(kRightArmJointNames.size());
for (const auto* name : kRightArmJointNames) {
auto motor = motor_manager->getMotor(name);
ASSERT_TRUE(motor != nullptr) << "getMotor failed for " << name;
if (motor->getMode() !=cmvr::msgs::RUN_MODE_PROFILE_VELOCITY) {
motor->setMode(cmvr::msgs::RUN_MODE_PROFILE_VELOCITY);
}
motor->setQd(0.0);
right_arm_motors.push_back(motor);
}
const int max_steps = 30000;
const int log_every = 1;
@ -367,23 +391,10 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
std::vector<double> qdot_next;
const bool ok = ibvs_controller.compute(q_now, qdot_next);
if (!ok || qdot_next.size() != kRightArmJointNames.size()) {
for (auto& motor : right_arm_motors) {
motor->setQd(0.0);
}
std::fill(qdot_cmd_prev.begin(), qdot_cmd_prev.end(), 0.0);
++fail_steps;
if ((step % log_every) == 0) {
std::cout << "[IBVS_REAL] step=" << step
<< " compute failed: "
<< cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus())
<< std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));
continue;
}
std::vector<double> qd_send(kRightArmJointNames.size(), 0.0);
std::vector<JointVelocityCommand> qd_send;
for (size_t i = 0; i < kRightArmJointNames.size(); ++i) {
qdot_lpf[i] = qdot_lpf_alpha * qdot_lpf[i] + (1.0 - qdot_lpf_alpha) * qdot_next[i];
double v = applySoftJointLimitVelocity(q_now[i], qdot_lpf[i],
@ -396,12 +407,10 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
v = std::clamp(v, qdot_cmd_prev[i] - dv_max, qdot_cmd_prev[i] + dv_max);
v = std::clamp(v, -qdot_max_send, qdot_max_send);
qdot_cmd_prev[i] = v;
qd_send[i] = v;
qd_send.push_back({kRightArmJointNames[i],v});
}
for (size_t i = 0; i < right_arm_motors.size(); ++i) {
right_arm_motors[i]->setQd(qd_send[i]);
}
robot->speedJ(qd_send);
++ok_steps;
if ((step % log_every) == 0) {
@ -415,7 +424,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
<< std::endl;
std::cout << "qd_cmd: " ;
for (const auto& it : qd_send) {
std::cout << it << " ";
std::cout << it.vel << " ";
}
std::cout << std::endl;
}
@ -428,9 +437,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
<< " fail_steps=" << fail_steps
<< std::endl;
for (auto& motor : right_arm_motors) {
motor->setQd(0.0);
}
robot->speedJ(0);
EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps.";
}