Skip to content

Commit

Permalink
Parse common iPOS options
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed Jan 23, 2024
1 parent 2a109b1 commit 428a2fc
Show file tree
Hide file tree
Showing 5 changed files with 77 additions and 122 deletions.
82 changes: 37 additions & 45 deletions libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,84 +20,76 @@ constexpr auto DEFAULT_DRIVE_STATE_TIMEOUT = 2.0;

bool TechnosoftIposBase::open(yarp::os::Searchable & config)
{
const auto * robotConfig = *reinterpret_cast<const yarp::os::Property * const *>(config.find("robotConfig").asBlob());
yarp::os::Property iposOptions;
iposOptions.fromString(config.findGroup("common").toString());
iposOptions.fromString(config.toString(), false); // override common options

const auto & commonGroup = robotConfig->findGroup("common-ipos");
yarp::os::Property iposGroup;

if (!commonGroup.isNull())
{
iposGroup.fromString(commonGroup.toString());
}

iposGroup.fromString(config.toString(), false); // override common options

const auto & driverGroup = robotConfig->findGroup(iposGroup.find("driver").asString());
const auto & motorGroup = robotConfig->findGroup(iposGroup.find("motor").asString());
const auto & gearboxGroup = robotConfig->findGroup(iposGroup.find("gearbox").asString());
const auto & encoderGroup = robotConfig->findGroup(iposGroup.find("encoder").asString());
const auto & driverOptions = config.findGroup("driver");
const auto & motorOptions = config.findGroup("motor");
const auto & gearboxOptions = config.findGroup("gearbox");
const auto & encoderOptions = config.findGroup("encoder");

canId = config.check("canId", yarp::os::Value(0), "CAN node ID").asInt32(); // id-specific

actualControlMode = VOCAB_CM_NOT_CONFIGURED;

axisName = config.check("name", yarp::os::Value(""), "axis name").asString(); // id-specific
jointType = iposGroup.check("type", yarp::os::Value(yarp::dev::VOCAB_JOINTTYPE_UNKNOWN), "joint type [atrv|atpr|unkn]").asVocab32();
max = iposGroup.check("max", yarp::os::Value(0.0), "max (meters or degrees)").asFloat64();
min = iposGroup.check("min", yarp::os::Value(0.0), "min (meters or degrees)").asFloat64();
maxVel = iposGroup.check("maxVel", yarp::os::Value(0.0), "maxVel (meters/second or degrees/second)").asFloat64();
refSpeed = iposGroup.check("refSpeed", yarp::os::Value(0.0), "ref speed (meters/second or degrees/second)").asFloat64();
refAcceleration = iposGroup.check("refAcceleration", yarp::os::Value(0.0), "ref acceleration (meters/second^2 or degrees/second^2)").asFloat64();
drivePeakCurrent = driverGroup.check("peakCurrent", yarp::os::Value(0.0), "peak drive current (amperes)").asFloat64();
k = motorGroup.check("k", yarp::os::Value(0.0), "motor constant").asFloat64();
tr = gearboxGroup.check("tr", yarp::os::Value(0.0), "reduction").asFloat64();
tr = tr * iposGroup.check("extraTr", yarp::os::Value(1.0), "extra reduction").asFloat64();
encoderPulses = encoderGroup.check("encoderPulses", yarp::os::Value(0), "encoderPulses").asInt32();
samplingPeriod = iposGroup.check("samplingPeriod", yarp::os::Value(0.0), "samplingPeriod (seconds)").asFloat64();
reverse = iposGroup.check("reverse", yarp::os::Value(false), "reverse motor encoder counts").asBool();
heartbeatPeriod = iposGroup.check("heartbeatPeriod", yarp::os::Value(0.0), "CAN heartbeat period (seconds)").asFloat64();
syncPeriod = iposGroup.check("syncPeriod", yarp::os::Value(0.0), "SYNC message period (seconds)").asFloat64();
jointType = iposOptions.check("type", yarp::os::Value(yarp::dev::VOCAB_JOINTTYPE_UNKNOWN), "joint type [atrv|atpr|unkn]").asVocab32();
max = iposOptions.check("max", yarp::os::Value(0.0), "max (meters or degrees)").asFloat64();
min = iposOptions.check("min", yarp::os::Value(0.0), "min (meters or degrees)").asFloat64();
maxVel = iposOptions.check("maxVel", yarp::os::Value(0.0), "maxVel (meters/second or degrees/second)").asFloat64();
refSpeed = iposOptions.check("refSpeed", yarp::os::Value(0.0), "ref speed (meters/second or degrees/second)").asFloat64();
refAcceleration = iposOptions.check("refAcceleration", yarp::os::Value(0.0), "ref acceleration (meters/second^2 or degrees/second^2)").asFloat64();
drivePeakCurrent = driverOptions.check("peakCurrent", yarp::os::Value(0.0), "peak drive current (amperes)").asFloat64();
k = motorOptions.check("k", yarp::os::Value(0.0), "motor constant").asFloat64();
tr = gearboxOptions.check("tr", yarp::os::Value(0.0), "reduction").asFloat64();
tr = tr * iposOptions.check("extraTr", yarp::os::Value(1.0), "extra reduction").asFloat64();
encoderPulses = encoderOptions.check("encoderPulses", yarp::os::Value(0), "encoderPulses").asInt32();
samplingPeriod = iposOptions.check("samplingPeriod", yarp::os::Value(0.0), "samplingPeriod (seconds)").asFloat64();
reverse = iposOptions.check("reverse", yarp::os::Value(false), "reverse motor encoder counts").asBool();
heartbeatPeriod = iposOptions.check("heartbeatPeriod", yarp::os::Value(0.0), "CAN heartbeat period (seconds)").asFloat64();
syncPeriod = iposOptions.check("syncPeriod", yarp::os::Value(0.0), "SYNC message period (seconds)").asFloat64();
// back-compat
auto initialMode = iposGroup.check("initialMode", yarp::os::Value(VOCAB_CM_IDLE), "initial YARP control mode vocab").asVocab32();
initialControlMode = iposGroup.check("initialControlMode", yarp::os::Value(initialMode), "initial YARP control mode vocab").asVocab32();
auto initialMode = iposOptions.check("initialMode", yarp::os::Value(VOCAB_CM_IDLE), "initial YARP control mode vocab").asVocab32();
initialControlMode = iposOptions.check("initialControlMode", yarp::os::Value(initialMode), "initial YARP control mode vocab").asVocab32();

if (!validateInitialState())
{
yCIError(IPOS, id()) << "Invalid configuration parameters";
return false;
}

double sdoTimeout = iposGroup.check("sdoTimeout", yarp::os::Value(DEFAULT_SDO_TIMEOUT),
double sdoTimeout = iposOptions.check("sdoTimeout", yarp::os::Value(DEFAULT_SDO_TIMEOUT),
"CAN SDO timeout (seconds)").asFloat64();
double driveStateTimeout = iposGroup.check("driveStateTimeout", yarp::os::Value(DEFAULT_DRIVE_STATE_TIMEOUT),
double driveStateTimeout = iposOptions.check("driveStateTimeout", yarp::os::Value(DEFAULT_DRIVE_STATE_TIMEOUT),
"CAN drive state timeout (seconds)").asFloat64();

can = new CanOpenNode(canId, sdoTimeout, driveStateTimeout);

// Manufacturer Status Register (1002h) and Modes of Operation Display (6061h)
tpdo1Conf.addMapping<std::uint32_t>(0x1002).addMapping<std::int8_t>(0x6061);

if (iposGroup.check("tpdo1InhibitTime", "TPDO1 inhibit time (seconds)"))
if (iposOptions.check("tpdo1InhibitTime", "TPDO1 inhibit time (seconds)"))
{
tpdo1Conf.setInhibitTime(iposGroup.find("tpdo1InhibitTime").asFloat64() * 1e4); // pass x100 microseconds
tpdo1Conf.setInhibitTime(iposOptions.find("tpdo1InhibitTime").asFloat64() * 1e4); // pass x100 microseconds
}

if (iposGroup.check("tpdo1EventTimer", "TPDO1 event timer (seconds)"))
if (iposOptions.check("tpdo1EventTimer", "TPDO1 event timer (seconds)"))
{
tpdo1Conf.setEventTimer(iposGroup.find("tpdo1EventTimer").asFloat64() * 1e3); // pass milliseconds
tpdo1Conf.setEventTimer(iposOptions.find("tpdo1EventTimer").asFloat64() * 1e3); // pass milliseconds
}

// Motion Error Register (2000h) and Detailed Error Register (2002h)
tpdo2Conf.addMapping<std::uint16_t>(0x2000).addMapping<std::uint16_t>(0x2002);

if (iposGroup.check("tpdo2InhibitTime", "TPDO2 inhibit time (seconds)"))
if (iposOptions.check("tpdo2InhibitTime", "TPDO2 inhibit time (seconds)"))
{
tpdo2Conf.setInhibitTime(iposGroup.find("tpdo2InhibitTime").asFloat64() * 1e4); // pass x100 microseconds
tpdo2Conf.setInhibitTime(iposOptions.find("tpdo2InhibitTime").asFloat64() * 1e4); // pass x100 microseconds
}

if (iposGroup.check("tpdo2EventTimer", "TPDO2 event timer (seconds)"))
if (iposOptions.check("tpdo2EventTimer", "TPDO2 event timer (seconds)"))
{
tpdo2Conf.setEventTimer(iposGroup.find("tpdo2EventTimer").asFloat64() * 1e3); // pass milliseconds
tpdo2Conf.setEventTimer(iposOptions.find("tpdo2EventTimer").asFloat64() * 1e3); // pass milliseconds
}

// Position actual internal value (6063h) and Torque actual value (6077h)
Expand All @@ -116,9 +108,9 @@ bool TechnosoftIposBase::open(yarp::os::Searchable & config)

can->nmt()->registerHandler(std::bind(&TechnosoftIposBase::handleNmt, this, _1));

if (iposGroup.check("monitorPeriod", "monitor thread period (seconds)"))
if (iposOptions.check("monitorPeriod", "monitor thread period (seconds)"))
{
double monitorPeriod = iposGroup.find("monitorPeriod").asFloat64();
double monitorPeriod = iposOptions.find("monitorPeriod").asFloat64();

if (monitorPeriod > 0.0)
{
Expand All @@ -130,7 +122,7 @@ bool TechnosoftIposBase::open(yarp::os::Searchable & config)
}
}

if (yarp::os::Value * val; iposGroup.check("disabledEncoders", val, "disabled encoder IDs") && val->isList())
if (yarp::os::Value * val; iposOptions.check("disabledEncoders", val, "disabled encoder IDs") && val->isList())
{
const auto * ids = val->asList();

Expand Down
36 changes: 9 additions & 27 deletions libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,51 +15,33 @@

using namespace roboticslab;

constexpr auto DEFAULT_USE_EMBEDDED = true;

// -----------------------------------------------------------------------------

bool TechnosoftIpos::open(yarp::os::Searchable & config)
{
if (!config.check("robotConfig") || !config.find("robotConfig").isBlob())
{
yCError(IPOS) << "Missing \"robotConfig\" property or not a blob";
return false;
}

const auto * robotConfig = *reinterpret_cast<const yarp::os::Property * const *>(config.find("robotConfig").asBlob());

const auto & commonGroup = robotConfig->findGroup("common-ipos");
yarp::os::Property iposGroup;

if (!commonGroup.isNull())
{
yCDebugOnce(IPOS) << commonGroup.toString();
iposGroup.fromString(commonGroup.toString());
}

iposGroup.fromString(config.toString(), false); // override common options

auto canId = config.check("canId", yarp::os::Value(0), "CAN node ID").asInt32(); // id-specific
auto useEmbeddedPid = iposGroup.check("useEmbeddedPid", yarp::os::Value(true), "use embedded PID").asBool();

const auto id = "ID" + std::to_string(canId);
yarp::os::Property options;
options.fromString(config.findGroup("common").toString());
options.fromString(config.toString(), false); // override common options

if (useEmbeddedPid)
if (options.check("useEmbeddedPid", yarp::os::Value(DEFAULT_USE_EMBEDDED), "use embedded PID").asBool())
{
yCIInfo(IPOS, id) << "Using embedded PID implementation";
yCIInfo(IPOS, id()) << "Using embedded PID implementation";
impl = new TechnosoftIposEmbedded;
}
else
{
#ifdef HAVE_EXTERNAL_PID_IMPL
yCIInfo(IPOS, id) << "Using external PID implementation";
yCIInfo(IPOS, id()) << "Using external PID implementation";
impl = new TechnosoftIposExternal;
#else
yCError(IPOS) << "External PID implementation not available";
return false;
#endif
}

impl->setId(id);
impl->setId(id());
return impl->open(config);
}

Expand Down
3 changes: 0 additions & 3 deletions libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,6 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver,
bool open(yarp::os::Searchable & config) override;
bool close() override;

std::string id() const override
{ return impl->id(); }

// --------- IWrapper declarations. Implementation in IWrapperImpl.cpp ---------

bool attach(yarp::dev::PolyDriver * driver) override
Expand Down
24 changes: 8 additions & 16 deletions libraries/YarpPlugins/TechnosoftIpos/embedded-pid/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,22 +17,14 @@ constexpr auto DEFAULT_ENABLE_CSV = false;

bool TechnosoftIposEmbedded::open(yarp::os::Searchable & config)
{
const auto * robotConfig = *reinterpret_cast<const yarp::os::Property * const *>(config.find("robotConfig").asBlob());

const auto & commonGroup = robotConfig->findGroup("common-ipos");
yarp::os::Property iposGroup;

if (!commonGroup.isNull())
{
iposGroup.fromString(commonGroup.toString());
}

iposGroup.fromString(config.toString(), false); // override common options

auto ipModeVal = iposGroup.check("ipMode", yarp::os::Value(DEFAULT_IP_MODE), "IP mode (pt, pvt)");
auto ipPeriodMsVal = iposGroup.check("ipPeriodMs", yarp::os::Value(DEFAULT_IP_PERIOD_MS), "IP period (ms)");
auto enableIpVal = iposGroup.check("enableIp", yarp::os::Value(DEFAULT_ENABLE_IP), "enable IP mode");
auto enableCsvVal = iposGroup.check("enableCsv", yarp::os::Value(DEFAULT_ENABLE_CSV), "enable CSV mode");
yarp::os::Property options;
options.fromString(config.findGroup("common").toString());
options.fromString(config.toString(), false); // override common options

auto ipModeVal = options.check("ipMode", yarp::os::Value(DEFAULT_IP_MODE), "IP mode (pt, pvt)");
auto ipPeriodMsVal = options.check("ipPeriodMs", yarp::os::Value(DEFAULT_IP_PERIOD_MS), "IP period (ms)");
auto enableIpVal = options.check("enableIp", yarp::os::Value(DEFAULT_ENABLE_IP), "enable IP mode");
auto enableCsvVal = options.check("enableCsv", yarp::os::Value(DEFAULT_ENABLE_CSV), "enable CSV mode");

if (!setRemoteVariableRaw("ipMode", {ipModeVal}) || !setRemoteVariableRaw("ipPeriodMs", {ipPeriodMsVal}) ||
!setRemoteVariableRaw("enableIp", {enableIpVal}) || !setRemoteVariableRaw("enableCsv", {enableCsvVal}))
Expand Down
54 changes: 23 additions & 31 deletions libraries/YarpPlugins/TechnosoftIpos/external-pid/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,36 +14,28 @@ constexpr auto DEFAULT_ENABLE_CSV = false;

bool TechnosoftIposExternal::open(yarp::os::Searchable & config)
{
const auto * robotConfig = *reinterpret_cast<const yarp::os::Property * const *>(config.find("robotConfig").asBlob());
yarp::os::Property options;
options.fromString(config.findGroup("common").toString());
options.fromString(config.toString(), false); // override common options

const auto & commonGroup = robotConfig->findGroup("common-ipos");
yarp::os::Property iposGroup;

if (!commonGroup.isNull())
{
iposGroup.fromString(commonGroup.toString());
}

iposGroup.fromString(config.toString(), false); // override common options

auto enableCsvVal = iposGroup.check("enableCsv", yarp::os::Value(DEFAULT_ENABLE_CSV), "enable CSV mode");
auto enableCsvVal = options.check("enableCsv", yarp::os::Value(DEFAULT_ENABLE_CSV), "enable CSV mode");

if (!setRemoteVariableRaw("enableCsv", {enableCsvVal}))
{
return false;
}

initialInteractionMode = iposGroup.check("initialInteractionMode", yarp::os::Value(yarp::dev::InteractionModeEnum::VOCAB_IM_UNKNOWN),
initialInteractionMode = options.check("initialInteractionMode", yarp::os::Value(yarp::dev::InteractionModeEnum::VOCAB_IM_UNKNOWN),
"initial YARP interaction mode vocab").asVocab32();

auto stiffness = iposGroup.check("stiffness", yarp::os::Value(0.0), "impedance stiffness (Nm)").asFloat64();
auto damping = iposGroup.check("damping", yarp::os::Value(0.0), "impedance damping (Nm*seconds)").asFloat64();
auto impedanceOffset = iposGroup.check("impedanceOffset", yarp::os::Value(0.0), "impedance offset").asFloat64();
auto stiffness = options.check("stiffness", yarp::os::Value(0.0), "impedance stiffness (Nm)").asFloat64();
auto damping = options.check("damping", yarp::os::Value(0.0), "impedance damping (Nm*seconds)").asFloat64();
auto impedanceOffset = options.check("impedanceOffset", yarp::os::Value(0.0), "impedance offset").asFloat64();

minStiffness = iposGroup.check("minStiffness", yarp::os::Value(0.0), "minimum impedance stiffness (Nm)").asFloat64();
maxStiffness = iposGroup.check("maxStiffness", yarp::os::Value(0.0), "maximum impedance stiffness (Nm)").asFloat64();
minDamping = iposGroup.check("minDamping", yarp::os::Value(0.0), "minimum impedance damping (Nm*seconds)").asFloat64();
maxDamping = iposGroup.check("maxDamping", yarp::os::Value(0.0), "maximum impedance damping (Nm*seconds)").asFloat64();
minStiffness = options.check("minStiffness", yarp::os::Value(0.0), "minimum impedance stiffness (Nm)").asFloat64();
maxStiffness = options.check("maxStiffness", yarp::os::Value(0.0), "maximum impedance stiffness (Nm)").asFloat64();
minDamping = options.check("minDamping", yarp::os::Value(0.0), "minimum impedance damping (Nm*seconds)").asFloat64();
maxDamping = options.check("maxDamping", yarp::os::Value(0.0), "maximum impedance damping (Nm*seconds)").asFloat64();

if (stiffness < minStiffness || stiffness > maxStiffness)
{
Expand All @@ -63,16 +55,16 @@ bool TechnosoftIposExternal::open(yarp::os::Searchable & config)
return false;
}

auto kp = iposGroup.check("kp", yarp::os::Value(0.0), "position PID Kp").asFloat64();
auto ki = iposGroup.check("ki", yarp::os::Value(0.0), "position PID Ki").asFloat64();
auto kd = iposGroup.check("kd", yarp::os::Value(0.0), "position PID Kd").asFloat64();
auto maxInt = iposGroup.check("maxInt", yarp::os::Value(0.0), "position PID saturation threshold").asFloat64();
auto maxOutput = iposGroup.check("maxOutput", yarp::os::Value(0.0), "position PID maximum output").asFloat64();
auto offset = iposGroup.check("offset", yarp::os::Value(0.0), "position PID offset").asFloat64();
auto scale = iposGroup.check("scale", yarp::os::Value(1.0), "position PID scale").asFloat64();
auto stictionUp = iposGroup.check("stictionUp", yarp::os::Value(0.0), "position PID stiction up").asFloat64();
auto stictionDown = iposGroup.check("stictionDown", yarp::os::Value(0.0), "position PID stiction down").asFloat64();
auto kff = iposGroup.check("kff", yarp::os::Value(0.0), "position PID feed-forward").asFloat64();
auto kp = options.check("kp", yarp::os::Value(0.0), "position PID Kp").asFloat64();
auto ki = options.check("ki", yarp::os::Value(0.0), "position PID Ki").asFloat64();
auto kd = options.check("kd", yarp::os::Value(0.0), "position PID Kd").asFloat64();
auto maxInt = options.check("maxInt", yarp::os::Value(0.0), "position PID saturation threshold").asFloat64();
auto maxOutput = options.check("maxOutput", yarp::os::Value(0.0), "position PID maximum output").asFloat64();
auto offset = options.check("offset", yarp::os::Value(0.0), "position PID offset").asFloat64();
auto scale = options.check("scale", yarp::os::Value(1.0), "position PID scale").asFloat64();
auto stictionUp = options.check("stictionUp", yarp::os::Value(0.0), "position PID stiction up").asFloat64();
auto stictionDown = options.check("stictionDown", yarp::os::Value(0.0), "position PID stiction down").asFloat64();
auto kff = options.check("kff", yarp::os::Value(0.0), "position PID feed-forward").asFloat64();

positionPid.setKp(kp);
positionPid.setKi(ki);
Expand All @@ -91,7 +83,7 @@ bool TechnosoftIposExternal::open(yarp::os::Searchable & config)
impedancePid.setScale(1.0);
impedancePid.setStictionValues(stictionUp, stictionDown); // re-use

errorLimit = iposGroup.check("errorLimit", yarp::os::Value(0.0), "position PID error limit (degrees)").asFloat64();
errorLimit = options.check("errorLimit", yarp::os::Value(0.0), "position PID error limit (degrees)").asFloat64();

if (errorLimit <= 0.0)
{
Expand Down

0 comments on commit 428a2fc

Please sign in to comment.