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

feat:add test

Browse Source
This commit is contained in:
lgv 2026-02-28 16:03:09 +08:00
parent 8c79471c6c
commit e37073f493
6 changed files with 70 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -58,7 +58,7 @@
<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="true">
<HeadCan id = " " devId = " " channelId ="2" enable="false">
<Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>

2
cmvr-es/controller/include/ibvs_controller.h
View File

@ -253,7 +253,7 @@ private:
// 参数
// ViSP 控制增益。
double lambda_{1.7};
double lambda_{0.7};
// tag 边长（米）。
double tag_size_m_{0.12};
// tag 半边长（米）。

15
cmvr-es/controller/src/ibvs_controller.cpp
View File

@ -51,14 +51,14 @@ IbvsController::IbvsController()
// AbstractCamera 相机系 -> ViSP 相机系
R_cv_ = (Eigen::Matrix3d() <<
1, 0, 0,
0, -1, 0,
0, 0, -1).finished();
0, 1, 0,
0, 0, 1).finished();
// AbstractCamera 相机系 -> URDF 相机系
R_camera_urdf_ = (Eigen::Matrix3d() <<
1, 0, 0,
0, -1, 0,
0, 0, -1).finished();
0, 1, 0,
0, 0, 1).finished();
detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
updateDepthControlPointInTag();
@ -338,6 +338,13 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
// q_{k+1} = q_k + qdot * dt
q_cmd_[i] += qdot[i] * dt;
}
std::cout << "b_cmd: " ;
for (const auto& it : q_cmd_) {
std::cout << it << " ";
}
std::cout << std::endl;
clampJointCommandInPlace(q_cmd_);
q_cmd_out = q_cmd_;

12
cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_sdo_response.cpp
View File

@ -31,11 +31,11 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
case msgs::POSITION_OFFSET_2008:
motor_status->set_position_offset(sdo_response.data());
}
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();
//
// 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();
}

10
cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp
View File

@ -31,9 +31,9 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
break;
}
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();
// 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();
}

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

@ -127,6 +127,11 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) {
return true;
};
ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q";
@ -159,6 +164,19 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
ASSERT_NE(robot, nullptr);
ASSERT_NE(camera, nullptr);
std::vector<JointPoint> init_cmd{};
init_cmd = {
{"R_SHOULDER_P", -0.2423},
{"R_SHOULDER_R", 1.2929},
{"R_SHOULDER_Y", 1.61},
{"R_ELBOW_R", 1.58},
{"R_WRIST_P", -2.8792},
{"R_WRIST_Y", 0.1150},
{"R_WRIST_R", -0.08},
};
robot->moveJ(init_cmd,1.0,2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
ASSERT_NO_THROW(camera->start());
struct RuntimeGuard {
std::shared_ptr<AbstractCamera> camera;
@ -181,12 +199,30 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
cmvr::IbvsController ibvs_controller;
ibvs_controller.setMu(0.1);
ibvs_controller.setLambda(0.6);
ibvs_controller.setQdotMax(0.6);
ibvs_controller.setTagSize(0.12);
ibvs_controller.setTrackedTagId(0);
ibvs_controller.setTarget(0.0, 0.0, 0.35, 3.14159265358979323846, 0.0, 0.0);
ibvs_controller.setTarget(0.0, 0.0, 0.40);
ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR);
ibvs_controller.setDepthZGain(1.0);
Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
// ibvs_controller.setAlignCameraToVisp(I); // RealSense optical -> ViSP
// ibvs_controller.setAlignCameraToUrdf(I); // optical -> URDF 的 R_CAM
Eigen::Matrix3d RxPi;
RxPi << 1,0,0,
0,-1,0,
0,0,-1;
ibvs_controller.setAlignCameraToUrdf(RxPi);
ibvs_controller.setAlignCameraToVisp(RxPi);
// ibvs_controller.setVelocityLimit6({0.15, 0.15, 0.20, 0, 0, 0});
// ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005});
ibvs_controller.setQdotMax(0.15);
ASSERT_TRUE(ibvs_controller.init(camera,
"/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf",
@ -205,6 +241,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
return false;
}
q_now[i] = it->second;
// std::cout << kRightArmJointNames[i] << " " << q_now[i] << std::endl;
}
return true;
};
@ -215,7 +252,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
const int max_steps = 30000;
const int log_every = 1;
const int cycle_ms = 33;
const int cycle_ms = 1;
int ok_steps = 0;
int fail_steps = 0;
@ -251,8 +288,8 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
continue;
}
auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.8);
// robot->servoJ(joint_cmd, 0.8, dt);
auto joint_cmd = buildRightArmJointCmd(q_cmd_next, 0.5);
robot->servoJ(joint_cmd, 0.5, 0.5);
++ok_steps;
if ((step % log_every) == 0) {
@ -264,6 +301,11 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
<< ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]"
<< " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage())
<< std::endl;
std::cout << "q_cmd: " ;
for (const auto& it : q_cmd_next) {
std::cout << it << " ";
}
std::cout << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms));