diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 6055f3c1..140febcb 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -38,6 +38,7 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { CSC_buffer_ = make_shared >(cfg.getAttrDefault("bufferSize", 50)); + upd_freq_ = 1000; upd_timer_ = make_shared(); upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), [this] { update_state_(); }); @@ -58,7 +59,7 @@ void HumanoidRobot::init() { std::vector(7, 1.0)); - ik_solver_ = std::make_shared("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + ik_solver_ = std::make_shared("/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", "PELVIS_S", "R_WRIST_R_S", "R_FINGER_TIP_FIXED"); @@ -67,10 +68,10 @@ void HumanoidRobot::init() { } speedl_config_.linear_velocity_max = 0.55; - speedl_config_.linear_acceleration_max = 0.80; - speedl_config_.linear_jerk_max = 3.30; + speedl_config_.linear_acceleration_max = 5.00; + speedl_config_.linear_jerk_max = 10.0; speedl_config_.angular_velocity_max = 1.00; - speedl_config_.angular_acceleration_max = 3.00; + speedl_config_.angular_acceleration_max = 5.00; speedl_config_.angular_jerk_max = 12.0; speedl_config_.joint_acceleration_max = std::vector(7, 8.0); speedl_config_.linear_target_replan_threshold = 1e-4; @@ -321,9 +322,8 @@ void HumanoidRobot::speedLWorkerLoop_() { if (!ik_solver_->speedLStep(target_twist, dt, q_now, - qd_now, qd_cmd, - cmvr::CartesianFrame::Tool, + cmvr::CartesianFrame::Base, true)) { LOG(ERROR) << "speedL worker: speedLStep() failed"; send_zero(); diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index ddf892f2..22c7e947 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -261,7 +261,7 @@ TEST(HumanoidRobotTest,speedJTest) { } TEST(HumanoidRobotTest, speedLSmokeTest) { - const XmlNode config("/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); + 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); @@ -270,10 +270,23 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { auto robot = std::dynamic_pointer_cast>(robot_abs); ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; - const std::vector twist_pos = {0.03, 0.0, 0.0, 0.0, 0.0, 0.0}; - const std::vector twist_neg = {-0.03, 0.0, 0.0, 0.0, 0.0, 0.0}; - const double acceleration = 0.20; - const double segment_time = 2.0; + std::vector 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)); + + const std::vector twist_pos = {0.08, 0, 0, 0.00, 0, 0}; + const std::vector twist_neg = {-0.08, 0, 0, 0.00, 0, 0}; + const double acceleration = 3.0; + const double segment_time = 1.0; const double settle_time = 1.0; const double sample_dt = 0.02; @@ -304,11 +317,18 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { } }; - sample_phase(twist_pos, segment_time, "+X"); - sample_phase(twist_neg, segment_time, "-X"); + robot->speedL({0.0, 0.0, 0.0, 0.0, 0.00, 0.0}, acceleration, 0.0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + + sample_phase(twist_pos, segment_time*0.8, "+X"); + sample_phase(twist_neg, segment_time, "-X"); + LOG(INFO) << "speedLSmokeTest phase: stop"; - ASSERT_NO_THROW(robot->stopSpeedL()); + robot->speedL({0.0, 0.0, 0.0, 0.0, 0.00, 0.0}, acceleration, 1); + // robot->moveJ(init_cmd,1.0,2.0); + // ASSERT_NO_THROW(robot->stopSpeedL()); const int settle_steps = static_cast(std::ceil(settle_time / sample_dt)); for (int i = 0; i < settle_steps; ++i) { const Eigen::Matrix cmd_twist = robot->getSpeedLCommandTwistBase(); @@ -354,8 +374,9 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { ylabel(ax2, "norm [m/s]"); legend(ax2, {"||command v||"}); grid(ax2, true); - - show(fig); + save("/home/lgv/cmvr/cmvr-es/data/plot1.png"); + std::cout << "Hello World" << std::endl; + // show(fig); } TEST(HumanoidRobotTest, SpeedLOpenLoopPlannerPlot) { diff --git a/data/plot.png b/data/plot.png new file mode 100644 index 00000000..fa1eda91 Binary files /dev/null and b/data/plot.png differ diff --git a/data/plot1.png b/data/plot1.png new file mode 100644 index 00000000..29ea5ccd Binary files /dev/null and b/data/plot1.png differ diff --git a/model/xiaoyan_description/dual_arm.urdf b/model/xiaoyan_description/dual_arm.urdf index e995eaa9..f1cc20d0 100644 --- a/model/xiaoyan_description/dual_arm.urdf +++ b/model/xiaoyan_description/dual_arm.urdf @@ -594,7 +594,7 @@ - +