diff --git a/config/robot/template_1_motor/motion_control/all_joint_mc.xml b/config/robot/template_1_motor/motion_control/all_joint_mc.xml
index 1bd0533..d8ef443 100644
--- a/config/robot/template_1_motor/motion_control/all_joint_mc.xml
+++ b/config/robot/template_1_motor/motion_control/all_joint_mc.xml
@@ -16,6 +16,9 @@ BSD-3-Clause license. -->
( "606C" )
( "6064" )
( "606C" )
+
+ ( false )
+ ( 500 )
( 0.1 )
( 100 )
( -23.0 )
diff --git a/device/CiA402MotionControl/CiA402MotionControl.cpp b/device/CiA402MotionControl/CiA402MotionControl.cpp
index 7b9d50f..60f0a35 100644
--- a/device/CiA402MotionControl/CiA402MotionControl.cpp
+++ b/device/CiA402MotionControl/CiA402MotionControl.cpp
@@ -111,6 +111,10 @@ struct CiA402MotionControl::Impl
std::vector degSToVel; // multiply deg/s to get device value
std::vector invertedMotionSenseDirection; // if true the torque, current, velocity and
// position have inverted sign
+ bool velocityFilterConfigPending{false};
+ std::vector velocityFilterEnableCfg;
+ std::vector velocityFilterCutoffHzCfg;
+ std::string velocityFilterEnableSummary;
struct Limits
{
std::vector maxPositionLimitDeg; // [deg] from 0x607D:1
@@ -780,6 +784,72 @@ struct CiA402MotionControl::Impl
return true;
}
+ bool applyVelocityFilterConfig()
+ {
+ constexpr auto logPrefix = "[applyVelocityFilterConfig]";
+
+ if (!velocityFilterConfigPending)
+ {
+ return true;
+ }
+
+ if (velocityFilterEnableCfg.size() != numAxes
+ || velocityFilterCutoffHzCfg.size() != numAxes)
+ {
+ yCError(CIA402,
+ "%s velocity filter config size mismatch (enable=%zu cutoff=%zu expected=%zu)",
+ logPrefix,
+ velocityFilterEnableCfg.size(),
+ velocityFilterCutoffHzCfg.size(),
+ numAxes);
+ return false;
+ }
+
+ for (size_t j = 0; j < numAxes; ++j)
+ {
+ const int slaveIdx = firstSlave + static_cast(j);
+ const uint8_t filterType = velocityFilterEnableCfg[j] ? 1u : 0u;
+
+ auto typeErr = ethercatManager.writeSDO(slaveIdx, 0x2021, 0x01, filterType);
+ if (typeErr != ::CiA402::EthercatManager::Error::NoError)
+ {
+ yCError(CIA402,
+ "%s failed to write velocity filter type (0x2021:01) for axis %zu",
+ logPrefix,
+ j);
+ return false;
+ }
+
+ auto cutoffErr = ethercatManager.writeSDO(slaveIdx,
+ 0x2021,
+ 0x02,
+ velocityFilterCutoffHzCfg[j]);
+ if (cutoffErr != ::CiA402::EthercatManager::Error::NoError)
+ {
+ yCError(CIA402,
+ "%s failed to write velocity filter cutoff (0x2021:02) for axis %zu",
+ logPrefix,
+ j);
+ return false;
+ }
+
+ yCDebug(CIA402,
+ "%s j=%zu velocity feedback filter type=%u cutoff=%u Hz",
+ logPrefix,
+ j,
+ static_cast(filterType),
+ velocityFilterCutoffHzCfg[j]);
+ }
+
+ yCInfo(CIA402,
+ "%s configured velocity feedback filters (enable=%s)",
+ logPrefix,
+ velocityFilterEnableSummary.c_str());
+
+ velocityFilterConfigPending = false;
+ return true;
+ }
+
void setSDORefSpeed(int j, double spDegS)
{
// ---- map JOINT deg/s -> LOOP SHAFT deg/s (based on pos loop source + mount) ----
@@ -1805,6 +1875,17 @@ bool CiA402MotionControl::open(yarp::os::Searchable& cfg)
return true;
};
+ auto vecBoolToString = [](const std::vector& v) -> std::string {
+ std::string s;
+ for (size_t i = 0; i < v.size(); ++i)
+ {
+ s += (v[i] ? "true" : "false");
+ if (i + 1 < v.size())
+ s += ", ";
+ }
+ return s;
+ };
+
// Encoder mounting configuration (where each encoder is physically located)
// enc1_mount must be list of "motor" or "joint"
// enc2_mount must be list of "motor", "joint" or "none"
@@ -1849,6 +1930,75 @@ bool CiA402MotionControl::open(yarp::os::Searchable& cfg)
if (!extractListOfDoubleFromSearchable(cfg, "timing_window_ms", timingWindowMs))
return false;
+ const bool hasVelocityFilterEnable = cfg.check("velocity_filter_enable");
+ const bool hasVelocityFilterCutoff = cfg.check("velocity_filter_cutoff_hz");
+ if (hasVelocityFilterEnable || hasVelocityFilterCutoff)
+ {
+ constexpr double kCutoffDefaultHz = 500.0;
+ std::vector velocityFilterEnable(m_impl->numAxes, false);
+ std::vector velocityFilterCutoffHz(m_impl->numAxes, kCutoffDefaultHz);
+
+ if (hasVelocityFilterEnable)
+ {
+ if (!extractListOfBoolFromSearchable(cfg,
+ "velocity_filter_enable",
+ velocityFilterEnable))
+ {
+ yCError(CIA402, "%s failed to parse velocity_filter_enable", logPrefix);
+ return false;
+ }
+ }
+
+ if (hasVelocityFilterCutoff)
+ {
+ if (!extractListOfDoubleFromSearchable(cfg,
+ "velocity_filter_cutoff_hz",
+ velocityFilterCutoffHz))
+ {
+ yCError(CIA402, "%s failed to parse velocity_filter_cutoff_hz", logPrefix);
+ return false;
+ }
+ }
+
+ constexpr double kCutoffMinHz = 5.0;
+ constexpr double kCutoffMaxHz = 2000.0;
+
+ m_impl->velocityFilterEnableCfg = velocityFilterEnable;
+ m_impl->velocityFilterCutoffHzCfg.resize(m_impl->numAxes);
+
+ for (size_t j = 0; j < m_impl->numAxes; ++j)
+ {
+ const double requestedHz = velocityFilterCutoffHz[j];
+ if (!std::isfinite(requestedHz))
+ {
+ yCError(CIA402, "%s velocity_filter_cutoff_hz[%zu] is not finite", logPrefix, j);
+ return false;
+ }
+
+ const double clampedHz = std::clamp(requestedHz, kCutoffMinHz, kCutoffMaxHz);
+ if (clampedHz != requestedHz)
+ {
+ yCWarning(CIA402,
+ "%s: axis %zu cutoff %.3f Hz clamped to %.3f Hz",
+ logPrefix,
+ j,
+ requestedHz,
+ clampedHz);
+ }
+
+ m_impl->velocityFilterCutoffHzCfg[j] = static_cast(std::llround(clampedHz));
+ }
+
+ m_impl->velocityFilterEnableSummary = vecBoolToString(velocityFilterEnable);
+ m_impl->velocityFilterConfigPending = true;
+ } else
+ {
+ m_impl->velocityFilterConfigPending = false;
+ m_impl->velocityFilterEnableCfg.clear();
+ m_impl->velocityFilterCutoffHzCfg.clear();
+ m_impl->velocityFilterEnableSummary.clear();
+ }
+
for (size_t j = 0; j < m_impl->numAxes; ++j)
{
m_impl->enc1Mount.push_back(parseMount(enc1MStr[j]));
@@ -1872,17 +2022,6 @@ bool CiA402MotionControl::open(yarp::os::Searchable& cfg)
m_impl->invertedMotionSenseDirection))
return false;
- auto vecBoolToString = [](const std::vector& v) -> std::string {
- std::string s;
- for (size_t i = 0; i < v.size(); ++i)
- {
- s += (v[i] ? "true" : "false");
- if (i + 1 < v.size())
- s += ", ";
- }
- return s;
- };
-
yCDebug(CIA402,
"%s: inverted_motion_sense_direction = [%s]",
logPrefix,
@@ -2087,6 +2226,12 @@ bool CiA402MotionControl::open(yarp::os::Searchable& cfg)
return false;
}
+ if (!m_impl->applyVelocityFilterConfig())
+ {
+ yCError(CIA402, "%s failed to apply velocity filter configuration", logPrefix);
+ return false;
+ }
+
// ---------------------------------------------------------------------
// Set the position limits (SDO 607D)
// ---------------------------------------------------------------------
diff --git a/doc/protocol_map.md b/doc/protocol_map.md
index e4b6d9a..edb11df 100644
--- a/doc/protocol_map.md
+++ b/doc/protocol_map.md
@@ -48,6 +48,8 @@ Notes:
| Gear ratio denominator | 0x6091:02 | UINT32 | Read | Motor:Load gear ratio (denominator) | Defaults to 1 |
| Profile velocity | 0x6081:00 | INT32 | R/W | PP profile parameter | Units per drive |
| Profile acceleration | 0x6083:00 | INT32 | R/W | PP profile parameter | Units per drive |
+| Velocity feedback filter type | 0x2021:01 | UINT8 | R/W | Enable/disable velocity LPF | 0=off, 1=1st order |
+| Velocity feedback cutoff | 0x2021:02 | UINT32 | R/W | Velocity LPF cutoff frequency | Hz, 5–2000, default 500 |
| Rated motor torque | 0x6076:00 | INT16/* | Read | Scale for 0x6071 | Nm, vendor sizing may vary |
| Error code | 0x603F:00 | UINT16 | Read | Fault diagnostics | Decoded in driver |