YARP CiA-402 EtherCAT Device 0.6.0
YARP device plugin for EtherCAT CiA-402 drives
Loading...
Searching...
No Matches
Classes | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Static Public Attributes | List of all members
yarp::dev::CiA402MotionControl::Impl Struct Reference
Collaboration diagram for yarp::dev::CiA402MotionControl::Impl:
[legend]

Classes

struct  ControlModeState
 
struct  Limits
 
struct  PositionProfileState
 
struct  SetPoints
 
struct  VariablesReadFromMotors
 

Public Types

enum class  PosSrc { S6064 , Enc1 , Enc2 }
 
enum class  VelSrc { S606C , Enc1 , Enc2 }
 
enum class  Mount { None , Motor , Joint }
 
enum class  SensorSrc { Unknown , Enc1 , Enc2 }
 

Public Member Functions

 Impl ()=default
 
 ~Impl ()=default
 
int32_t jointDegToTargetCounts (size_t j, double jointDeg) const
 
double targetCountsToJointDeg (size_t j, int32_t counts) const
 
bool setPositionCountsLimits (int axis, int32_t minCounts, int32_t maxCounts)
 
bool readEncoderResolutions ()
 
bool readSiVelocityUnits ()
 
double loopCountsToJointDeg (std::size_t j, double counts) const
 
bool setPositionWindowDeg (int j, double winDeg, double winTime_ms)
 
bool getPositionControlStrategy (uint16_t &strategyValue)
 
bool setPositionControlStrategy (uint16_t strategyValue)
 
bool readSimplePidGains (std::vector< double > &kpNmPerDeg, std::vector< double > &kdNmSecPerDeg)
 
bool configureSimplePidGains (const std::vector< double > &kpNmPerDeg, const std::vector< double > &kdNmSecPerDeg)
 
bool readMotorConstants ()
 
bool readGearRatios ()
 
bool readTorqueValues ()
 
void setSDORefSpeed (int j, double spDegS)
 
bool setSetPoints ()
 
bool readFeedback ()
 
int ciaOpToYarpWithFlavor (int op, int cstFlavor)
 
std::string describe603F (uint16_t code)
 

Static Public Member Functions

static std::tuple< double, double, const char * > decode60A9 (uint32_t v)
 
static int yarpToCiaOp (int cm)
 
static int ciaOpToYarp (int op)
 
static std::string_view yarpToString (int op)
 

Public Attributes

::CiA402::EthercatManager ethercatManager
 
std::mutex CiA402MotionControlMutex
 
size_t numAxes {0}
 
int firstSlave {1}
 
std::string expectedName {}
 
std::vector< double > gearRatio
 
std::vector< double > gearRatioInv
 
std::vector< double > torqueConstants
 
std::vector< double > maxCurrentsA
 
std::vector< double > ratedMotorTorqueNm
 
std::vector< double > maxMotorTorqueNm
 
std::vector< uint32_t > enc1Res
 
std::vector< uint32_t > enc2Res
 
std::vector< double > enc1ResInv
 
std::vector< double > enc2ResInv
 
std::vector< Mount > enc1Mount
 
std::vector< Mount > enc2Mount
 
std::vector< PosSrc > posSrcJoint
 
std::vector< PosSrc > posSrcMotor
 
std::vector< VelSrc > velSrcJoint
 
std::vector< VelSrc > velSrcMotor
 
std::vector< SensorSrc > posLoopSrc
 
std::vector< SensorSrc > velLoopSrc
 
std::vector< double > velToDegS
 
std::vector< double > degSToVel
 
std::vector< bool > invertedMotionSenseDirection
 
struct yarp::dev::CiA402MotionControl::Impl::Limits limits
 
VariablesReadFromMotors variables
 
ControlModeState controlModeState
 
SetPoints setPoints
 
std::vector< std::unique_ptr< CiA402::StateMachine > > sm
 
std::vector< bool > velLatched
 
std::vector< bool > trqLatched
 
std::vector< bool > currLatched
 
std::vector< bool > posLatched
 
std::vector< double > torqueSeedNm
 
std::vector< int > lastActiveMode
 
std::vector< uint32_t > tsLastRaw
 
std::vector< uint64_t > tsWraps
 
PositionProfileState ppState
 
bool opRequested {false}
 
std::chrono::steady_clock::time_point avgWindowStart {std::chrono::steady_clock::now()}
 
double runTimeAccumSec {0.0}
 
std::size_t runTimeSamples {0}
 
yarp::dev::InteractionModeEnum dummyInteractionMode
 

Static Public Attributes

static constexpr std::string_view kClassName = "CiA402MotionControl"
 
static constexpr double MICROSECONDS_TO_SECONDS = 1e-6
 
static constexpr uint64_t TIMESTAMP_WRAP_PERIOD_US = (((uint64_t)1 << 32) - 1) / 100 + 1
 
static constexpr uint32_t TIMESTAMP_WRAP_HALF_RANGE = TIMESTAMP_WRAP_PERIOD_US / 2
 

Detailed Description

Private implementation holding configuration, runtime state, and helpers. Concurrency:

  • variables.mutex guards feedback snapshots returned to YARP APIs
  • controlModeState.mutex guards target/active control modes
  • setPoints.mutex guards user setpoints and their first‑cycle latches Units:
  • Positions: degrees (joint and motor)
  • Velocities: degrees/second
  • Torques: joint Nm (converted to motor Nm and then to per‑thousand for PDOs)

Definition at line 39 of file CiA402MotionControl.cpp.

Member Enumeration Documentation

◆ Mount

enum class yarp::dev::CiA402MotionControl::Impl::Mount
strong
Enumerator
None 
Motor 
Joint 

Definition at line 55 of file CiA402MotionControl.cpp.

◆ PosSrc

enum class yarp::dev::CiA402MotionControl::Impl::PosSrc
strong
Enumerator
S6064 
Enc1 
Enc2 

Definition at line 41 of file CiA402MotionControl.cpp.

◆ SensorSrc

enum class yarp::dev::CiA402MotionControl::Impl::SensorSrc
strong
Enumerator
Unknown 
Enc1 
Enc2 

Definition at line 62 of file CiA402MotionControl.cpp.

◆ VelSrc

enum class yarp::dev::CiA402MotionControl::Impl::VelSrc
strong
Enumerator
S606C 
Enc1 
Enc2 

Definition at line 48 of file CiA402MotionControl.cpp.

Constructor & Destructor Documentation

◆ Impl()

yarp::dev::CiA402MotionControl::Impl::Impl ( )
default

◆ ~Impl()

yarp::dev::CiA402MotionControl::Impl::~Impl ( )
default

Member Function Documentation

◆ ciaOpToYarp()

static int yarp::dev::CiA402MotionControl::Impl::ciaOpToYarp ( int op)
inlinestatic

Definition at line 1597 of file CiA402MotionControl.cpp.

◆ ciaOpToYarpWithFlavor()

int yarp::dev::CiA402MotionControl::Impl::ciaOpToYarpWithFlavor ( int op,
int cstFlavor )
inline

Definition at line 1588 of file CiA402MotionControl.cpp.

◆ configureSimplePidGains()

bool yarp::dev::CiA402MotionControl::Impl::configureSimplePidGains ( const std::vector< double > & kpNmPerDeg,
const std::vector< double > & kdNmSecPerDeg )
inline

Definition at line 731 of file CiA402MotionControl.cpp.

◆ decode60A9()

static std::tuple< double, double, const char * > yarp::dev::CiA402MotionControl::Impl::decode60A9 ( uint32_t v)
inlinestatic

Definition at line 472 of file CiA402MotionControl.cpp.

◆ describe603F()

std::string yarp::dev::CiA402MotionControl::Impl::describe603F ( uint16_t code)
inline

Decode CiA 402 error code (0x603F:00) into a human‑readable string. Includes explicit vendor codes seen in the field and broader family fallbacks.

Definition at line 1647 of file CiA402MotionControl.cpp.

◆ getPositionControlStrategy()

bool yarp::dev::CiA402MotionControl::Impl::getPositionControlStrategy ( uint16_t & strategyValue)
inline

Definition at line 626 of file CiA402MotionControl.cpp.

◆ jointDegToTargetCounts()

int32_t yarp::dev::CiA402MotionControl::Impl::jointDegToTargetCounts ( size_t j,
double jointDeg ) const
inline

Definition at line 306 of file CiA402MotionControl.cpp.

◆ loopCountsToJointDeg()

double yarp::dev::CiA402MotionControl::Impl::loopCountsToJointDeg ( std::size_t j,
double counts ) const
inline

Definition at line 536 of file CiA402MotionControl.cpp.

◆ readEncoderResolutions()

bool yarp::dev::CiA402MotionControl::Impl::readEncoderResolutions ( )
inline

Definition at line 428 of file CiA402MotionControl.cpp.

◆ readFeedback()

bool yarp::dev::CiA402MotionControl::Impl::readFeedback ( )
inline

Definition at line 1284 of file CiA402MotionControl.cpp.

◆ readGearRatios()

bool yarp::dev::CiA402MotionControl::Impl::readGearRatios ( )
inline

Read the gear-ratio (motor-revs : load-revs) for every axis and cache both the ratio and its inverse.

0x6091:01 numerator (UNSIGNED32, default 1) 0x6091:02 denominator (UNSIGNED32, default 1)

If the object is missing or invalid the code silently falls back to 1:1.

Definition at line 906 of file CiA402MotionControl.cpp.

◆ readMotorConstants()

bool yarp::dev::CiA402MotionControl::Impl::readMotorConstants ( )
inline

Definition at line 826 of file CiA402MotionControl.cpp.

◆ readSimplePidGains()

bool yarp::dev::CiA402MotionControl::Impl::readSimplePidGains ( std::vector< double > & kpNmPerDeg,
std::vector< double > & kdNmSecPerDeg )
inline

Definition at line 678 of file CiA402MotionControl.cpp.

◆ readSiVelocityUnits()

bool yarp::dev::CiA402MotionControl::Impl::readSiVelocityUnits ( )
inline

Definition at line 498 of file CiA402MotionControl.cpp.

◆ readTorqueValues()

bool yarp::dev::CiA402MotionControl::Impl::readTorqueValues ( )
inline

Definition at line 964 of file CiA402MotionControl.cpp.

◆ setPositionControlStrategy()

bool yarp::dev::CiA402MotionControl::Impl::setPositionControlStrategy ( uint16_t strategyValue)
inline

Definition at line 649 of file CiA402MotionControl.cpp.

◆ setPositionCountsLimits()

bool yarp::dev::CiA402MotionControl::Impl::setPositionCountsLimits ( int axis,
int32_t minCounts,
int32_t maxCounts )
inline

Definition at line 393 of file CiA402MotionControl.cpp.

◆ setPositionWindowDeg()

bool yarp::dev::CiA402MotionControl::Impl::setPositionWindowDeg ( int j,
double winDeg,
double winTime_ms )
inline

Definition at line 582 of file CiA402MotionControl.cpp.

◆ setSDORefSpeed()

void yarp::dev::CiA402MotionControl::Impl::setSDORefSpeed ( int j,
double spDegS )
inline

Definition at line 1006 of file CiA402MotionControl.cpp.

◆ setSetPoints()

bool yarp::dev::CiA402MotionControl::Impl::setSetPoints ( )
inline

Push user setpoints into RxPDOs according to the active control mode.

  • Torque (CST): joint Nm → motor Nm → per‑thousand of rated motor torque (0x6071)
  • Velocity (CSV): joint deg/s → loop‑shaft deg/s (mount aware) → rpm (0x60FF) First‑cycle latches (velLatched/trqLatched) zero the command once when entering.

Definition at line 1040 of file CiA402MotionControl.cpp.

◆ targetCountsToJointDeg()

double yarp::dev::CiA402MotionControl::Impl::targetCountsToJointDeg ( size_t j,
int32_t counts ) const
inline

Definition at line 349 of file CiA402MotionControl.cpp.

◆ yarpToCiaOp()

static int yarp::dev::CiA402MotionControl::Impl::yarpToCiaOp ( int cm)
inlinestatic

Definition at line 1563 of file CiA402MotionControl.cpp.

◆ yarpToString()

static std::string_view yarp::dev::CiA402MotionControl::Impl::yarpToString ( int op)
inlinestatic

Definition at line 1619 of file CiA402MotionControl.cpp.

Member Data Documentation

◆ avgWindowStart

std::chrono::steady_clock::time_point yarp::dev::CiA402MotionControl::Impl::avgWindowStart {std::chrono::steady_clock::now()}

Definition at line 293 of file CiA402MotionControl.cpp.

◆ CiA402MotionControlMutex

std::mutex yarp::dev::CiA402MotionControl::Impl::CiA402MotionControlMutex

Definition at line 76 of file CiA402MotionControl.cpp.

◆ controlModeState

ControlModeState yarp::dev::CiA402MotionControl::Impl::controlModeState

Definition at line 180 of file CiA402MotionControl.cpp.

◆ currLatched

std::vector<bool> yarp::dev::CiA402MotionControl::Impl::currLatched

Definition at line 272 of file CiA402MotionControl.cpp.

◆ degSToVel

std::vector<double> yarp::dev::CiA402MotionControl::Impl::degSToVel

Definition at line 111 of file CiA402MotionControl.cpp.

◆ dummyInteractionMode

yarp::dev::InteractionModeEnum yarp::dev::CiA402MotionControl::Impl::dummyInteractionMode
Initial value:
{
yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF}

Dummy interaction mode for all the joints

Definition at line 298 of file CiA402MotionControl.cpp.

◆ enc1Mount

std::vector<Mount> yarp::dev::CiA402MotionControl::Impl::enc1Mount

Definition at line 105 of file CiA402MotionControl.cpp.

◆ enc1Res

std::vector<uint32_t> yarp::dev::CiA402MotionControl::Impl::enc1Res

Definition at line 103 of file CiA402MotionControl.cpp.

◆ enc1ResInv

std::vector<double> yarp::dev::CiA402MotionControl::Impl::enc1ResInv

Definition at line 104 of file CiA402MotionControl.cpp.

◆ enc2Mount

std::vector<Mount> yarp::dev::CiA402MotionControl::Impl::enc2Mount

Definition at line 105 of file CiA402MotionControl.cpp.

◆ enc2Res

std::vector<uint32_t> yarp::dev::CiA402MotionControl::Impl::enc2Res

Definition at line 103 of file CiA402MotionControl.cpp.

◆ enc2ResInv

std::vector<double> yarp::dev::CiA402MotionControl::Impl::enc2ResInv

Definition at line 104 of file CiA402MotionControl.cpp.

◆ ethercatManager

::CiA402::EthercatManager yarp::dev::CiA402MotionControl::Impl::ethercatManager

Definition at line 73 of file CiA402MotionControl.cpp.

◆ expectedName

std::string yarp::dev::CiA402MotionControl::Impl::expectedName {}

Definition at line 92 of file CiA402MotionControl.cpp.

◆ firstSlave

int yarp::dev::CiA402MotionControl::Impl::firstSlave {1}

Definition at line 91 of file CiA402MotionControl.cpp.

◆ gearRatio

std::vector<double> yarp::dev::CiA402MotionControl::Impl::gearRatio

Definition at line 97 of file CiA402MotionControl.cpp.

◆ gearRatioInv

std::vector<double> yarp::dev::CiA402MotionControl::Impl::gearRatioInv

Definition at line 98 of file CiA402MotionControl.cpp.

◆ invertedMotionSenseDirection

std::vector<bool> yarp::dev::CiA402MotionControl::Impl::invertedMotionSenseDirection

Definition at line 112 of file CiA402MotionControl.cpp.

◆ kClassName

std::string_view yarp::dev::CiA402MotionControl::Impl::kClassName = "CiA402MotionControl"
staticconstexpr

Definition at line 70 of file CiA402MotionControl.cpp.

◆ lastActiveMode

std::vector<int> yarp::dev::CiA402MotionControl::Impl::lastActiveMode

Definition at line 275 of file CiA402MotionControl.cpp.

◆ limits

struct yarp::dev::CiA402MotionControl::Impl::Limits yarp::dev::CiA402MotionControl::Impl::limits

◆ maxCurrentsA

std::vector<double> yarp::dev::CiA402MotionControl::Impl::maxCurrentsA

Definition at line 100 of file CiA402MotionControl.cpp.

◆ maxMotorTorqueNm

std::vector<double> yarp::dev::CiA402MotionControl::Impl::maxMotorTorqueNm

Definition at line 102 of file CiA402MotionControl.cpp.

◆ MICROSECONDS_TO_SECONDS

double yarp::dev::CiA402MotionControl::Impl::MICROSECONDS_TO_SECONDS = 1e-6
staticconstexpr

Definition at line 79 of file CiA402MotionControl.cpp.

◆ numAxes

size_t yarp::dev::CiA402MotionControl::Impl::numAxes {0}

Definition at line 90 of file CiA402MotionControl.cpp.

◆ opRequested

bool yarp::dev::CiA402MotionControl::Impl::opRequested {false}

Definition at line 290 of file CiA402MotionControl.cpp.

◆ posLatched

std::vector<bool> yarp::dev::CiA402MotionControl::Impl::posLatched

Definition at line 273 of file CiA402MotionControl.cpp.

◆ posLoopSrc

std::vector<SensorSrc> yarp::dev::CiA402MotionControl::Impl::posLoopSrc

Definition at line 108 of file CiA402MotionControl.cpp.

◆ posSrcJoint

std::vector<PosSrc> yarp::dev::CiA402MotionControl::Impl::posSrcJoint

Definition at line 106 of file CiA402MotionControl.cpp.

◆ posSrcMotor

std::vector<PosSrc> yarp::dev::CiA402MotionControl::Impl::posSrcMotor

Definition at line 106 of file CiA402MotionControl.cpp.

◆ ppState

PositionProfileState yarp::dev::CiA402MotionControl::Impl::ppState

Definition at line 288 of file CiA402MotionControl.cpp.

◆ ratedMotorTorqueNm

std::vector<double> yarp::dev::CiA402MotionControl::Impl::ratedMotorTorqueNm

Definition at line 101 of file CiA402MotionControl.cpp.

◆ runTimeAccumSec

double yarp::dev::CiA402MotionControl::Impl::runTimeAccumSec {0.0}

Definition at line 294 of file CiA402MotionControl.cpp.

◆ runTimeSamples

std::size_t yarp::dev::CiA402MotionControl::Impl::runTimeSamples {0}

Definition at line 295 of file CiA402MotionControl.cpp.

◆ setPoints

SetPoints yarp::dev::CiA402MotionControl::Impl::setPoints

Definition at line 266 of file CiA402MotionControl.cpp.

◆ sm

std::vector<std::unique_ptr<CiA402::StateMachine> > yarp::dev::CiA402MotionControl::Impl::sm

Definition at line 269 of file CiA402MotionControl.cpp.

◆ TIMESTAMP_WRAP_HALF_RANGE

uint32_t yarp::dev::CiA402MotionControl::Impl::TIMESTAMP_WRAP_HALF_RANGE = TIMESTAMP_WRAP_PERIOD_US / 2
staticconstexpr

Definition at line 87 of file CiA402MotionControl.cpp.

◆ TIMESTAMP_WRAP_PERIOD_US

uint64_t yarp::dev::CiA402MotionControl::Impl::TIMESTAMP_WRAP_PERIOD_US = (((uint64_t)1 << 32) - 1) / 100 + 1
staticconstexpr

Definition at line 83 of file CiA402MotionControl.cpp.

◆ torqueConstants

std::vector<double> yarp::dev::CiA402MotionControl::Impl::torqueConstants

Definition at line 99 of file CiA402MotionControl.cpp.

◆ torqueSeedNm

std::vector<double> yarp::dev::CiA402MotionControl::Impl::torqueSeedNm

Definition at line 274 of file CiA402MotionControl.cpp.

◆ trqLatched

std::vector<bool> yarp::dev::CiA402MotionControl::Impl::trqLatched

Definition at line 272 of file CiA402MotionControl.cpp.

◆ tsLastRaw

std::vector<uint32_t> yarp::dev::CiA402MotionControl::Impl::tsLastRaw

Definition at line 278 of file CiA402MotionControl.cpp.

◆ tsWraps

std::vector<uint64_t> yarp::dev::CiA402MotionControl::Impl::tsWraps

Definition at line 279 of file CiA402MotionControl.cpp.

◆ variables

VariablesReadFromMotors yarp::dev::CiA402MotionControl::Impl::variables

Definition at line 162 of file CiA402MotionControl.cpp.

◆ velLatched

std::vector<bool> yarp::dev::CiA402MotionControl::Impl::velLatched

Definition at line 272 of file CiA402MotionControl.cpp.

◆ velLoopSrc

std::vector<SensorSrc> yarp::dev::CiA402MotionControl::Impl::velLoopSrc

Definition at line 109 of file CiA402MotionControl.cpp.

◆ velSrcJoint

std::vector<VelSrc> yarp::dev::CiA402MotionControl::Impl::velSrcJoint

Definition at line 107 of file CiA402MotionControl.cpp.

◆ velSrcMotor

std::vector<VelSrc> yarp::dev::CiA402MotionControl::Impl::velSrcMotor

Definition at line 107 of file CiA402MotionControl.cpp.

◆ velToDegS

std::vector<double> yarp::dev::CiA402MotionControl::Impl::velToDegS

Definition at line 110 of file CiA402MotionControl.cpp.

The documentation for this struct was generated from the following file: