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

test(ethercat): strengthen real motor trajectory coverage

Browse Source
This commit is contained in:
lgv 2026-07-30 14:12:16 +08:00
parent abe69c46f0
commit 2ca03d88da
5 changed files with 245 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
cmvr-es/common/CMakeLists.txt
View File

@ -29,6 +29,12 @@ target_link_libraries(common PUBLIC
add_library(cmvr_es::common ALIAS common)
install(TARGETS common LIBRARY DESTINATION lib)
add_executable(support_functions_test
math/support_functions_test.cpp
)
target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es)
target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog)
#add_executable(image_display_test
# utils/visualization/image_display_test.cpp
#)

34
cmvr-es/common/math/support_functions_test.cpp Normal file
View File

@ -0,0 +1,34 @@
#include <cstdint>
#include <limits>
#include <gtest/gtest.h>
#include "common/math/support_functions.h"
TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceTreatsFullTurnsAsEquivalent)
{
constexpr std::int64_t period = 65536LL * 101LL;
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(5254257, -1364879, period), 0);
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(-10883488, -17502624, period), 0);
}
TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceUsesShortestWrappedDistance)
{
constexpr std::int64_t period = 100;
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(3, 97, period), 6);
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(97, 3, period), 6);
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(10, 40, period), 30);
}
TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceHandlesInt32Range)
{
constexpr std::int64_t period = 65536LL * 101LL;
const auto distance = SupportFunctions::cyclicAbsoluteDifference(
std::numeric_limits<std::int32_t>::min(),
std::numeric_limits<std::int32_t>::max(), period);
EXPECT_GE(distance, 0);
EXPECT_LE(distance, period / 2);
}

38
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp
View File

@ -1,6 +1,7 @@
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <iostream>
@ -73,17 +74,42 @@ TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
return;
}
EXPECT_TRUE(runtime.writePdo<std::uint16_t>(1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000));
EXPECT_TRUE(runtime.writePdo<std::int8_t>(1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0));
const std::array command_writes{
EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000),
EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0),
};
auto invalid_writes = command_writes;
invalid_writes[1].bit_length = 16;
const auto generation_before = runtime.commandGeneration();
EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size()));
EXPECT_EQ(runtime.commandGeneration(), generation_before);
EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size()));
EXPECT_EQ(runtime.commandGeneration(), generation_before + 1);
const int settle_ms = 1000;
std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms));
EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1);
EXPECT_TRUE(runtime.isHealthy());
std::uint16_t statusword = 0;
EXPECT_TRUE(runtime.readPdo<std::uint16_t>(1, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword));
std::array feedback_reads{
EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
1, msgs::CIA402_STATUS_WORD_6041, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
1, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
};
auto invalid_feedback_reads = feedback_reads;
invalid_feedback_reads[1].bit_length = 16;
EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(),
invalid_feedback_reads.size()));
ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size()));
std::int8_t mode_display = 0;
EXPECT_TRUE(runtime.readPdo<std::int8_t>(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display));
const auto statusword =
EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(feedback_reads[0]);
const auto mode_display =
EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(feedback_reads[1]);
std::cout << "CIA402 statusword: 0x" << std::hex << statusword
<< ", mode display: " << std::dec << static_cast<int>(mode_display)

199
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp vendored
View File

@ -7,6 +7,8 @@
#include <limits>
#include <memory>
#include <thread>
#include <utility>
#include <vector>
#include <gtest/gtest.h>
@ -24,14 +26,18 @@ constexpr const char* kMotorConfigFile =
"devices/motor/ethercat_motors_four_real_test.pb.txt";
constexpr std::array<int, 4> kFourMotorIds{1, 2, 3, 4};
constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
constexpr std::chrono::milliseconds kPrintPeriod{100};
constexpr std::chrono::milliseconds kStatsSamplePeriod{10};
constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500};
constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
constexpr double kPi = 3.14159265358979323846;
constexpr double kFourMotorAmplitudeRad = 0.2;
constexpr double kFourMotorPeriodS = 1.0;
constexpr double kRaisedCosineCoefficientRad = 0.2;
constexpr double kFourMotorPeriodS = 2.0;
constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
constexpr double kMinimumPositionExcursionRad = 0.2;
constexpr double kMaximumAbsoluteTrackingErrorRad = 0.15;
constexpr double kMaximumRmsTrackingErrorRad = 0.08;
constexpr double kMaximumErrorSpreadRad = 0.10;
constexpr double kFinalPositionToleranceRad = 0.05;
class DeviceManagerDestroyGuard {
public:
@ -41,6 +47,61 @@ public:
}
};
class MotorManagerStopGuard {
public:
explicit MotorManagerStopGuard(std::shared_ptr<MotorManager> motor_manager)
: motor_manager_(std::move(motor_manager))
{
}
~MotorManagerStopGuard()
{
if (motor_manager_ && !motor_manager_->stop()) {
std::cerr << "failed to stop motor manager during test cleanup" << std::endl;
}
}
private:
std::shared_ptr<MotorManager> motor_manager_;
};
class MultiMotorSafetyGuard {
public:
explicit MultiMotorSafetyGuard(
const std::vector<std::shared_ptr<AbstractMotor>>& motors)
: motors_(motors)
{
}
~MultiMotorSafetyGuard()
{
if (armed_) {
stop();
}
}
bool stop()
{
bool all_ok = true;
for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
if (*it && !(*it)->quickStop()) {
all_ok = false;
}
}
for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
if (*it && !(*it)->torqueOff()) {
all_ok = false;
}
}
armed_ = !all_ok;
return all_ok;
}
private:
const std::vector<std::shared_ptr<AbstractMotor>>& motors_;
bool armed_{true};
};
struct TrackingErrorStats {
std::int64_t sample_count{0};
double sum_error{0.0};
@ -140,31 +201,16 @@ TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
ASSERT_NE(motor_manager, nullptr);
MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
// for (int motor_id = 1; motor_id <= 4; ++motor_id) {
// printMotorState(motor_id, motor_manager->getMotor(motor_id));
// }
auto motor = motor_manager->getMotor(4);
motor->calibrateZeroQ();
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
motor->commandProfileVelocity(-2,5);
for (int i = 1; i <= 50; ++i)
{
auto q = motor->getQ();
auto qd = motor->getQd();
std::cout << "q=" << q << ", qd=" << qd << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(100));
for (const int motor_id : kFourMotorIds) {
auto motor = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
ASSERT_NE(motor, nullptr);
printMotorState(motor_id, motor);
}
motor->quickStop();
}
TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory)
{
ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
DeviceManagerDestroyGuard guard;
@ -173,16 +219,21 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
ASSERT_NE(motor_manager, nullptr);
MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
std::array<std::shared_ptr<AbstractMotor>, kFourMotorIds.size()> motors;
std::vector<std::shared_ptr<AbstractMotor>> motors(kFourMotorIds.size());
for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) {
const int motor_id = kFourMotorIds[i];
motors[i] = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
ASSERT_NE(motors[i], nullptr);
printMotorState(motor_id, motors[i]);
}
MultiMotorSafetyGuard safety_guard(motors);
std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
for (const auto& motor : motors) {
ASSERT_TRUE(motor->torqueOff());
}
for (std::size_t i = 0; i < motors.size(); ++i) {
const int motor_id = kFourMotorIds[i];
std::cout << "before calibrateZeroQ: ";
@ -196,48 +247,64 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
ASSERT_TRUE(motor->torqueOn());
}
for (const auto& motor : motors) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
}
std::array<double, kFourMotorIds.size()> center_q{};
for (std::size_t i = 0; i < motors.size(); ++i) {
center_q[i] = motors[i]->getQ();
}
std::vector<double> center_q;
std::vector<double> actual_qd;
ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd));
const double omega = 2.0 * kPi / kFourMotorPeriodS;
std::cout << "command four motors in CSP, duration="
<< kFourMotorTrajectoryDuration.count()
<< " ms, command_period=" << kCyclicCommandPeriod.count()
<< " ms, amplitude=" << kFourMotorAmplitudeRad
<< " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad
<< " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad
<< " rad, period=" << kFourMotorPeriodS
<< " s" << std::endl;
const auto start_time = std::chrono::steady_clock::now();
const auto total_ticks = kFourMotorTrajectoryDuration / kCyclicCommandPeriod;
const auto end_time = start_time + kFourMotorTrajectoryDuration;
auto next_command_time = start_time;
auto next_stats_time = start_time;
std::uint64_t missed_command_deadlines = 0;
std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
std::vector<double> target_q(motors.size(), 0.0);
std::vector<double> target_qd(motors.size(), 0.0);
std::vector<double> actual_q;
std::array<double, kFourMotorIds.size()> minimum_actual_q{};
std::array<double, kFourMotorIds.size()> maximum_actual_q{};
std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin());
std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin());
double max_error_spread_rad = 0.0;
double sum_error_spread_sq = 0.0;
std::int64_t error_spread_sample_count = 0;
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
const auto elapsed = tick * kCyclicCommandPeriod;
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
while (true) {
std::this_thread::sleep_until(next_command_time);
const auto now = std::chrono::steady_clock::now();
if (now > end_time) {
break;
}
const double t_s = std::chrono::duration<double>(now - start_time).count();
std::array<double, kFourMotorIds.size()> target_q{};
std::array<double, kFourMotorIds.size()> target_qd{};
for (std::size_t i = 0; i < motors.size(); ++i) {
const double theta = omega * t_s + kFourMotorPhaseRad[i];
target_q[i] = center_q[i] + kFourMotorAmplitudeRad * (1.0 - std::cos(theta));
target_qd[i] = kFourMotorAmplitudeRad * omega * std::sin(theta);
ASSERT_TRUE(motors[i]->commandCyclicPosition(target_q[i], target_qd[i]));
target_q[i] =
center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta));
target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta);
}
ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
if (elapsed.count() % kStatsSamplePeriod.count() == 0) {
if (now >= next_stats_time) {
ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
double min_error = std::numeric_limits<double>::max();
double max_error = std::numeric_limits<double>::lowest();
for (std::size_t i = 0; i < motors.size(); ++i) {
const double theta = omega * t_s + kFourMotorPhaseRad[i];
const double error = motors[i]->getQ() - target_q[i];
const double error = actual_q[i] - target_q[i];
error_stats[i].add(error, theta);
minimum_actual_q[i] = std::min(minimum_actual_q[i], actual_q[i]);
maximum_actual_q[i] = std::max(maximum_actual_q[i], actual_q[i]);
min_error = std::min(min_error, error);
max_error = std::max(max_error, error);
}
@ -245,31 +312,24 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
sum_error_spread_sq += error_spread * error_spread;
++error_spread_sample_count;
next_stats_time = now + kStatsSamplePeriod;
}
// if (elapsed.count() % kPrintPeriod.count() == 0) {
// std::cout << "t=" << elapsed.count() << " ms" << std::endl;
// for (std::size_t i = 0; i < motors.size(); ++i) {
// std::cout << " motor_id=" << kFourMotorIds[i]
// << ", target_q=" << target_q[i]
// << " rad, target_qd=" << target_qd[i]
// << " rad/s, q=" << motors[i]->getQ()
// << " rad, qd=" << motors[i]->getQd()
// << " rad/s" << std::endl;
// }
// }
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
next_command_time += kCyclicCommandPeriod;
const auto command_complete_time = std::chrono::steady_clock::now();
if (next_command_time <= command_complete_time) {
const auto skipped_periods =
(command_complete_time - next_command_time) / kCyclicCommandPeriod + 1;
missed_command_deadlines += static_cast<std::uint64_t>(skipped_periods);
next_command_time += skipped_periods * kCyclicCommandPeriod;
}
}
const auto hold_start_time = std::chrono::steady_clock::now();
const auto hold_ticks = kHoldAfterTrajectoryDuration / kCyclicCommandPeriod;
for (std::int64_t tick = 0; tick <= hold_ticks; ++tick) {
for (std::size_t i = 0; i < motors.size(); ++i) {
ASSERT_TRUE(motors[i]->commandCyclicPosition(center_q[i], 0.0));
}
std::this_thread::sleep_until(hold_start_time + (tick + 1) * kCyclicCommandPeriod);
}
std::copy(center_q.begin(), center_q.end(), target_q.begin());
std::fill(target_qd.begin(), target_qd.end(), 0.0);
ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
std::this_thread::sleep_for(kHoldAfterTrajectoryDuration);
ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
std::cout << "after four motor CSP trajectory" << std::endl;
for (std::size_t i = 0; i < motors.size(); ++i) {
@ -284,6 +344,7 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
std::cout << "four motor CSP tracking error statistics, sample_period="
<< kStatsSamplePeriod.count()
<< " ms, samples=" << error_stats.front().sample_count
<< ", missed_command_deadlines=" << missed_command_deadlines
<< ", max_error_spread=" << max_error_spread_rad
<< " rad, rms_error_spread=" << rms_error_spread
<< " rad" << std::endl;
@ -303,7 +364,19 @@ TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionSinTrajectory)
<< " rad (" << radToDeg(relative_phase)
<< " deg, " << relative_phase_ms
<< " ms)" << std::endl;
EXPECT_GE(maximum_actual_q[i] - minimum_actual_q[i],
kMinimumPositionExcursionRad)
<< "motor_id=" << kFourMotorIds[i] << " did not complete enough motion";
EXPECT_LE(error_stats[i].max_abs_error,
kMaximumAbsoluteTrackingErrorRad)
<< "motor_id=" << kFourMotorIds[i] << " exceeded maximum tracking error";
EXPECT_LE(error_stats[i].rms(), kMaximumRmsTrackingErrorRad)
<< "motor_id=" << kFourMotorIds[i] << " exceeded RMS tracking error";
EXPECT_NEAR(actual_q[i], center_q[i], kFinalPositionToleranceRad)
<< "motor_id=" << kFourMotorIds[i] << " did not return to its start position";
}
EXPECT_LE(max_error_spread_rad, kMaximumErrorSpreadRad);
EXPECT_TRUE(safety_guard.stop());
}
} // namespace cmvr::device

70
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp vendored
View File

@ -23,7 +23,6 @@ namespace cmvr::device {
namespace {
constexpr int kMotorId = 1;
constexpr std::chrono::milliseconds kModeSettleDelay{100};
constexpr std::chrono::milliseconds kCommandSamplePeriod{100};
constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000};
@ -119,10 +118,10 @@ config::MotorConfigItem createMotorConfig()
config::MotorConfigItem config;
config.set_id(kMotorId);
config.set_joint_name("ethercat_test_joint");
config.set_limit_q_lb(-6.14);
config.set_limit_q_ub(6.14);
config.set_limit_q_lb(-36.14);
config.set_limit_q_ub(36.14);
config.set_limit_qd(10.0);
config.set_limit_qdd(10.0);
config.set_limit_qdd(100.0);
config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
config.set_gear_ratio(kDefaultGearRatio);
return config;
@ -239,6 +238,9 @@ TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter)
ASSERT_TRUE(motor->calibrateZeroQ());
printMotorState("after calibrateZeroQ", *motor);
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
ASSERT_TRUE(motor->commandProfilePosition(1.5,0.8,3.0));
sampleMotorState(*motor, kFeedbackSampleDuration);
}
TEST(EyouMotorRealTest, CommandProfilePosition)
@ -251,8 +253,7 @@ TEST(EyouMotorRealTest, CommandProfilePosition)
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
sampleMotorState(*motor, kFeedbackSampleDuration);
@ -267,9 +268,10 @@ TEST(EyouMotorRealTest, CommandProfileVelocity)
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
@ -289,21 +291,20 @@ TEST(EyouMotorRealTest, CommandCyclicPosition)
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
const std::chrono::milliseconds trajectory_duration{15000};
const std::chrono::milliseconds trajectory_duration{12000};
const double period_s = 6.0;
const double amplitude_rad = 3;
const double phase_rad = 0.0;
const double excursion_rad = 3.0;
const double center_q = motor->getQ();
const double omega = 2.0 * kPi / period_s;
std::cout << "motor.commandCyclicPosition(sin), center_q=" << center_q
std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q
<< " rad, period=" << period_s
<< " s, amplitude=" << amplitude_rad
<< " rad, phase=" << phase_rad
<< " s, excursion=" << excursion_rad
<< " rad, command_period=" << kCyclicCommandPeriod.count()
<< " ms" << std::endl;
@ -312,9 +313,11 @@ TEST(EyouMotorRealTest, CommandCyclicPosition)
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
const auto elapsed = tick * kCyclicCommandPeriod;
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
const double theta = omega * t_s + phase_rad;
const double target_q = center_q + amplitude_rad * std::sin(theta);
const double target_qd = amplitude_rad * omega * std::cos(theta);
const double theta = omega * t_s;
const double target_q =
center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta));
const double target_qd =
0.5 * excursion_rad * omega * std::sin(theta);
ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
@ -337,19 +340,22 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
const std::chrono::milliseconds trajectory_duration{15000};
const double period_s = 6.0;
const double velocity_amplitude_rad_s = 5.0;
const std::chrono::milliseconds trajectory_duration{12000};
const double period_s = 5.0;
const double excursion_rad = 5.0;
const double phase_rad = 0.0;
const double omega = 2.0 * kPi / period_s;
const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega;
std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
<< " s, velocity_amplitude=" << velocity_amplitude_rad_s
<< " rad/s, phase=" << phase_rad
<< " rad/s, excursion=" << excursion_rad
<< " rad, phase=" << phase_rad
<< " rad, command_period=" << kCyclicCommandPeriod.count()
<< " ms" << std::endl;
@ -367,6 +373,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
<< " ms, target_qd=" << target_qd
<< " rad/s";
printMotorState("", *motor);
printRawEthercatFeedback("raw feedback", runtime);
}
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
}
@ -374,6 +381,7 @@ TEST(EyouMotorRealTest, CommandCyclicVelocity)
std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl;
ASSERT_TRUE(motor->commandCyclicVelocity(0.0));
sampleMotorState(*motor, std::chrono::milliseconds{500});
printRawEthercatFeedback("raw feedback after stop", runtime);
}
TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
@ -388,8 +396,7 @@ TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
const std::chrono::milliseconds run_duration{2000};
const double period_s = 6.0;
@ -444,8 +451,7 @@ TEST(EyouMotorRealTest, QuickStopInProfilePosition)
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
const std::chrono::milliseconds quick_stop_time{1000};
const double start_q = motor->getQ();
@ -484,8 +490,7 @@ TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
const std::chrono::milliseconds quick_stop_time{2000};
const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
@ -521,8 +526,7 @@ TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
std::this_thread::sleep_for(kModeSettleDelay);
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
const std::chrono::milliseconds run_duration{2000};
const double start_q = motor->getQ();