Skip to content

Add velocity feedback filter parameters and update protocol map #41

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
GiulioRomualdi wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Add velocity feedback filter parameters and update protocol map #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6f75ec6
Add velocity feedback filter parameters and update protocol map
GiulioRomualdi Oct 6, 2025
cb512ec
Add velocity feedback filter configuration support
GiulioRomualdi Oct 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions config/robot/template_1_motor/motion_control/all_joint_mc.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,9 @@ BSD-3-Clause license. -->
<param name="velocity_feedback_motor"> ( "606C" )</param>
<param name="position_feedback_joint"> ( "6064" )</param>
<param name="velocity_feedback_joint"> ( "606C" )</param>
<!-- Optional: enable low-pass filtering on velocity feedback -->
<param name="velocity_filter_enable">( false )</param>
<param name="velocity_filter_cutoff_hz">( 500 )</param>
<param name="position_window_deg"> ( 0.1 )</param>
<param name="timing_window_ms"> ( 100 )</param>
<param name="pos_limit_min_deg"> ( -23.0 )</param>
Expand Down
167 changes: 156 additions & 11 deletions device/CiA402MotionControl/CiA402MotionControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,10 @@ struct CiA402MotionControl::Impl
std::vector<double> degSToVel; // multiply deg/s to get device value
std::vector<bool> invertedMotionSenseDirection; // if true the torque, current, velocity and
// position have inverted sign
bool velocityFilterConfigPending{false};
std::vector<bool> velocityFilterEnableCfg;
std::vector<uint32_t> velocityFilterCutoffHzCfg;
std::string velocityFilterEnableSummary;
struct Limits
{
std::vector<double> maxPositionLimitDeg; // [deg] from 0x607D:1
Expand Down Expand Up @@ -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<int>(j);
const uint8_t filterType = velocityFilterEnableCfg[j] ? 1u : 0u;

auto typeErr = ethercatManager.writeSDO<uint8_t>(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<uint32_t>(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<unsigned>(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) ----
Expand Down Expand Up @@ -1805,6 +1875,17 @@ bool CiA402MotionControl::open(yarp::os::Searchable& cfg)
return true;
};

auto vecBoolToString = [](const std::vector<bool>& 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"
Expand Down Expand Up @@ -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<bool> velocityFilterEnable(m_impl->numAxes, false);
std::vector<double> 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<uint32_t>(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]));
Expand All @@ -1872,17 +2022,6 @@ bool CiA402MotionControl::open(yarp::os::Searchable& cfg)
m_impl->invertedMotionSenseDirection))
return false;

auto vecBoolToString = [](const std::vector<bool>& 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,
Expand Down Expand Up @@ -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)
// ---------------------------------------------------------------------
Expand Down
2 changes: 2 additions & 0 deletions doc/protocol_map.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 |

Expand Down
Loading