From 977018fd9088437505e51de42ac84807c50d4762 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 2 Dec 2024 11:48:06 +0100 Subject: [PATCH 01/11] ControlBoardDriver: add iJointCoupling interface It is still draft, a lot of things are missing --- plugins/controlboard/include/ControlBoard.hh | 3 +- .../controlboard/include/ControlBoardData.hh | 6 +- plugins/controlboard/src/ControlBoard.cpp | 121 +++++++---- .../controlboard/src/ControlBoardDriver.cpp | 190 +++++++++--------- 4 files changed, 187 insertions(+), 133 deletions(-) diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index 2f3ea1ca..6e72e4e5 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -60,6 +60,7 @@ private: std::string m_deviceId; gz::sim::Entity m_modelEntity; yarp::dev::PolyDriver m_controlBoardDriver; + yarp::dev::PolyDriver m_coupling_driver; ControlBoardData m_controlBoardData; yarp::os::Network m_yarpNetwork; yarp::os::Property m_pluginParameters; @@ -90,7 +91,7 @@ private: bool setYarpPIDsParam(const std::vector& pidParams, const std::string& paramName, std::vector& yarpPIDs, - size_t numberOfJoints); + size_t numberOfPhysicalJoints); void setJointPositionPIDs(AngleUnitEnum cUnits, const std::vector& yarpPIDs); double convertUserGainToGazeboGain(JointProperties& joint, double value); double convertGazeboGainToUserGain(JointProperties& joint, double value); diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh index ef36a48f..af99deaa 100644 --- a/plugins/controlboard/include/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -66,9 +67,10 @@ class ControlBoardData { public: std::mutex mutex; - std::vector joints; + std::vector physicalJoints; + std::vector actuatedAxes; yarp::os::Stamp simTime; - + yarp::dev::IJointCoupling* ijointcoupling{nullptr}; // TODO (xela95): read this value from configuration file std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1); }; diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 79a8e39a..3d7734d8 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -132,6 +132,25 @@ void ControlBoard::Configure(const Entity& _entity, return; } + auto& coupling_group_bottle = m_pluginParameters.findGroup("COUPLING"); + if (!coupling_group_bottle.isNull()) { + m_pluginParameters.unput("device"); + auto coupling_device_str = coupling_group_bottle.find("device").asString(); + m_pluginParameters.put("device", coupling_device_str); + if(!m_coupling_driver.open(m_pluginParameters)) + { + yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in opening yarp " + "coupling driver"; + return; + } + if(!m_coupling_driver.view(m_controlBoardData.ijointcoupling)) { + yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in getting " + "IJointCoupling interface"; + return; + } + + } + if (!DeviceRegistry::getHandler() ->setDevice(_entity, _ecm, yarpDeviceName, &m_controlBoardDriver, m_deviceId)) { @@ -214,14 +233,14 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm) { std::lock_guard lock(m_controlBoardData.mutex); - m_controlBoardData.joints.resize(jointsFromConfigNum); + m_controlBoardData.physicalJoints.resize(jointsFromConfigNum); auto model = Model(m_modelEntity); auto jointEntititesCount = model.JointCount(_ecm); yInfo() << "Found " + std::to_string(jointEntititesCount) + " joints from the model description."; - m_controlBoardData.joints.resize(jointsFromConfigNum); + m_controlBoardData.physicalJoints.resize(jointsFromConfigNum); for (size_t i = 0; i < jointsFromConfigNum; i++) { std::string jointFromConfigName = jointNames.at(i); @@ -241,7 +260,7 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm) gzJoint.EnableTransmittedWrenchCheck(_ecm, true); // Initialize JointProperties object - m_controlBoardData.joints[i].name = jointFromConfigName; + m_controlBoardData.physicalJoints[i].name = jointFromConfigName; yInfo() << "Joint " << jointFromConfigName << " added to the control board data."; } @@ -282,7 +301,7 @@ bool ControlBoard::readJointsMeasurements(const gz::sim::EntityComponentManager& auto model = Model(m_modelEntity); Joint gzJoint; - for (auto& joint : m_controlBoardData.joints) + for (auto& joint : m_controlBoardData.physicalJoints) { try { @@ -348,7 +367,7 @@ void ControlBoard::checkForJointsHwFault() { std::lock_guard lock(m_controlBoardData.mutex); - for (auto& joint : m_controlBoardData.joints) + for (auto& joint : m_controlBoardData.physicalJoints) { if (joint.controlMode != VOCAB_CM_HW_FAULT && std::abs(joint.torque) > joint.maxTorqueAbs) { @@ -365,7 +384,7 @@ bool ControlBoard::updateTrajectories(const UpdateInfo& _info, EntityComponentMa // TODO: execute the following at control update time - for (auto& joint : m_controlBoardData.joints) + for (auto& joint : m_controlBoardData.physicalJoints) { switch (joint.controlMode) { @@ -394,7 +413,7 @@ bool ControlBoard::updateReferences(const UpdateInfo& _info, EntityComponentMana double forceReference{}; Joint gzJoint; - for (auto& joint : m_controlBoardData.joints) + for (auto& joint : m_controlBoardData.physicalJoints) { switch (joint.controlMode) { @@ -448,7 +467,7 @@ bool ControlBoard::initializePIDsForPositionControl() } Bottle pidGroup = m_pluginParameters.findGroup("POSITION_CONTROL"); - size_t numberOfJoints = m_controlBoardData.joints.size(); + size_t numberOfPhysicalJoints = m_controlBoardData.physicalJoints.size(); auto cUnits = AngleUnitEnum::DEG; // control units block @@ -479,8 +498,8 @@ bool ControlBoard::initializePIDsForPositionControl() { if (controlLawValue.asString() == "joint_pid_gazebo_v1") { - for (size_t i = 0; i < numberOfJoints; i++) - m_controlBoardData.joints[i].positionControlLaw = "joint_pid_gazebo_v1"; + for (size_t i = 0; i < numberOfPhysicalJoints; i++) + m_controlBoardData.physicalJoints[i].positionControlLaw = "joint_pid_gazebo_v1"; } else { yError() << "invalid controlLaw value"; @@ -492,7 +511,7 @@ bool ControlBoard::initializePIDsForPositionControl() return false; } - std::vector yarpPIDs(numberOfJoints); + std::vector yarpPIDs(numberOfPhysicalJoints); std::vector> parameters = {{"kp", "Pid kp parameter"}, @@ -508,11 +527,11 @@ bool ControlBoard::initializePIDsForPositionControl() for (const auto& param : parameters) { std::vector pidParams{}; - if (!tryGetGroup(pidGroup, pidParams, param.first, param.second, numberOfJoints)) + if (!tryGetGroup(pidGroup, pidParams, param.first, param.second, numberOfPhysicalJoints)) { return false; } - setYarpPIDsParam(pidParams, param.first, yarpPIDs, numberOfJoints); + setYarpPIDsParam(pidParams, param.first, yarpPIDs, numberOfPhysicalJoints); } setJointPositionPIDs(cUnits, yarpPIDs); @@ -544,7 +563,7 @@ bool ControlBoard::tryGetGroup(const Bottle& in, bool ControlBoard::setYarpPIDsParam(const std::vector& pidParams, const std::string& paramName, std::vector& yarpPIDs, - size_t numberOfJoints) + size_t numberOfPhysicalJoints) { std::unordered_map pidParamNameMap = {{"kp", 0}, @@ -557,7 +576,7 @@ bool ControlBoard::setYarpPIDsParam(const std::vector& pidParams, {"stictionUp", 7}, {"stictionDwn", 8}}; - for (size_t i = 0; i < numberOfJoints; i++) + for (size_t i = 0; i < numberOfPhysicalJoints; i++) { switch (pidParamNameMap[paramName]) { @@ -600,14 +619,14 @@ bool ControlBoard::setYarpPIDsParam(const std::vector& pidParams, void ControlBoard::setJointPositionPIDs(AngleUnitEnum cUnits, const std::vector& yarpPIDs) { - for (size_t i = 0; i < m_controlBoardData.joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); i++) { auto& jointPositionPID - = m_controlBoardData.joints[i].pidControllers[yarp::dev::VOCAB_PIDTYPE_POSITION]; + = m_controlBoardData.physicalJoints[i].pidControllers[yarp::dev::VOCAB_PIDTYPE_POSITION]; if (cUnits == AngleUnitEnum::DEG) { - auto& joint = m_controlBoardData.joints.at(i); + auto& joint = m_controlBoardData.physicalJoints.at(i); jointPositionPID.SetPGain(convertUserGainToGazeboGain(joint, yarpPIDs[i].kp) / pow(2, yarpPIDs[i].scale)); jointPositionPID.SetIGain(convertUserGainToGazeboGain(joint, yarpPIDs[i].ki) @@ -661,24 +680,56 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM } Bottle limitsGroup = m_pluginParameters.findGroup("LIMITS"); - size_t numberOfJoints = m_controlBoardData.joints.size(); + size_t numberOfPhysicalJoints = m_controlBoardData.physicalJoints.size(); std::vector limitMinGroup, limitMaxGroup; - if (!(tryGetGroup(limitsGroup, limitMinGroup, "jntPosMin", "", numberOfJoints) - && tryGetGroup(limitsGroup, limitMaxGroup, "jntPosMax", "", numberOfJoints))) + if (!(tryGetGroup(limitsGroup, limitMinGroup, "jntPosMin", "", numberOfPhysicalJoints) + && tryGetGroup(limitsGroup, limitMaxGroup, "jntPosMax", "", numberOfPhysicalJoints))) { yError() << "Error while reading joint position limits from plugin configuration"; return false; } - for (size_t i = 0; i < numberOfJoints; ++i) + for (size_t i = 0; i < numberOfPhysicalJoints; ++i) { // TODO: access gazebo joint position limits and use them to check if software limits // ([LIMITS] group) are consistent. In case they are not defined set them as sw limits. - auto& joint = m_controlBoardData.joints[i]; + auto& joint = m_controlBoardData.physicalJoints[i]; joint.positionLimitMin = limitMinGroup.at(i); joint.positionLimitMax = limitMaxGroup.at(i); } + // We have also to set the actuated axes limits + if (m_controlBoardData.ijointcoupling) + { + auto& couplingGroup = m_pluginParameters.findGroup("COUPLING"); + if(couplingGroup.isNull()) + { + yError() << "Failed to get coupling group"; + return false; + } + size_t nrOfActuatedAxes{0}; + bool ok = m_controlBoardData.ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); + if(!ok) + { + yError() << "Failed to get number of actuated axes"; + return false; + } + m_controlBoardData.actuatedAxes.resize(nrOfActuatedAxes); + std::vector actuatedAxisPosLimitsMin, actuatedAxisPosLimitsMax; + if (!(tryGetGroup(couplingGroup, actuatedAxisPosLimitsMin, "actuatedAxesPosMin", "", numberOfPhysicalJoints) + && tryGetGroup(couplingGroup, actuatedAxisPosLimitsMax, "actuatedAxesPosMax", "", numberOfPhysicalJoints))) + { + yError() << "Error while reading joint position limits from plugin configuration"; + return false; + } + for (size_t i = 0; i< m_controlBoardData.actuatedAxes.size(); ++i) + { + auto& actuatedAxis = m_controlBoardData.actuatedAxes[i]; + actuatedAxis.positionLimitMin = actuatedAxisPosLimitsMin.at(i); + actuatedAxis.positionLimitMax = actuatedAxisPosLimitsMax.at(i); + } + + } return true; } @@ -729,7 +780,7 @@ bool ControlBoard::initializeTrajectoryGenerators() } } - for (auto& joint : m_controlBoardData.joints) + for (auto& joint : m_controlBoardData.physicalJoints) { joint.trajectoryGenerator = yarp::dev::gzyarp::TrajectoryGeneratorFactory::create(trajectoryType); @@ -756,7 +807,7 @@ bool ControlBoard::initializeTrajectoryGeneratorReferences(Bottle& trajectoryGen refSpeedGroup, "refSpeed", "", - m_controlBoardData.joints.size())) + m_controlBoardData.physicalJoints.size())) { yWarning() << "Parameter refSpeed not found in TRAJECTORY_GENERATION group. Defaults " "will be applied"; @@ -767,7 +818,7 @@ bool ControlBoard::initializeTrajectoryGeneratorReferences(Bottle& trajectoryGen refAccelerationGroup, "refAcceleration", "", - m_controlBoardData.joints.size())) + m_controlBoardData.physicalJoints.size())) { yWarning() << "Parameter refAcceleration not found in TRAJECTORY_GENERATION group. " "Defaults will be applied"; @@ -784,9 +835,9 @@ bool ControlBoard::initializeTrajectoryGeneratorReferences(Bottle& trajectoryGen // Set trajectory generation reference speed and acceleration // TODO: manage different joint types - for (size_t i = 0; i < m_controlBoardData.joints.size(); ++i) + for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); ++i) { - auto& joint = m_controlBoardData.joints[i]; + auto& joint = m_controlBoardData.physicalJoints[i]; if (useDefaultSpeedRef) { joint.trajectoryGenerationRefSpeed = 10.0; // [deg/s] @@ -834,7 +885,7 @@ bool ControlBoard::parseInitialConfiguration(std::vector& initialConfigu while (ss >> tmp) { - if (counter >= m_controlBoardData.joints.size()) + if (counter >= m_controlBoardData.physicalJoints.size()) { yWarning() << "Too many elements in initial configuration, stopping at element " << (counter + 1); @@ -851,14 +902,14 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen { std::lock_guard lock(m_controlBoardData.mutex); - std::vector initialConfigurations(m_controlBoardData.joints.size()); + std::vector initialConfigurations(m_controlBoardData.physicalJoints.size()); if (parseInitialConfiguration(initialConfigurations)) { yInfo() << "Initial configuration found, initializing trajectory generator with it"; - for (size_t i = 0; i < m_controlBoardData.joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); i++) { - auto& joint = m_controlBoardData.joints.at(i); + auto& joint = m_controlBoardData.physicalJoints.at(i); auto gzPos = initialConfigurations[i]; auto userPos = convertGazeboToUser(joint, gzPos); // Reset joint properties @@ -884,9 +935,9 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen yWarning() << "No initial configuration found, initializing trajectory generator with " "current values"; - for (size_t i = 0; i < m_controlBoardData.joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); i++) { - auto& joint = m_controlBoardData.joints.at(i); + auto& joint = m_controlBoardData.physicalJoints.at(i); auto gzJoint = Joint(Model(m_modelEntity).JointByName(ecm, joint.name)); if (gzJoint.Position(ecm).has_value() && gzJoint.Position(ecm).value().size() > 0) { @@ -909,7 +960,7 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen } // Reset control mode - for (auto& joint : m_controlBoardData.joints) + for (auto& joint : m_controlBoardData.physicalJoints) { joint.controlMode = VOCAB_CM_POSITION; } diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 06e2c93c..93cc4021 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -50,7 +50,7 @@ bool ControlBoardDriver::getInteractionMode(int axis, yarp::dev::InteractionMode { std::lock_guard lock(m_controlBoardData->mutex); - *mode = m_controlBoardData->joints.at(axis).interactionMode; + *mode = m_controlBoardData->physicalJoints.at(axis).interactionMode; return true; } @@ -89,7 +89,7 @@ bool ControlBoardDriver::getInteractionModes(yarp::dev::InteractionModeEnum* mod return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getInteractionMode(i, &modes[i])) { @@ -106,7 +106,7 @@ bool ControlBoardDriver::setInteractionMode(int axis, yarp::dev::InteractionMode try { - m_controlBoardData->joints.at(axis).interactionMode = mode; + m_controlBoardData->physicalJoints.at(axis).interactionMode = mode; } catch (const std::exception& e) { yError() << "Error while setting interaction mode for axis " + std::to_string(axis) + ": \n" @@ -151,7 +151,7 @@ bool ControlBoardDriver::setInteractionModes(yarp::dev::InteractionModeEnum* mod return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::setInteractionMode(i, modes[i])) { @@ -174,14 +174,14 @@ bool ControlBoardDriver::getControlMode(int j, int* mode) return false; } - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting control mode: joint index " + std::to_string(j) + " out of range"; return false; } - *mode = m_controlBoardData->joints.at(j).controlMode; + *mode = m_controlBoardData->physicalJoints.at(j).controlMode; return true; } @@ -194,7 +194,7 @@ bool ControlBoardDriver::getControlModes(int* modes) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getControlMode(i, &modes[i])) { @@ -235,7 +235,7 @@ bool ControlBoardDriver::setControlMode(const int j, const int mode) int desired_mode = mode; - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting control mode: joint index out of range"; return false; @@ -255,7 +255,7 @@ bool ControlBoardDriver::setControlMode(const int j, const int mode) } // If joint is in hw fault, only a force idle command can recover it - if (m_controlBoardData->joints.at(j).controlMode == VOCAB_CM_HW_FAULT + if (m_controlBoardData->physicalJoints.at(j).controlMode == VOCAB_CM_HW_FAULT && mode != VOCAB_CM_FORCE_IDLE) { return true; @@ -267,7 +267,7 @@ bool ControlBoardDriver::setControlMode(const int j, const int mode) desired_mode = VOCAB_CM_IDLE; } - m_controlBoardData->joints.at(j).controlMode = desired_mode; + m_controlBoardData->physicalJoints.at(j).controlMode = desired_mode; return true; } @@ -304,7 +304,7 @@ bool ControlBoardDriver::setControlModes(int* modes) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::setControlMode(i, modes[i])) { @@ -321,7 +321,7 @@ bool ControlBoardDriver::getAxisName(int axis, std::string& name) { // TODO integrate with IJointCoupled interface - name = m_controlBoardData->joints.at(axis).name; + name = m_controlBoardData->physicalJoints.at(axis).name; return true; } @@ -341,14 +341,14 @@ bool ControlBoardDriver::setLimits(int axis, double min, double max) { std::lock_guard lock(m_controlBoardData->mutex); - if (axis < 0 || axis >= m_controlBoardData->joints.size()) + if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting limits: axis index out of range"; return false; } - m_controlBoardData->joints.at(axis).positionLimitMin = min; - m_controlBoardData->joints.at(axis).positionLimitMax = max; + m_controlBoardData->physicalJoints.at(axis).positionLimitMin = min; + m_controlBoardData->physicalJoints.at(axis).positionLimitMax = max; return true; } @@ -367,14 +367,14 @@ bool ControlBoardDriver::getLimits(int axis, double* min, double* max) yError() << "Error while getting limits: max is null"; return false; } - if (axis < 0 || axis >= m_controlBoardData->joints.size()) + if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting limits: axis index out of range"; return false; } - *min = m_controlBoardData->joints.at(axis).positionLimitMin; - *max = m_controlBoardData->joints.at(axis).positionLimitMax; + *min = m_controlBoardData->physicalJoints.at(axis).positionLimitMin; + *max = m_controlBoardData->physicalJoints.at(axis).positionLimitMax; return true; } @@ -383,14 +383,14 @@ bool ControlBoardDriver::setVelLimits(int axis, double min, double max) { std::lock_guard lock(m_controlBoardData->mutex); - if (axis < 0 || axis >= m_controlBoardData->joints.size()) + if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting velocity limits: axis index out of range"; return false; } - m_controlBoardData->joints.at(axis).velocityLimitMin = min; - m_controlBoardData->joints.at(axis).velocityLimitMax = max; + m_controlBoardData->physicalJoints.at(axis).velocityLimitMin = min; + m_controlBoardData->physicalJoints.at(axis).velocityLimitMax = max; return true; } @@ -409,14 +409,14 @@ bool ControlBoardDriver::getVelLimits(int axis, double* min, double* max) yError() << "Error while getting velocity limits: max is null"; return false; } - if (axis < 0 || axis >= m_controlBoardData->joints.size()) + if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting velocity limits: axis index out of range"; return false; } - *min = m_controlBoardData->joints.at(axis).velocityLimitMin; - *max = m_controlBoardData->joints.at(axis).velocityLimitMax; + *min = m_controlBoardData->physicalJoints.at(axis).velocityLimitMin; + *max = m_controlBoardData->physicalJoints.at(axis).velocityLimitMax; return true; } @@ -446,7 +446,7 @@ bool ControlBoardDriver::getAxes(int* ax) // TODO integrate with IJointCoupled interface std::lock_guard lock(m_controlBoardData->mutex); - *ax = m_controlBoardData->joints.size(); + *ax = m_controlBoardData->physicalJoints.size(); return true; } @@ -460,13 +460,13 @@ bool ControlBoardDriver::getRefTorque(int j, double* t) yError() << "Error while getting reference torque: t is null"; return false; } - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting reference torque: joint index out of range"; return false; } - *t = m_controlBoardData->joints.at(j).refTorque; + *t = m_controlBoardData->physicalJoints.at(j).refTorque; return true; } @@ -479,7 +479,7 @@ bool ControlBoardDriver::getRefTorques(double* t) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getRefTorque(i, &t[i])) { @@ -494,7 +494,7 @@ bool ControlBoardDriver::setRefTorque(int j, double t) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting reference torque: joint index " + std::to_string(j) + " out of range"; @@ -505,7 +505,7 @@ bool ControlBoardDriver::setRefTorque(int j, double t) return false; } - m_controlBoardData->joints.at(j).refTorque = t; + m_controlBoardData->physicalJoints.at(j).refTorque = t; return true; } @@ -518,7 +518,7 @@ bool ControlBoardDriver::setRefTorques(const double* t) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::setRefTorque(i, t[i])) { @@ -587,14 +587,14 @@ bool ControlBoardDriver::getTorque(int j, double* t) yError() << "Error while getting torque: t is null"; return false; } - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting torque: joint index " + std::to_string(j) + " out of range"; return false; } - *t = m_controlBoardData->joints.at(j).torque; + *t = m_controlBoardData->physicalJoints.at(j).torque; return true; } @@ -607,7 +607,7 @@ bool ControlBoardDriver::getTorques(double* t) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getTorque(i, &t[i])) { @@ -632,15 +632,15 @@ bool ControlBoardDriver::getTorqueRange(int j, double* min, double* max) yError() << "Error while getting torque range: max is null"; return false; } - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting torque range: joint index " + std::to_string(j) + " out of range"; return false; } - *min = -m_controlBoardData->joints.at(j).maxTorqueAbs; - *max = m_controlBoardData->joints.at(j).maxTorqueAbs; + *min = -m_controlBoardData->physicalJoints.at(j).maxTorqueAbs; + *max = m_controlBoardData->physicalJoints.at(j).maxTorqueAbs; return true; } @@ -658,7 +658,7 @@ bool ControlBoardDriver::getTorqueRanges(double* min, double* max) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getTorqueRange(i, &min[i], &max[i])) { @@ -675,21 +675,21 @@ bool ControlBoardDriver::setPosition(int j, double ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting position: joint index " + std::to_string(j) + " out of range"; return false; } - if (m_controlBoardData->joints.at(j).controlMode != VOCAB_CM_POSITION_DIRECT) + if (m_controlBoardData->physicalJoints.at(j).controlMode != VOCAB_CM_POSITION_DIRECT) { yError() << "Error while setting position: joint " + std::to_string(j) + " is not in position direct mode"; return false; } - m_controlBoardData->joints.at(j).refPosition = ref; + m_controlBoardData->physicalJoints.at(j).refPosition = ref; return true; } @@ -726,7 +726,7 @@ bool ControlBoardDriver::setPositions(const double* refs) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::setPosition(i, refs[i])) { @@ -739,14 +739,14 @@ bool ControlBoardDriver::setPositions(const double* refs) bool ControlBoardDriver::getRefPosition(const int joint, double* ref) { - if (joint < 0 || joint >= m_controlBoardData->joints.size()) + if (joint < 0 || joint >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting reference position: joint index " + std::to_string(joint) + " out of range"; return false; } - *ref = m_controlBoardData->joints.at(joint).refPosition; + *ref = m_controlBoardData->physicalJoints.at(joint).refPosition; return true; } @@ -759,7 +759,7 @@ bool ControlBoardDriver::getRefPositions(double* refs) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getRefPosition(i, &refs[i])) { @@ -800,21 +800,21 @@ bool ControlBoardDriver::positionMove(int j, double ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting reference position for trajectory generation: joint index " + std::to_string(j) + " out of range"; return false; } - auto& joint = m_controlBoardData->joints.at(j); + auto& joint = m_controlBoardData->physicalJoints.at(j); joint.trajectoryGenerationRefPosition = ref; // TODO: use getLimits when recursive mutexes are implemented - auto limitMin = m_controlBoardData->joints.at(j).positionLimitMin; - auto limitMax = m_controlBoardData->joints.at(j).positionLimitMax; + auto limitMin = m_controlBoardData->physicalJoints.at(j).positionLimitMin; + auto limitMax = m_controlBoardData->physicalJoints.at(j).positionLimitMax; joint.trajectoryGenerator->setLimits(limitMin, limitMax); joint.trajectoryGenerator->initTrajectory(joint.position, @@ -835,7 +835,7 @@ bool ControlBoardDriver::positionMove(const double* refs) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::positionMove(i, refs[i])) { @@ -849,7 +849,7 @@ bool ControlBoardDriver::positionMove(const double* refs) bool ControlBoardDriver::relativeMove(int j, double delta) { // Check on valid joint number done in setPosition - return setPosition(j, m_controlBoardData->joints.at(j).position + delta); + return setPosition(j, m_controlBoardData->physicalJoints.at(j).position + delta); } bool ControlBoardDriver::relativeMove(const double* deltas) @@ -860,7 +860,7 @@ bool ControlBoardDriver::relativeMove(const double* deltas) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::relativeMove(i, deltas[i])) { @@ -875,13 +875,13 @@ bool ControlBoardDriver::checkMotionDone(int j, bool* flag) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while checking motion done: joint index out of range"; return false; } - *flag = m_controlBoardData->joints.at(j).isMotionDone; + *flag = m_controlBoardData->physicalJoints.at(j).isMotionDone; return true; } @@ -894,7 +894,7 @@ bool ControlBoardDriver::checkMotionDone(bool* flag) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::checkMotionDone(i, &flag[i])) { @@ -909,13 +909,13 @@ bool ControlBoardDriver::setRefSpeed(int j, double sp) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting reference speed: joint index out of range"; return false; } - m_controlBoardData->joints.at(j).trajectoryGenerationRefSpeed = sp; + m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefSpeed = sp; return true; } @@ -928,7 +928,7 @@ bool ControlBoardDriver::setRefSpeeds(const double* spds) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::setRefSpeed(i, spds[i])) { @@ -943,13 +943,13 @@ bool ControlBoardDriver::setRefAcceleration(int j, double acc) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting reference acceleration: joint index out of range"; return false; } - m_controlBoardData->joints.at(j).trajectoryGenerationRefAcceleration = acc; + m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefAcceleration = acc; return true; } @@ -962,7 +962,7 @@ bool ControlBoardDriver::setRefAccelerations(const double* accs) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::setRefAcceleration(i, accs[i])) { @@ -977,13 +977,13 @@ bool ControlBoardDriver::getRefSpeed(int j, double* ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting reference speed: joint index out of range"; return false; } - *ref = m_controlBoardData->joints.at(j).trajectoryGenerationRefSpeed; + *ref = m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefSpeed; return true; } @@ -996,7 +996,7 @@ bool ControlBoardDriver::getRefSpeeds(double* spds) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getRefSpeed(i, &spds[i])) { @@ -1011,13 +1011,13 @@ bool ControlBoardDriver::getRefAcceleration(int j, double* acc) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting reference acceleration: joint index out of range"; return false; } - *acc = m_controlBoardData->joints.at(j).trajectoryGenerationRefAcceleration; + *acc = m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefAcceleration; return true; } @@ -1030,7 +1030,7 @@ bool ControlBoardDriver::getRefAccelerations(double* accs) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getRefAcceleration(i, &accs[i])) { @@ -1045,24 +1045,24 @@ bool ControlBoardDriver::stop(int j) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while stopping: joint index out of range"; return false; } - switch (m_controlBoardData->joints.at(j).controlMode) + switch (m_controlBoardData->physicalJoints.at(j).controlMode) { case VOCAB_CM_POSITION: - m_controlBoardData->joints.at(j).trajectoryGenerationRefPosition - = m_controlBoardData->joints.at(j).position; - m_controlBoardData->joints.at(j).trajectoryGenerator->abortTrajectory( - m_controlBoardData->joints.at(j).position); + m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefPosition + = m_controlBoardData->physicalJoints.at(j).position; + m_controlBoardData->physicalJoints.at(j).trajectoryGenerator->abortTrajectory( + m_controlBoardData->physicalJoints.at(j).position); break; case VOCAB_CM_POSITION_DIRECT: - m_controlBoardData->joints.at(j).trajectoryGenerationRefPosition - = m_controlBoardData->joints.at(j).position; - m_controlBoardData->joints.at(j).refPosition = m_controlBoardData->joints.at(j).position; + m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefPosition + = m_controlBoardData->physicalJoints.at(j).position; + m_controlBoardData->physicalJoints.at(j).refPosition = m_controlBoardData->physicalJoints.at(j).position; break; case VOCAB_CM_VELOCITY: // TODO velocity control @@ -1079,7 +1079,7 @@ bool ControlBoardDriver::stop(int j) bool ControlBoardDriver::stop() { - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::stop(i)) { @@ -1282,14 +1282,14 @@ bool ControlBoardDriver::getTargetPosition(const int joint, double* ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (joint < 0 || joint >= m_controlBoardData->joints.size()) + if (joint < 0 || joint >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting target position: joint index " + std::to_string(joint) + " out of range"; return false; } - *ref = m_controlBoardData->joints.at(joint).trajectoryGenerationRefPosition; + *ref = m_controlBoardData->physicalJoints.at(joint).trajectoryGenerationRefPosition; return true; } @@ -1302,7 +1302,7 @@ bool ControlBoardDriver::getTargetPositions(double* refs) return false; } - for (size_t i = 0; i < m_controlBoardData->joints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getTargetPosition(i, &refs[i])) { @@ -1541,20 +1541,20 @@ bool ControlBoardDriver::resetEncoder(int j) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while resetting encoder: joint index out of range"; return false; } - m_controlBoardData->joints.at(j).zeroPosition = m_controlBoardData->joints.at(j).position; + m_controlBoardData->physicalJoints.at(j).zeroPosition = m_controlBoardData->physicalJoints.at(j).position; return true; } bool ControlBoardDriver::resetEncoders() { - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::resetEncoder(i)) { @@ -1569,14 +1569,14 @@ bool ControlBoardDriver::setEncoder(int j, double val) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while setting encoder: joint index " + std::to_string(j) + " out of range"; return false; } - m_controlBoardData->joints.at(j).zeroPosition = m_controlBoardData->joints.at(j).position - val; + m_controlBoardData->physicalJoints.at(j).zeroPosition = m_controlBoardData->physicalJoints.at(j).position - val; return true; } @@ -1589,7 +1589,7 @@ bool ControlBoardDriver::setEncoders(const double* vals) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::setEncoder(i, vals[i])) { @@ -1609,14 +1609,14 @@ bool ControlBoardDriver::getEncoder(int j, double* v) yError() << "Error while getting encoder: v is null"; return false; } - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting encoder: joint index " + std::to_string(j) + " out of range"; return false; } - *v = m_controlBoardData->joints.at(j).position - m_controlBoardData->joints.at(j).zeroPosition; + *v = m_controlBoardData->physicalJoints.at(j).position - m_controlBoardData->physicalJoints.at(j).zeroPosition; return true; } @@ -1629,7 +1629,7 @@ bool ControlBoardDriver::getEncoders(double* encs) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getEncoder(i, &encs[i])) { @@ -1650,14 +1650,14 @@ bool ControlBoardDriver::getEncoderSpeed(int j, double* sp) return false; } - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting encoder speed: joint index " + std::to_string(j) + " out of range"; return false; } - *sp = m_controlBoardData->joints.at(j).velocity; + *sp = m_controlBoardData->physicalJoints.at(j).velocity; return true; } @@ -1670,7 +1670,7 @@ bool ControlBoardDriver::getEncoderSpeeds(double* spds) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getEncoderSpeed(i, &spds[i])) { @@ -1709,7 +1709,7 @@ bool ControlBoardDriver::getEncoderTimed(int j, double* encs, double* time) yError() << "Error while getting encoder: time is null"; return false; } - if (j < 0 || j >= m_controlBoardData->joints.size()) + if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) { yError() << "Error while getting encoder: joint index " + std::to_string(j) + " out of range"; @@ -1717,7 +1717,7 @@ bool ControlBoardDriver::getEncoderTimed(int j, double* encs, double* time) } *encs - = m_controlBoardData->joints.at(j).position - m_controlBoardData->joints.at(j).zeroPosition; + = m_controlBoardData->physicalJoints.at(j).position - m_controlBoardData->physicalJoints.at(j).zeroPosition; return true; } @@ -1735,7 +1735,7 @@ bool ControlBoardDriver::getEncodersTimed(double* encs, double* time) return false; } - for (int i = 0; i < m_controlBoardData->joints.size(); i++) + for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) { if (!ControlBoardDriver::getEncoderTimed(i, &encs[i], &time[i])) { From f84a803e7997c59f6be57d1c1ca048d3277f3c66 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 2 Dec 2024 16:22:44 +0100 Subject: [PATCH 02/11] Controlboard: added coupling in resetPositionsAndTrajectoryGenerators --- plugins/controlboard/src/ControlBoard.cpp | 37 ++++++++++++++++++++--- 1 file changed, 32 insertions(+), 5 deletions(-) diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 3d7734d8..8b30375a 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -934,7 +934,7 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen { yWarning() << "No initial configuration found, initializing trajectory generator with " "current values"; - + yarp::sig::Vector initialPositionAct, initialPositionPhys, limitsMin, limitsMax; for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); i++) { auto& joint = m_controlBoardData.physicalJoints.at(i); @@ -947,16 +947,43 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen joint.trajectoryGenerationRefPosition = userPos; joint.refPosition = userPos; joint.position = userPos; + initialPositionPhys.push_back(userPos); } - auto limitMin = joint.positionLimitMin; - auto limitMax = joint.positionLimitMax; + } + + if (m_controlBoardData.ijointcoupling) { + initialPositionAct.resize(m_controlBoardData.actuatedAxes.size()); + auto ok = m_controlBoardData.ijointcoupling->convertFromPhysicalJointsToActuatedAxesPos(initialPositionPhys, initialPositionAct); + if(!ok) + { + yError() << "Failed to convert from physical joints to actuated axes"; + return; + } + for(size_t i=0; isetLimits(limitMin, limitMax); - joint.trajectoryGenerator->initTrajectory(joint.position, - joint.position, + joint.trajectoryGenerator->initTrajectory(initialPositionAct[i], + initialPositionAct[i], joint.trajectoryGenerationRefSpeed, joint.trajectoryGenerationRefAcceleration, m_controlBoardData.controlUpdatePeriod); } + } // Reset control mode From f0e680e5cb8141a96b808c11917c5d1efcbe8a0d Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 5 Dec 2024 10:33:49 +0100 Subject: [PATCH 03/11] WIP COUPLING --- plugins/controlboard/src/ControlBoard.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 8b30375a..466759bf 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -383,11 +383,13 @@ bool ControlBoard::updateTrajectories(const UpdateInfo& _info, EntityComponentMa std::lock_guard lock(m_controlBoardData.mutex); // TODO: execute the following at control update time - + for (auto& joint : m_controlBoardData.physicalJoints) { + switch (joint.controlMode) { + // PUT THE COUPLING HERE!! case VOCAB_CM_POSITION: joint.refPosition = joint.trajectoryGenerator->computeTrajectory(); joint.isMotionDone = joint.trajectoryGenerator->isMotionDone(); From 95e9b66c565afca91d399a7a25212f162ade8c82 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 5 Dec 2024 17:12:24 +0100 Subject: [PATCH 04/11] Coupling: seems a reasonable implementation now --- plugins/controlboard/include/ControlBoard.hh | 3 +- .../controlboard/include/ControlBoardData.hh | 1 + plugins/controlboard/src/ControlBoard.cpp | 35 ++++++++++++++----- 3 files changed, 28 insertions(+), 11 deletions(-) diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index 6e72e4e5..65b88c1b 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -76,8 +76,7 @@ private: void updateSimTime(const gz::sim::UpdateInfo& _info); bool readJointsMeasurements(const gz::sim::EntityComponentManager& _ecm); void checkForJointsHwFault(); - bool - updateTrajectories(const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm); + bool updateTrajectories(const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm); bool updateReferences(const gz::sim::UpdateInfo& _info, gz::sim::EntityComponentManager& _ecm); double getJointTorqueFromTransmittedWrench(const gz::sim::Joint& gzJoint, const gz::msgs::Wrench& wrench, diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh index af99deaa..07cb2465 100644 --- a/plugins/controlboard/include/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -71,6 +71,7 @@ public: std::vector actuatedAxes; yarp::os::Stamp simTime; yarp::dev::IJointCoupling* ijointcoupling{nullptr}; + std::vector* actuated_joints_handle; // TODO (xela95): read this value from configuration file std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1); }; diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 466759bf..d06bcc74 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -384,7 +384,7 @@ bool ControlBoard::updateTrajectories(const UpdateInfo& _info, EntityComponentMa // TODO: execute the following at control update time - for (auto& joint : m_controlBoardData.physicalJoints) + for (auto& joint : *m_controlBoardData.actuated_joints_handle) { switch (joint.controlMode) @@ -415,6 +415,22 @@ bool ControlBoard::updateReferences(const UpdateInfo& _info, EntityComponentMana double forceReference{}; Joint gzJoint; + + if(m_controlBoardData.ijointcoupling){ + yarp::sig::Vector actuatedAxesRefPositions, physicalJointsRefPositions; + for (auto& joint: m_controlBoardData.actuatedAxes) + { + actuatedAxesRefPositions.push_back(joint.refPosition); + } + physicalJointsRefPositions.resize(m_controlBoardData.physicalJoints.size()); + + m_controlBoardData.ijointcoupling->convertFromActuatedAxesToPhysicalJointsPos(actuatedAxesRefPositions, physicalJointsRefPositions); + for(auto i = 0; i < m_controlBoardData.physicalJoints.size(); i++) + { + m_controlBoardData.physicalJoints[i].refPosition = physicalJointsRefPositions[i]; + } + } + for (auto& joint : m_controlBoardData.physicalJoints) { switch (joint.controlMode) @@ -741,6 +757,8 @@ bool ControlBoard::initializeTrajectoryGenerators() // Read from configuration auto trajectoryGeneratorsGroup = m_pluginParameters.findGroup("TRAJECTORY_GENERATION"); bool missingConfiguration = false; + // In case of coupled systems, the trajectory generators has to be initialized only for the actuated axes + m_controlBoardData.actuated_joints_handle = m_controlBoardData.ijointcoupling ? &(m_controlBoardData.actuatedAxes) : &(m_controlBoardData.physicalJoints); if (trajectoryGeneratorsGroup.isNull()) { yWarning() << "Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults to " @@ -782,7 +800,7 @@ bool ControlBoard::initializeTrajectoryGenerators() } } - for (auto& joint : m_controlBoardData.physicalJoints) + for (auto& joint : *m_controlBoardData.actuated_joints_handle) { joint.trajectoryGenerator = yarp::dev::gzyarp::TrajectoryGeneratorFactory::create(trajectoryType); @@ -836,10 +854,10 @@ bool ControlBoard::initializeTrajectoryGeneratorReferences(Bottle& trajectoryGen } // Set trajectory generation reference speed and acceleration - // TODO: manage different joint types - for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); ++i) + // TODO: manage different joint types HERE + for (size_t i = 0; i < m_controlBoardData.actuated_joints_handle->size(); ++i) { - auto& joint = m_controlBoardData.physicalJoints[i]; + auto& joint = m_controlBoardData.actuated_joints_handle->at(i); if (useDefaultSpeedRef) { joint.trajectoryGenerationRefSpeed = 10.0; // [deg/s] @@ -973,12 +991,11 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen else { // With no coupling, actuated axes are the same as physical joints initialPositionAct = initialPositionPhys; } - - for (size_t i=0; isize(); i++) { + auto& joint = m_controlBoardData.actuated_joints_handle->at(i); auto limitMin = m_controlBoardData.ijointcoupling ? m_controlBoardData.actuatedAxes[i].positionLimitMin : m_controlBoardData.physicalJoints[i].positionLimitMin; auto limitMax = m_controlBoardData.ijointcoupling ? m_controlBoardData.actuatedAxes[i].positionLimitMax : m_controlBoardData.physicalJoints[i].positionLimitMax; - joint.trajectoryGenerator->setLimits(limitMin, limitMax); + joint.trajectoryGenerator->setLimits(joint.positionLimitMin, joint.positionLimitMax); joint.trajectoryGenerator->initTrajectory(initialPositionAct[i], initialPositionAct[i], joint.trajectoryGenerationRefSpeed, From 6f96bcd4c40d8b2070aaba8854f389a3f005ab4c Mon Sep 17 00:00:00 2001 From: Nicogene Date: Wed, 11 Dec 2024 17:09:39 +0100 Subject: [PATCH 05/11] WIP started refactoring, it does not compile yet --- .../controlboard/include/ControlBoardData.hh | 18 +- plugins/controlboard/src/ControlBoard.cpp | 8 +- .../controlboard/src/ControlBoardDriver.cpp | 170 ++++++++++++------ 3 files changed, 132 insertions(+), 64 deletions(-) diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh index 07cb2465..599ac236 100644 --- a/plugins/controlboard/include/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -34,8 +34,7 @@ struct PidControlTypeEnumHashFunction } }; -struct JointProperties -{ +struct CommonJointProperties { std::string name; yarp::dev::InteractionModeEnum interactionMode{yarp::dev::InteractionModeEnum::VOCAB_IM_STIFF}; yarp::conf::vocab32_t controlMode{VOCAB_CM_IDLE}; @@ -53,9 +52,19 @@ struct JointProperties double velocity{0.0}; // Joint velocity [deg/s] double velocityLimitMin{std::numeric_limits::min()}; double velocityLimitMax{std::numeric_limits::max()}; +} + +struct PhysicalJointProperties +{ + CommonJointProperties commonJointProperties; std::unordered_map pidControllers; std::string positionControlLaw; // TODO: verify usefulness of this field +}; + +struct ActuatedAxesProperties +{ + CommonJointProperties commonJointProperties; std::unique_ptr trajectoryGenerator; double trajectoryGenerationRefPosition{0.0}; double trajectoryGenerationRefSpeed{0.0}; @@ -67,11 +76,10 @@ class ControlBoardData { public: std::mutex mutex; - std::vector physicalJoints; - std::vector actuatedAxes; + std::vector physicalJoints; + std::vector actuatedAxes; yarp::os::Stamp simTime; yarp::dev::IJointCoupling* ijointcoupling{nullptr}; - std::vector* actuated_joints_handle; // TODO (xela95): read this value from configuration file std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1); }; diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index d06bcc74..bb9f8211 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -366,8 +366,7 @@ ControlBoard::getJointTorqueFromTransmittedWrench(const Joint& gzJoint, void ControlBoard::checkForJointsHwFault() { std::lock_guard lock(m_controlBoardData.mutex); - - for (auto& joint : m_controlBoardData.physicalJoints) + for (auto& joint : *m_controlBoardData.actuated_joints_handle) { if (joint.controlMode != VOCAB_CM_HW_FAULT && std::abs(joint.torque) > joint.maxTorqueAbs) { @@ -1010,6 +1009,11 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen { joint.controlMode = VOCAB_CM_POSITION; } + + for (auto& joint : m_controlBoardData.actuatedAxes) + { + joint.controlMode = VOCAB_CM_POSITION; + } } } // namespace gzyarp diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 93cc4021..32789b9e 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -50,7 +50,7 @@ bool ControlBoardDriver::getInteractionMode(int axis, yarp::dev::InteractionMode { std::lock_guard lock(m_controlBoardData->mutex); - *mode = m_controlBoardData->physicalJoints.at(axis).interactionMode; + *mode = m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.interactionMode; return true; } @@ -89,7 +89,7 @@ bool ControlBoardDriver::getInteractionModes(yarp::dev::InteractionModeEnum* mod return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getInteractionMode(i, &modes[i])) { @@ -103,10 +103,36 @@ bool ControlBoardDriver::getInteractionModes(yarp::dev::InteractionModeEnum* mod bool ControlBoardDriver::setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) { std::lock_guard lock(m_controlBoardData->mutex); - + yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; try { - m_controlBoardData->physicalJoints.at(axis).interactionMode = mode; + // If there is coupling let's check if we have to change the interaction mode for all the coupled joints + if (m_controlBoardData->ijointcoupling) + { + m_controlBoardData->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); + m_controlBoardData->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); + // If the joint is coupled, we have to change the interaction mode for all the coupled joints + if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) + { + for (auto& actuatedAxis : coupledActuatedAxes) + { + m_controlBoardData->actuatedAxes.at(actuatedAxis).commonJointProperties.interactionMode = mode; + } + for (auto& physicalJoint : coupledPhysicalJoints) + { + m_controlBoardData->physicalJoints.at(physicalJoint).commonJointProperties.interactionMode = mode; + } + } // if the joint is not coupled, we change the interaction mode only for the selected joint + else + { + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.interactionMode = mode; + m_controlBoardData->physicalJoints.at(axis).commonJointProperties.interactionMode = mode; + } + } // No coupling, we change the interaction mode only for the selected joint + else { + m_controlBoardData->physicalJoints.at(axis).commonJointProperties.interactionMode = mode; + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.interactionMode = mode; + } } catch (const std::exception& e) { yError() << "Error while setting interaction mode for axis " + std::to_string(axis) + ": \n" @@ -151,7 +177,7 @@ bool ControlBoardDriver::setInteractionModes(yarp::dev::InteractionModeEnum* mod return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::setInteractionMode(i, modes[i])) { @@ -174,14 +200,14 @@ bool ControlBoardDriver::getControlMode(int j, int* mode) return false; } - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting control mode: joint index " + std::to_string(j) + " out of range"; return false; } - *mode = m_controlBoardData->physicalJoints.at(j).controlMode; + *mode = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.controlMode; return true; } @@ -194,7 +220,7 @@ bool ControlBoardDriver::getControlModes(int* modes) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getControlMode(i, &modes[i])) { @@ -235,7 +261,7 @@ bool ControlBoardDriver::setControlMode(const int j, const int mode) int desired_mode = mode; - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting control mode: joint index out of range"; return false; @@ -255,7 +281,7 @@ bool ControlBoardDriver::setControlMode(const int j, const int mode) } // If joint is in hw fault, only a force idle command can recover it - if (m_controlBoardData->physicalJoints.at(j).controlMode == VOCAB_CM_HW_FAULT + if (m_controlBoardData->actuatedAxes.at(j).controlMode == VOCAB_CM_HW_FAULT && mode != VOCAB_CM_FORCE_IDLE) { return true; @@ -267,7 +293,35 @@ bool ControlBoardDriver::setControlMode(const int j, const int mode) desired_mode = VOCAB_CM_IDLE; } - m_controlBoardData->physicalJoints.at(j).controlMode = desired_mode; + yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; + // If there is coupling let's check if we have to change the interaction mode for all the coupled joints + if (m_controlBoardData->ijointcoupling) + { + m_controlBoardData->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); + m_controlBoardData->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); + // If the joint is coupled, we have to change the interaction mode for all the coupled joints + if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) + { + for (auto& actuatedAxis : coupledActuatedAxes) + { + m_controlBoardData->actuatedAxes.at(actuatedAxis).commonJointProperties.controlMode = desired_mode; + } + for (auto& physicalJoint : coupledPhysicalJoints) + { + m_controlBoardData->physicalJoints.at(physicalJoint).commonJointProperties.controlMode = desired_mode; + } + } // if the joint is not coupled, we change the interaction mode only for the selected joint + else + { + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; + m_controlBoardData->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; + } + } // No coupling, we change the interaction mode only for the selected joint + else { + m_controlBoardData->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; + } + return true; } @@ -304,7 +358,7 @@ bool ControlBoardDriver::setControlModes(int* modes) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::setControlMode(i, modes[i])) { @@ -321,7 +375,7 @@ bool ControlBoardDriver::getAxisName(int axis, std::string& name) { // TODO integrate with IJointCoupled interface - name = m_controlBoardData->physicalJoints.at(axis).name; + name = m_controlBoardData->actuatedAxes.at(axis).name; return true; } @@ -341,14 +395,14 @@ bool ControlBoardDriver::setLimits(int axis, double min, double max) { std::lock_guard lock(m_controlBoardData->mutex); - if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) + if (axis < 0 || axis >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting limits: axis index out of range"; return false; } - m_controlBoardData->physicalJoints.at(axis).positionLimitMin = min; - m_controlBoardData->physicalJoints.at(axis).positionLimitMax = max; + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.positionLimitMin = min; + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.positionLimitMax = max; return true; } @@ -367,14 +421,14 @@ bool ControlBoardDriver::getLimits(int axis, double* min, double* max) yError() << "Error while getting limits: max is null"; return false; } - if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) + if (axis < 0 || axis >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting limits: axis index out of range"; return false; } - *min = m_controlBoardData->physicalJoints.at(axis).positionLimitMin; - *max = m_controlBoardData->physicalJoints.at(axis).positionLimitMax; + *min = m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.positionLimitMin; + *max = m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.positionLimitMax; return true; } @@ -383,14 +437,14 @@ bool ControlBoardDriver::setVelLimits(int axis, double min, double max) { std::lock_guard lock(m_controlBoardData->mutex); - if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) + if (axis < 0 || axis >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting velocity limits: axis index out of range"; return false; } - m_controlBoardData->physicalJoints.at(axis).velocityLimitMin = min; - m_controlBoardData->physicalJoints.at(axis).velocityLimitMax = max; + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.velocityLimitMin = min; + m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.velocityLimitMax = max; return true; } @@ -409,14 +463,14 @@ bool ControlBoardDriver::getVelLimits(int axis, double* min, double* max) yError() << "Error while getting velocity limits: max is null"; return false; } - if (axis < 0 || axis >= m_controlBoardData->physicalJoints.size()) + if (axis < 0 || axis >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting velocity limits: axis index out of range"; return false; } - *min = m_controlBoardData->physicalJoints.at(axis).velocityLimitMin; - *max = m_controlBoardData->physicalJoints.at(axis).velocityLimitMax; + *min = m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.velocityLimitMin; + *max = m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.velocityLimitMax; return true; } @@ -446,7 +500,7 @@ bool ControlBoardDriver::getAxes(int* ax) // TODO integrate with IJointCoupled interface std::lock_guard lock(m_controlBoardData->mutex); - *ax = m_controlBoardData->physicalJoints.size(); + *ax = m_controlBoardData->actuatedAxes.size(); return true; } @@ -460,13 +514,13 @@ bool ControlBoardDriver::getRefTorque(int j, double* t) yError() << "Error while getting reference torque: t is null"; return false; } - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting reference torque: joint index out of range"; return false; } - *t = m_controlBoardData->physicalJoints.at(j).refTorque; + *t = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.refTorque; return true; } @@ -479,7 +533,7 @@ bool ControlBoardDriver::getRefTorques(double* t) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getRefTorque(i, &t[i])) { @@ -494,7 +548,7 @@ bool ControlBoardDriver::setRefTorque(int j, double t) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting reference torque: joint index " + std::to_string(j) + " out of range"; @@ -505,7 +559,7 @@ bool ControlBoardDriver::setRefTorque(int j, double t) return false; } - m_controlBoardData->physicalJoints.at(j).refTorque = t; + m_controlBoardData->actuatedAxes.at(j).commonJointProperties.refTorque = t; return true; } @@ -518,7 +572,7 @@ bool ControlBoardDriver::setRefTorques(const double* t) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::setRefTorque(i, t[i])) { @@ -587,14 +641,14 @@ bool ControlBoardDriver::getTorque(int j, double* t) yError() << "Error while getting torque: t is null"; return false; } - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting torque: joint index " + std::to_string(j) + " out of range"; return false; } - *t = m_controlBoardData->physicalJoints.at(j).torque; + *t = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.torque; return true; } @@ -607,7 +661,7 @@ bool ControlBoardDriver::getTorques(double* t) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getTorque(i, &t[i])) { @@ -632,15 +686,15 @@ bool ControlBoardDriver::getTorqueRange(int j, double* min, double* max) yError() << "Error while getting torque range: max is null"; return false; } - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting torque range: joint index " + std::to_string(j) + " out of range"; return false; } - *min = -m_controlBoardData->physicalJoints.at(j).maxTorqueAbs; - *max = m_controlBoardData->physicalJoints.at(j).maxTorqueAbs; + *min = -m_controlBoardData->actuatedAxes.at(j).commonJointProperties.maxTorqueAbs; + *max = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.maxTorqueAbs; return true; } @@ -658,7 +712,7 @@ bool ControlBoardDriver::getTorqueRanges(double* min, double* max) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getTorqueRange(i, &min[i], &max[i])) { @@ -675,21 +729,21 @@ bool ControlBoardDriver::setPosition(int j, double ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting position: joint index " + std::to_string(j) + " out of range"; return false; } - if (m_controlBoardData->physicalJoints.at(j).controlMode != VOCAB_CM_POSITION_DIRECT) + if (m_controlBoardData->actuatedAxes.at(j).controlMode != VOCAB_CM_POSITION_DIRECT) { yError() << "Error while setting position: joint " + std::to_string(j) + " is not in position direct mode"; return false; } - m_controlBoardData->physicalJoints.at(j).refPosition = ref; + m_controlBoardData->actuatedAxes.at(j).commonJointProperties.refPosition = ref; return true; } @@ -726,7 +780,7 @@ bool ControlBoardDriver::setPositions(const double* refs) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::setPosition(i, refs[i])) { @@ -739,14 +793,14 @@ bool ControlBoardDriver::setPositions(const double* refs) bool ControlBoardDriver::getRefPosition(const int joint, double* ref) { - if (joint < 0 || joint >= m_controlBoardData->physicalJoints.size()) + if (joint < 0 || joint >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting reference position: joint index " + std::to_string(joint) + " out of range"; return false; } - *ref = m_controlBoardData->physicalJoints.at(joint).refPosition; + *ref = m_controlBoardData->actuatedAxes.at(joint).commonJointProperties.refPosition; return true; } @@ -759,7 +813,7 @@ bool ControlBoardDriver::getRefPositions(double* refs) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getRefPosition(i, &refs[i])) { @@ -800,24 +854,24 @@ bool ControlBoardDriver::positionMove(int j, double ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting reference position for trajectory generation: joint index " + std::to_string(j) + " out of range"; return false; } - auto& joint = m_controlBoardData->physicalJoints.at(j); + auto& joint = m_controlBoardData->actuatedAxes.at(j); joint.trajectoryGenerationRefPosition = ref; // TODO: use getLimits when recursive mutexes are implemented - auto limitMin = m_controlBoardData->physicalJoints.at(j).positionLimitMin; - auto limitMax = m_controlBoardData->physicalJoints.at(j).positionLimitMax; + auto limitMin = joint.commonJointProperties.positionLimitMin; + auto limitMax = joint.commonJointProperties.positionLimitMax; joint.trajectoryGenerator->setLimits(limitMin, limitMax); - joint.trajectoryGenerator->initTrajectory(joint.position, + joint.trajectoryGenerator->initTrajectory(joint.commonJointProperties.position, joint.trajectoryGenerationRefPosition, joint.trajectoryGenerationRefSpeed, joint.trajectoryGenerationRefAcceleration, @@ -835,7 +889,7 @@ bool ControlBoardDriver::positionMove(const double* refs) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::positionMove(i, refs[i])) { @@ -849,7 +903,7 @@ bool ControlBoardDriver::positionMove(const double* refs) bool ControlBoardDriver::relativeMove(int j, double delta) { // Check on valid joint number done in setPosition - return setPosition(j, m_controlBoardData->physicalJoints.at(j).position + delta); + return setPosition(j, m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position + delta); } bool ControlBoardDriver::relativeMove(const double* deltas) @@ -860,7 +914,7 @@ bool ControlBoardDriver::relativeMove(const double* deltas) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::relativeMove(i, deltas[i])) { @@ -875,13 +929,13 @@ bool ControlBoardDriver::checkMotionDone(int j, bool* flag) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while checking motion done: joint index out of range"; return false; } - *flag = m_controlBoardData->physicalJoints.at(j).isMotionDone; + *flag = m_controlBoardData->actuatedAxes.at(j).isMotionDone; return true; } @@ -894,7 +948,7 @@ bool ControlBoardDriver::checkMotionDone(bool* flag) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::checkMotionDone(i, &flag[i])) { @@ -905,6 +959,8 @@ bool ControlBoardDriver::checkMotionDone(bool* flag) return true; } +// TODO CONTINUE HERE! + bool ControlBoardDriver::setRefSpeed(int j, double sp) { std::lock_guard lock(m_controlBoardData->mutex); From 175e5dfeb8fbd81cb453e2350171014c1ccba78b Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 12 Dec 2024 11:11:33 +0100 Subject: [PATCH 06/11] ControlBoardDriver: finish first draft --- .../controlboard/src/ControlBoardDriver.cpp | 83 ++++++++++--------- 1 file changed, 42 insertions(+), 41 deletions(-) diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 32789b9e..50cdf946 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -959,19 +959,18 @@ bool ControlBoardDriver::checkMotionDone(bool* flag) return true; } -// TODO CONTINUE HERE! bool ControlBoardDriver::setRefSpeed(int j, double sp) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting reference speed: joint index out of range"; return false; } - m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefSpeed = sp; + m_controlBoardData->actuatedAxes.at(j).trajectoryGenerationRefSpeed = sp; return true; } @@ -984,7 +983,7 @@ bool ControlBoardDriver::setRefSpeeds(const double* spds) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::setRefSpeed(i, spds[i])) { @@ -999,13 +998,13 @@ bool ControlBoardDriver::setRefAcceleration(int j, double acc) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting reference acceleration: joint index out of range"; return false; } - m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefAcceleration = acc; + m_controlBoardData->actuatedAxes.at(j).trajectoryGenerationRefAcceleration = acc; return true; } @@ -1018,7 +1017,7 @@ bool ControlBoardDriver::setRefAccelerations(const double* accs) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::setRefAcceleration(i, accs[i])) { @@ -1033,13 +1032,13 @@ bool ControlBoardDriver::getRefSpeed(int j, double* ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting reference speed: joint index out of range"; return false; } - *ref = m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefSpeed; + *ref = m_controlBoardData->actuatedAxes.at(j).trajectoryGenerationRefSpeed; return true; } @@ -1052,7 +1051,7 @@ bool ControlBoardDriver::getRefSpeeds(double* spds) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getRefSpeed(i, &spds[i])) { @@ -1067,13 +1066,13 @@ bool ControlBoardDriver::getRefAcceleration(int j, double* acc) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting reference acceleration: joint index out of range"; return false; } - *acc = m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefAcceleration; + *acc = m_controlBoardData->actuatedAxes.at(j).trajectoryGenerationRefAcceleration; return true; } @@ -1086,7 +1085,7 @@ bool ControlBoardDriver::getRefAccelerations(double* accs) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getRefAcceleration(i, &accs[i])) { @@ -1101,24 +1100,24 @@ bool ControlBoardDriver::stop(int j) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while stopping: joint index out of range"; return false; } - switch (m_controlBoardData->physicalJoints.at(j).controlMode) + switch (m_controlBoardData->actuatedAxes.at(j).commonJointProperties.controlMode) { case VOCAB_CM_POSITION: - m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefPosition - = m_controlBoardData->physicalJoints.at(j).position; - m_controlBoardData->physicalJoints.at(j).trajectoryGenerator->abortTrajectory( - m_controlBoardData->physicalJoints.at(j).position); + m_controlBoardData->actuatedAxes.at(j).trajectoryGenerationRefPosition + = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position; + m_controlBoardData->actuatedAxes.at(j).trajectoryGenerator->abortTrajectory( + m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position); break; case VOCAB_CM_POSITION_DIRECT: - m_controlBoardData->physicalJoints.at(j).trajectoryGenerationRefPosition - = m_controlBoardData->physicalJoints.at(j).position; - m_controlBoardData->physicalJoints.at(j).refPosition = m_controlBoardData->physicalJoints.at(j).position; + m_controlBoardData->actuatedAxes.at(j).trajectoryGenerationRefPosition + = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position; + m_controlBoardData->actuatedAxes.at(j).commonJointProperties.refPosition = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position; break; case VOCAB_CM_VELOCITY: // TODO velocity control @@ -1135,7 +1134,7 @@ bool ControlBoardDriver::stop(int j) bool ControlBoardDriver::stop() { - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::stop(i)) { @@ -1338,14 +1337,14 @@ bool ControlBoardDriver::getTargetPosition(const int joint, double* ref) { std::lock_guard lock(m_controlBoardData->mutex); - if (joint < 0 || joint >= m_controlBoardData->physicalJoints.size()) + if (joint < 0 || joint >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting target position: joint index " + std::to_string(joint) + " out of range"; return false; } - *ref = m_controlBoardData->physicalJoints.at(joint).trajectoryGenerationRefPosition; + *ref = m_controlBoardData->actuatedAxes.at(joint).trajectoryGenerationRefPosition; return true; } @@ -1358,7 +1357,7 @@ bool ControlBoardDriver::getTargetPositions(double* refs) return false; } - for (size_t i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (size_t i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getTargetPosition(i, &refs[i])) { @@ -1589,6 +1588,8 @@ bool ControlBoardDriver::isPidEnabled(const PidControlTypeEnum& pidtype, int j, // IEncodersTimed + + /** * Since we don't know how to reset gazebo encoders, we will simply add the actual value to the * future encoders readings @@ -1597,20 +1598,20 @@ bool ControlBoardDriver::resetEncoder(int j) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while resetting encoder: joint index out of range"; return false; } - m_controlBoardData->physicalJoints.at(j).zeroPosition = m_controlBoardData->physicalJoints.at(j).position; + m_controlBoardData->actuatedAxes.at(j).commonJointProperties.zeroPosition = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position; return true; } bool ControlBoardDriver::resetEncoders() { - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::resetEncoder(i)) { @@ -1625,14 +1626,14 @@ bool ControlBoardDriver::setEncoder(int j, double val) { std::lock_guard lock(m_controlBoardData->mutex); - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while setting encoder: joint index " + std::to_string(j) + " out of range"; return false; } - m_controlBoardData->physicalJoints.at(j).zeroPosition = m_controlBoardData->physicalJoints.at(j).position - val; + m_controlBoardData->actuatedAxes.at(j).commonJointProperties.zeroPosition = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position - val; return true; } @@ -1645,7 +1646,7 @@ bool ControlBoardDriver::setEncoders(const double* vals) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::setEncoder(i, vals[i])) { @@ -1665,14 +1666,14 @@ bool ControlBoardDriver::getEncoder(int j, double* v) yError() << "Error while getting encoder: v is null"; return false; } - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting encoder: joint index " + std::to_string(j) + " out of range"; return false; } - *v = m_controlBoardData->physicalJoints.at(j).position - m_controlBoardData->physicalJoints.at(j).zeroPosition; + *v = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position - m_controlBoardData->actuatedAxes.at(j).commonJointProperties.zeroPosition; return true; } @@ -1685,7 +1686,7 @@ bool ControlBoardDriver::getEncoders(double* encs) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getEncoder(i, &encs[i])) { @@ -1706,14 +1707,14 @@ bool ControlBoardDriver::getEncoderSpeed(int j, double* sp) return false; } - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting encoder speed: joint index " + std::to_string(j) + " out of range"; return false; } - *sp = m_controlBoardData->physicalJoints.at(j).velocity; + *sp = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.velocity; return true; } @@ -1726,7 +1727,7 @@ bool ControlBoardDriver::getEncoderSpeeds(double* spds) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getEncoderSpeed(i, &spds[i])) { @@ -1765,7 +1766,7 @@ bool ControlBoardDriver::getEncoderTimed(int j, double* encs, double* time) yError() << "Error while getting encoder: time is null"; return false; } - if (j < 0 || j >= m_controlBoardData->physicalJoints.size()) + if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) { yError() << "Error while getting encoder: joint index " + std::to_string(j) + " out of range"; @@ -1773,7 +1774,7 @@ bool ControlBoardDriver::getEncoderTimed(int j, double* encs, double* time) } *encs - = m_controlBoardData->physicalJoints.at(j).position - m_controlBoardData->physicalJoints.at(j).zeroPosition; + = m_controlBoardData->actuatedAxes.at(j).commonJointProperties.position - m_controlBoardData->actuatedAxes.at(j).commonJointProperties.zeroPosition; return true; } @@ -1791,7 +1792,7 @@ bool ControlBoardDriver::getEncodersTimed(double* encs, double* time) return false; } - for (int i = 0; i < m_controlBoardData->physicalJoints.size(); i++) + for (int i = 0; i < m_controlBoardData->actuatedAxes.size(); i++) { if (!ControlBoardDriver::getEncoderTimed(i, &encs[i], &time[i])) { From 5755eb82827fad688e14c42fcf9a6ef0ea6927b4 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 12 Dec 2024 11:21:58 +0100 Subject: [PATCH 07/11] Add refactored version of setInteractionMode/setControlMode --- plugins/controlboard/CMakeLists.txt | 1 + .../controlboard/include/ControlBoardData.hh | 3 + plugins/controlboard/src/ControlBoardData.cpp | 113 ++++++++++++++++++ .../controlboard/src/ControlBoardDriver.cpp | 106 +--------------- 4 files changed, 119 insertions(+), 104 deletions(-) create mode 100644 plugins/controlboard/src/ControlBoardData.cpp diff --git a/plugins/controlboard/CMakeLists.txt b/plugins/controlboard/CMakeLists.txt index 0c868c43..1d7a69b7 100644 --- a/plugins/controlboard/CMakeLists.txt +++ b/plugins/controlboard/CMakeLists.txt @@ -7,6 +7,7 @@ set(HEADER_FILES set(SRC_FILES src/ControlBoard.cpp + src/ControlBoardData.cpp src/ControlBoardDriver.cpp src/ControlBoardTrajectory.cpp ) diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh index 599ac236..9df7a308 100644 --- a/plugins/controlboard/include/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -82,6 +82,9 @@ public: yarp::dev::IJointCoupling* ijointcoupling{nullptr}; // TODO (xela95): read this value from configuration file std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1); + + bool setInteractionMode((int axis, yarp::dev::InteractionModeEnum mode); + bool setControlMode(int j, int mode); }; class IControlBoardData diff --git a/plugins/controlboard/src/ControlBoardData.cpp b/plugins/controlboard/src/ControlBoardData.cpp new file mode 100644 index 00000000..a1a61784 --- /dev/null +++ b/plugins/controlboard/src/ControlBoardData.cpp @@ -0,0 +1,113 @@ + +#include "ControlBoardData.h" + +namespace yarp::dev::gzyarp +{ +bool ControlBoardData::setInteractionMode(int axis, InteractionModeEnum mode) +{ + yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; + try + { + // If there is coupling let's check if we have to change the interaction mode for all the coupled joints + if (this->ijointcoupling) + { + this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); + this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); + // If the joint is coupled, we have to change the interaction mode for all the coupled joints + if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) + { + for (auto& actuatedAxis : coupledActuatedAxes) + { + this->actuatedAxes.at(actuatedAxis).commonJointProperties.interactionMode = mode; + } + for (auto& physicalJoint : coupledPhysicalJoints) + { + this->physicalJoints.at(physicalJoint).commonJointProperties.interactionMode = mode; + } + } // if the joint is not coupled, we change the interaction mode only for the selected joint + else + { + this->actuatedAxes.at(axis).commonJointProperties.interactionMode = mode; + this->physicalJoints.at(axis).commonJointProperties.interactionMode = mode; + } + } // No coupling, we change the interaction mode only for the selected joint + else { + this->physicalJoints.at(axis).commonJointProperties.interactionMode = mode; + this->actuatedAxes.at(axis).commonJointProperties.interactionMode = mode; + } + } catch (const std::exception& e) + { + yError() << "Error while setting interaction mode for axis " + std::to_string(axis) + ": \n" + + e.what(); + return false; + } +} + +bool ControlBoardData::setControlMode(int j, int mode) { + int desired_mode = mode; + + if (j < 0 || j >= this->actuatedAxes.size()) + { + yError() << "Error while setting control mode: joint index out of range"; + return false; + } + + // Only accept supported control modes + // The only not supported control mode is + // (for now) VOCAB_CM_MIXED + if (!(mode == VOCAB_CM_POSITION || mode == VOCAB_CM_POSITION_DIRECT || mode == VOCAB_CM_VELOCITY + || mode == VOCAB_CM_TORQUE || mode == VOCAB_CM_MIXED || mode == VOCAB_CM_PWM + || mode == VOCAB_CM_CURRENT || mode == VOCAB_CM_IDLE || mode == VOCAB_CM_FORCE_IDLE)) + { + yWarning() << "request control mode " << yarp::os::Vocab32::decode(mode) + << " that is not supported by " + << " gz-sim-yarp-controlboard-system plugin."; + return false; + } + + // If joint is in hw fault, only a force idle command can recover it + if (this->actuatedAxes.at(j).controlMode == VOCAB_CM_HW_FAULT + && mode != VOCAB_CM_FORCE_IDLE) + { + return true; + } + + if (mode == VOCAB_CM_FORCE_IDLE) + { + // Clean the fault status and set control mode to idle + desired_mode = VOCAB_CM_IDLE; + } + + yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; + // If there is coupling let's check if we have to change the interaction mode for all the coupled joints + if (this->ijointcoupling) + { + this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); + this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); + // If the joint is coupled, we have to change the interaction mode for all the coupled joints + if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) + { + for (auto& actuatedAxis : coupledActuatedAxes) + { + this->actuatedAxes.at(actuatedAxis).commonJointProperties.controlMode = desired_mode; + } + for (auto& physicalJoint : coupledPhysicalJoints) + { + this->physicalJoints.at(physicalJoint).commonJointProperties.controlMode = desired_mode; + } + } // if the joint is not coupled, we change the interaction mode only for the selected joint + else + { + this->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; + this->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; + } + } // No coupling, we change the interaction mode only for the selected joint + else { + this->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; + this->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; + } + +} + + +} // namespace yarp::dev::gzyarp diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 50cdf946..32401aa8 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -103,44 +103,7 @@ bool ControlBoardDriver::getInteractionModes(yarp::dev::InteractionModeEnum* mod bool ControlBoardDriver::setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) { std::lock_guard lock(m_controlBoardData->mutex); - yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; - try - { - // If there is coupling let's check if we have to change the interaction mode for all the coupled joints - if (m_controlBoardData->ijointcoupling) - { - m_controlBoardData->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); - m_controlBoardData->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); - // If the joint is coupled, we have to change the interaction mode for all the coupled joints - if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) - { - for (auto& actuatedAxis : coupledActuatedAxes) - { - m_controlBoardData->actuatedAxes.at(actuatedAxis).commonJointProperties.interactionMode = mode; - } - for (auto& physicalJoint : coupledPhysicalJoints) - { - m_controlBoardData->physicalJoints.at(physicalJoint).commonJointProperties.interactionMode = mode; - } - } // if the joint is not coupled, we change the interaction mode only for the selected joint - else - { - m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.interactionMode = mode; - m_controlBoardData->physicalJoints.at(axis).commonJointProperties.interactionMode = mode; - } - } // No coupling, we change the interaction mode only for the selected joint - else { - m_controlBoardData->physicalJoints.at(axis).commonJointProperties.interactionMode = mode; - m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.interactionMode = mode; - } - } catch (const std::exception& e) - { - yError() << "Error while setting interaction mode for axis " + std::to_string(axis) + ": \n" - + e.what(); - return false; - } - - return true; + return m_controlBoardData->setInteractionMode(axis, mode); } bool ControlBoardDriver::setInteractionModes(int n_joints, @@ -258,72 +221,7 @@ bool ControlBoardDriver::getControlModes(const int n_joint, const int* joints, i bool ControlBoardDriver::setControlMode(const int j, const int mode) { std::lock_guard lock(m_controlBoardData->mutex); - - int desired_mode = mode; - - if (j < 0 || j >= m_controlBoardData->actuatedAxes.size()) - { - yError() << "Error while setting control mode: joint index out of range"; - return false; - } - - // Only accept supported control modes - // The only not supported control mode is - // (for now) VOCAB_CM_MIXED - if (!(mode == VOCAB_CM_POSITION || mode == VOCAB_CM_POSITION_DIRECT || mode == VOCAB_CM_VELOCITY - || mode == VOCAB_CM_TORQUE || mode == VOCAB_CM_MIXED || mode == VOCAB_CM_PWM - || mode == VOCAB_CM_CURRENT || mode == VOCAB_CM_IDLE || mode == VOCAB_CM_FORCE_IDLE)) - { - yWarning() << "request control mode " << yarp::os::Vocab32::decode(mode) - << " that is not supported by " - << " gz-sim-yarp-controlboard-system plugin."; - return false; - } - - // If joint is in hw fault, only a force idle command can recover it - if (m_controlBoardData->actuatedAxes.at(j).controlMode == VOCAB_CM_HW_FAULT - && mode != VOCAB_CM_FORCE_IDLE) - { - return true; - } - - if (mode == VOCAB_CM_FORCE_IDLE) - { - // Clean the fault status and set control mode to idle - desired_mode = VOCAB_CM_IDLE; - } - - yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; - // If there is coupling let's check if we have to change the interaction mode for all the coupled joints - if (m_controlBoardData->ijointcoupling) - { - m_controlBoardData->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); - m_controlBoardData->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); - // If the joint is coupled, we have to change the interaction mode for all the coupled joints - if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) - { - for (auto& actuatedAxis : coupledActuatedAxes) - { - m_controlBoardData->actuatedAxes.at(actuatedAxis).commonJointProperties.controlMode = desired_mode; - } - for (auto& physicalJoint : coupledPhysicalJoints) - { - m_controlBoardData->physicalJoints.at(physicalJoint).commonJointProperties.controlMode = desired_mode; - } - } // if the joint is not coupled, we change the interaction mode only for the selected joint - else - { - m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; - m_controlBoardData->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; - } - } // No coupling, we change the interaction mode only for the selected joint - else { - m_controlBoardData->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; - m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; - } - - - return true; + return m_controlBoardData->setControlMode(j, mode); } bool ControlBoardDriver::setControlModes(const int n_joint, const int* joints, int* modes) From 27b52f9388ecbf79438bbb65e7908b085bc82aa3 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 12 Dec 2024 15:52:55 +0100 Subject: [PATCH 08/11] ControlBoard: first compiling version after the refactoring for introducting the coupling --- plugins/controlboard/include/ControlBoard.hh | 13 +- .../controlboard/include/ControlBoardData.hh | 4 +- plugins/controlboard/src/ControlBoard.cpp | 262 ++++++++++-------- plugins/controlboard/src/ControlBoardData.cpp | 23 +- .../controlboard/src/ControlBoardDriver.cpp | 4 +- 5 files changed, 175 insertions(+), 131 deletions(-) diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index 65b88c1b..f96815bc 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -65,6 +65,10 @@ private: yarp::os::Network m_yarpNetwork; yarp::os::Property m_pluginParameters; gz::sim::EntityComponentManager* m_ecm; + yarp::sig::Vector m_physicalJointsPositionBuffer, m_physicalJointsVelocityBuffer, + m_physicalJointsTorqueBuffer; + yarp::sig::Vector m_actuatedAxesPositionBuffer, m_actuatedAxesVelocityBuffer, + m_actuatedAxesTorqueBuffer; enum class AngleUnitEnum { @@ -92,15 +96,16 @@ private: std::vector& yarpPIDs, size_t numberOfPhysicalJoints); void setJointPositionPIDs(AngleUnitEnum cUnits, const std::vector& yarpPIDs); - double convertUserGainToGazeboGain(JointProperties& joint, double value); - double convertGazeboGainToUserGain(JointProperties& joint, double value); - double convertGazeboToUser(JointProperties& joint, double value); - double convertUserToGazebo(JointProperties& joint, double value); + double convertUserGainToGazeboGain(PhysicalJointProperties& joint, double value); + double convertGazeboGainToUserGain(PhysicalJointProperties& joint, double value); + double convertGazeboToUser(PhysicalJointProperties& joint, double value); + double convertUserToGazebo(PhysicalJointProperties& joint, double value); bool initializeJointPositionLimits(const gz::sim::EntityComponentManager& ecm); bool initializeTrajectoryGenerators(); bool initializeTrajectoryGeneratorReferences(yarp::os::Bottle& trajectoryGeneratorsGroup); bool parseInitialConfiguration(std::vector& initialConfigurations); void resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponentManager& ecm); + void configureBuffers(); }; } // namespace gzyarp diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh index 9df7a308..8ceb844c 100644 --- a/plugins/controlboard/include/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -52,7 +52,7 @@ struct CommonJointProperties { double velocity{0.0}; // Joint velocity [deg/s] double velocityLimitMin{std::numeric_limits::min()}; double velocityLimitMax{std::numeric_limits::max()}; -} +}; struct PhysicalJointProperties { @@ -83,7 +83,7 @@ public: // TODO (xela95): read this value from configuration file std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1); - bool setInteractionMode((int axis, yarp::dev::InteractionModeEnum mode); + bool setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode); bool setControlMode(int j, int mode); }; diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index bb9f8211..c5a393ec 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -175,7 +175,7 @@ void ControlBoard::Configure(const Entity& _entity, yError() << "gz-sim-yarp-controlboard-system: failed setting joint properties"; return; } - + configureBuffers(); resetPositionsAndTrajectoryGenerators(_ecm); configureHelper.setConfigureIsSuccessful(true); @@ -260,7 +260,7 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm) gzJoint.EnableTransmittedWrenchCheck(_ecm, true); // Initialize JointProperties object - m_controlBoardData.physicalJoints[i].name = jointFromConfigName; + m_controlBoardData.physicalJoints[i].commonJointProperties.name = jointFromConfigName; yInfo() << "Joint " << jointFromConfigName << " added to the control board data."; } @@ -301,48 +301,71 @@ bool ControlBoard::readJointsMeasurements(const gz::sim::EntityComponentManager& auto model = Model(m_modelEntity); Joint gzJoint; - for (auto& joint : m_controlBoardData.physicalJoints) + for (int i =0; iconvertFromPhysicalJointsToActuatedAxesPos(m_physicalJointsPositionBuffer, m_actuatedAxesPositionBuffer); + m_controlBoardData.ijointcoupling->convertFromPhysicalJointsToActuatedAxesVel(m_physicalJointsPositionBuffer, m_physicalJointsVelocityBuffer, m_actuatedAxesVelocityBuffer); + for (int i = 0; i < m_controlBoardData.actuatedAxes.size(); i++) + { + m_controlBoardData.actuatedAxes[i].commonJointProperties.position = m_actuatedAxesPositionBuffer[i]; + m_controlBoardData.actuatedAxes[i].commonJointProperties.velocity = m_actuatedAxesVelocityBuffer[i]; + } + } + else { + // If no coupling is present, the actuated axes are the same as the physical joints + for (int i = 0; i < m_controlBoardData.actuatedAxes.size(); i++) + { + m_controlBoardData.actuatedAxes[i].commonJointProperties.position = m_controlBoardData.physicalJoints[i].commonJointProperties.position; + m_controlBoardData.actuatedAxes[i].commonJointProperties.velocity = m_controlBoardData.physicalJoints[i].commonJointProperties.velocity; + m_controlBoardData.actuatedAxes[i].commonJointProperties.torque = m_controlBoardData.physicalJoints[i].commonJointProperties.torque; + } + } return true; } @@ -366,13 +389,13 @@ ControlBoard::getJointTorqueFromTransmittedWrench(const Joint& gzJoint, void ControlBoard::checkForJointsHwFault() { std::lock_guard lock(m_controlBoardData.mutex); - for (auto& joint : *m_controlBoardData.actuated_joints_handle) + for (auto& joint : m_controlBoardData.actuatedAxes) { - if (joint.controlMode != VOCAB_CM_HW_FAULT && std::abs(joint.torque) > joint.maxTorqueAbs) + if (joint.commonJointProperties.controlMode != VOCAB_CM_HW_FAULT && std::abs(joint.commonJointProperties.torque) > joint.commonJointProperties.maxTorqueAbs) { - joint.controlMode = VOCAB_CM_HW_FAULT; - yError() << "An hardware fault occurred on joint " << joint.name - << " torque too big! ( " << joint.torque << " )"; + joint.commonJointProperties.controlMode = VOCAB_CM_HW_FAULT; + yError() << "An hardware fault occurred on joint " << joint.commonJointProperties.name + << " torque too big! ( " << joint.commonJointProperties.torque << " )"; } } } @@ -383,14 +406,14 @@ bool ControlBoard::updateTrajectories(const UpdateInfo& _info, EntityComponentMa // TODO: execute the following at control update time - for (auto& joint : *m_controlBoardData.actuated_joints_handle) + for (auto& joint : m_controlBoardData.actuatedAxes) { - switch (joint.controlMode) + switch (joint.commonJointProperties.controlMode) { // PUT THE COUPLING HERE!! case VOCAB_CM_POSITION: - joint.refPosition = joint.trajectoryGenerator->computeTrajectory(); + joint.commonJointProperties.refPosition = joint.trajectoryGenerator->computeTrajectory(); joint.isMotionDone = joint.trajectoryGenerator->isMotionDone(); break; case VOCAB_CM_MIXED: @@ -416,33 +439,38 @@ bool ControlBoard::updateReferences(const UpdateInfo& _info, EntityComponentMana if(m_controlBoardData.ijointcoupling){ - yarp::sig::Vector actuatedAxesRefPositions, physicalJointsRefPositions; - for (auto& joint: m_controlBoardData.actuatedAxes) + for (int i = 0; i < m_controlBoardData.actuatedAxes.size(); i++) { - actuatedAxesRefPositions.push_back(joint.refPosition); + m_actuatedAxesPositionBuffer[i] = m_controlBoardData.actuatedAxes[i].commonJointProperties.refPosition; } - physicalJointsRefPositions.resize(m_controlBoardData.physicalJoints.size()); - - m_controlBoardData.ijointcoupling->convertFromActuatedAxesToPhysicalJointsPos(actuatedAxesRefPositions, physicalJointsRefPositions); + m_controlBoardData.ijointcoupling->convertFromActuatedAxesToPhysicalJointsPos(m_actuatedAxesPositionBuffer, m_physicalJointsPositionBuffer); + for(auto i = 0; i < m_controlBoardData.physicalJoints.size(); i++) { - m_controlBoardData.physicalJoints[i].refPosition = physicalJointsRefPositions[i]; + m_controlBoardData.physicalJoints[i].commonJointProperties.refPosition = m_physicalJointsPositionBuffer[i]; + } + } + else { + // If no coupling is present, the actuated axes are the same as the physical joints + for (int i = 0; i < m_controlBoardData.actuatedAxes.size(); i++) + { + m_controlBoardData.physicalJoints[i].commonJointProperties.refPosition = m_controlBoardData.actuatedAxes[i].commonJointProperties.refPosition; } } for (auto& joint : m_controlBoardData.physicalJoints) { - switch (joint.controlMode) + switch (joint.commonJointProperties.controlMode) { case VOCAB_CM_TORQUE: - forceReference = joint.refTorque; + forceReference = joint.commonJointProperties.refTorque; break; case VOCAB_CM_POSITION: case VOCAB_CM_POSITION_DIRECT: { // TODO manage motor positions instead of joint positions when implemented auto& pid = joint.pidControllers[yarp::dev::VOCAB_PIDTYPE_POSITION]; - forceReference = pid.Update(convertUserToGazebo(joint, joint.position) - - convertUserToGazebo(joint, joint.refPosition), + forceReference = pid.Update(convertUserToGazebo(joint, joint.commonJointProperties.position) + - convertUserToGazebo(joint, joint.commonJointProperties.refPosition), _info.dt); break; } @@ -451,7 +479,7 @@ bool ControlBoard::updateReferences(const UpdateInfo& _info, EntityComponentMana forceReference = 0.0; break; default: - yWarning() << "Joint " << joint.name << " control mode " << joint.controlMode + yWarning() << "Joint " << joint.commonJointProperties.name << " control mode " << joint.commonJointProperties.controlMode << " is currently not implemented"; return false; }; @@ -460,10 +488,10 @@ bool ControlBoard::updateReferences(const UpdateInfo& _info, EntityComponentMana try { - gzJoint = Joint(Model(m_modelEntity).JointByName(_ecm, joint.name)); + gzJoint = Joint(Model(m_modelEntity).JointByName(_ecm, joint.commonJointProperties.name)); } catch (const std::exception& e) { - yError() << "Error while trying to access joint " << joint.name; + yError() << "Error while trying to access joint " << joint.commonJointProperties.name; return false; } @@ -664,25 +692,25 @@ void ControlBoard::setJointPositionPIDs(AngleUnitEnum cUnits, } } -double ControlBoard::convertUserGainToGazeboGain(JointProperties& joint, double value) +double ControlBoard::convertUserGainToGazeboGain(PhysicalJointProperties& joint, double value) { // TODO discriminate between joint types return gzyarp::convertDegreeGainToRadianGains(value); } -double ControlBoard::convertGazeboGainToUserGain(JointProperties& joint, double value) +double ControlBoard::convertGazeboGainToUserGain(PhysicalJointProperties& joint, double value) { // TODO discriminate between joint types return gzyarp::convertRadianGainToDegreeGains(value); } -double ControlBoard::convertGazeboToUser(JointProperties& joint, double value) +double ControlBoard::convertGazeboToUser(PhysicalJointProperties& joint, double value) { // TODO discriminate between joint types return convertRadiansToDegrees(value); } -double ControlBoard::convertUserToGazebo(JointProperties& joint, double value) +double ControlBoard::convertUserToGazebo(PhysicalJointProperties& joint, double value) { // TODO discriminate between joint types return convertDegreesToRadians(value); @@ -712,8 +740,8 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM // TODO: access gazebo joint position limits and use them to check if software limits // ([LIMITS] group) are consistent. In case they are not defined set them as sw limits. auto& joint = m_controlBoardData.physicalJoints[i]; - joint.positionLimitMin = limitMinGroup.at(i); - joint.positionLimitMax = limitMaxGroup.at(i); + joint.commonJointProperties.positionLimitMin = limitMinGroup.at(i); + joint.commonJointProperties.positionLimitMax = limitMaxGroup.at(i); } // We have also to set the actuated axes limits if (m_controlBoardData.ijointcoupling) @@ -724,17 +752,9 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM yError() << "Failed to get coupling group"; return false; } - size_t nrOfActuatedAxes{0}; - bool ok = m_controlBoardData.ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); - if(!ok) - { - yError() << "Failed to get number of actuated axes"; - return false; - } - m_controlBoardData.actuatedAxes.resize(nrOfActuatedAxes); std::vector actuatedAxisPosLimitsMin, actuatedAxisPosLimitsMax; - if (!(tryGetGroup(couplingGroup, actuatedAxisPosLimitsMin, "actuatedAxesPosMin", "", numberOfPhysicalJoints) - && tryGetGroup(couplingGroup, actuatedAxisPosLimitsMax, "actuatedAxesPosMax", "", numberOfPhysicalJoints))) + if (!(tryGetGroup(couplingGroup, actuatedAxisPosLimitsMin, "actuatedAxesPosMin", "", m_controlBoardData.actuatedAxes.size()) + && tryGetGroup(couplingGroup, actuatedAxisPosLimitsMax, "actuatedAxesPosMax", "", m_controlBoardData.actuatedAxes.size()))) { yError() << "Error while reading joint position limits from plugin configuration"; return false; @@ -742,8 +762,8 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM for (size_t i = 0; i< m_controlBoardData.actuatedAxes.size(); ++i) { auto& actuatedAxis = m_controlBoardData.actuatedAxes[i]; - actuatedAxis.positionLimitMin = actuatedAxisPosLimitsMin.at(i); - actuatedAxis.positionLimitMax = actuatedAxisPosLimitsMax.at(i); + actuatedAxis.commonJointProperties.positionLimitMin = actuatedAxisPosLimitsMin.at(i); + actuatedAxis.commonJointProperties.positionLimitMax = actuatedAxisPosLimitsMax.at(i); } } @@ -756,8 +776,6 @@ bool ControlBoard::initializeTrajectoryGenerators() // Read from configuration auto trajectoryGeneratorsGroup = m_pluginParameters.findGroup("TRAJECTORY_GENERATION"); bool missingConfiguration = false; - // In case of coupled systems, the trajectory generators has to be initialized only for the actuated axes - m_controlBoardData.actuated_joints_handle = m_controlBoardData.ijointcoupling ? &(m_controlBoardData.actuatedAxes) : &(m_controlBoardData.physicalJoints); if (trajectoryGeneratorsGroup.isNull()) { yWarning() << "Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults to " @@ -799,7 +817,7 @@ bool ControlBoard::initializeTrajectoryGenerators() } } - for (auto& joint : *m_controlBoardData.actuated_joints_handle) + for (auto& joint : m_controlBoardData.actuatedAxes) { joint.trajectoryGenerator = yarp::dev::gzyarp::TrajectoryGeneratorFactory::create(trajectoryType); @@ -854,9 +872,9 @@ bool ControlBoard::initializeTrajectoryGeneratorReferences(Bottle& trajectoryGen // Set trajectory generation reference speed and acceleration // TODO: manage different joint types HERE - for (size_t i = 0; i < m_controlBoardData.actuated_joints_handle->size(); ++i) + for (size_t i = 0; i < m_controlBoardData.actuatedAxes.size(); ++i) { - auto& joint = m_controlBoardData.actuated_joints_handle->at(i); + auto& joint = m_controlBoardData.actuatedAxes.at(i); if (useDefaultSpeedRef) { joint.trajectoryGenerationRefSpeed = 10.0; // [deg/s] @@ -874,15 +892,15 @@ bool ControlBoard::initializeTrajectoryGeneratorReferences(Bottle& trajectoryGen } // Clip trajectory generation reference velocities according to max joint limit - if (joint.trajectoryGenerationRefSpeed > joint.velocityLimitMax) + if (joint.trajectoryGenerationRefSpeed > joint.commonJointProperties.velocityLimitMax) { - joint.trajectoryGenerationRefSpeed = joint.velocityLimitMax; + joint.trajectoryGenerationRefSpeed = joint.commonJointProperties.velocityLimitMax; } - yDebug() << "Joint " << joint.name + yDebug() << "Joint " << joint.commonJointProperties.name << " trajectory generation reference speed: " << joint.trajectoryGenerationRefSpeed << " [deg/s]"; - yDebug() << "Joint " << joint.name << " trajectory generation reference acceleration: " + yDebug() << "Joint " << joint.commonJointProperties.name << " trajectory generation reference acceleration: " << joint.trajectoryGenerationRefAcceleration << " [deg/s^2]"; } @@ -932,88 +950,108 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen auto gzPos = initialConfigurations[i]; auto userPos = convertGazeboToUser(joint, gzPos); // Reset joint properties - joint.trajectoryGenerationRefPosition = userPos; - joint.refPosition = userPos; - joint.position = userPos; + joint.commonJointProperties.refPosition = userPos; + joint.commonJointProperties.position = userPos; + m_physicalJointsPositionBuffer[i] = userPos; // Reset position of gazebo joint // TODO(xela-95): store joint entity in JointProperties - Joint(Model(m_modelEntity).JointByName(ecm, joint.name)) + Joint(Model(m_modelEntity).JointByName(ecm, joint.commonJointProperties.name)) .ResetPosition(ecm, std::vector{gzPos}); - auto limitMin = joint.positionLimitMin; - auto limitMax = joint.positionLimitMax; - joint.trajectoryGenerator->setLimits(limitMin, limitMax); - joint.trajectoryGenerator->initTrajectory(joint.position, - joint.position, - joint.trajectoryGenerationRefSpeed, - joint.trajectoryGenerationRefAcceleration, - m_controlBoardData.controlUpdatePeriod); } } else { yWarning() << "No initial configuration found, initializing trajectory generator with " "current values"; - yarp::sig::Vector initialPositionAct, initialPositionPhys, limitsMin, limitsMax; for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); i++) { auto& joint = m_controlBoardData.physicalJoints.at(i); - auto gzJoint = Joint(Model(m_modelEntity).JointByName(ecm, joint.name)); + auto gzJoint = Joint(Model(m_modelEntity).JointByName(ecm, joint.commonJointProperties.name)); if (gzJoint.Position(ecm).has_value() && gzJoint.Position(ecm).value().size() > 0) { auto gzPos = gzJoint.Position(ecm).value().at(0); auto userPos = convertGazeboToUser(joint, gzPos); // Reset joint properties - joint.trajectoryGenerationRefPosition = userPos; - joint.refPosition = userPos; - joint.position = userPos; - initialPositionPhys.push_back(userPos); + joint.commonJointProperties.refPosition = userPos; + joint.commonJointProperties.position = userPos; + m_physicalJointsPositionBuffer[i] = userPos; } } + } - if (m_controlBoardData.ijointcoupling) { - initialPositionAct.resize(m_controlBoardData.actuatedAxes.size()); - auto ok = m_controlBoardData.ijointcoupling->convertFromPhysicalJointsToActuatedAxesPos(initialPositionPhys, initialPositionAct); - if(!ok) - { - yError() << "Failed to convert from physical joints to actuated axes"; - return; - } - for(size_t i=0; iconvertFromPhysicalJointsToActuatedAxesPos(m_physicalJointsPositionBuffer, m_actuatedAxesPositionBuffer); + if(!ok) + { + yError() << "Failed to convert from physical joints to actuated axes"; + return; } - for (size_t i=0; isize(); i++) { - auto& joint = m_controlBoardData.actuated_joints_handle->at(i); - auto limitMin = m_controlBoardData.ijointcoupling ? m_controlBoardData.actuatedAxes[i].positionLimitMin : m_controlBoardData.physicalJoints[i].positionLimitMin; - auto limitMax = m_controlBoardData.ijointcoupling ? m_controlBoardData.actuatedAxes[i].positionLimitMax : m_controlBoardData.physicalJoints[i].positionLimitMax; - joint.trajectoryGenerator->setLimits(joint.positionLimitMin, joint.positionLimitMax); - joint.trajectoryGenerator->initTrajectory(initialPositionAct[i], - initialPositionAct[i], - joint.trajectoryGenerationRefSpeed, - joint.trajectoryGenerationRefAcceleration, - m_controlBoardData.controlUpdatePeriod); + for(size_t i=0; isetLimits(limitMin, limitMax); + joint.trajectoryGenerator->initTrajectory(m_actuatedAxesPositionBuffer[i], + m_actuatedAxesPositionBuffer[i], + joint.trajectoryGenerationRefSpeed, + joint.trajectoryGenerationRefAcceleration, + m_controlBoardData.controlUpdatePeriod); + } // Reset control mode - for (auto& joint : m_controlBoardData.physicalJoints) + for (int i=0; igetNrOfActuatedAxes(nrOfActuatedAxes); + if(!ok) + { + yError() << "Failed to get number of actuated axes"; + return; + } + m_controlBoardData.actuatedAxes.resize(nrOfActuatedAxes); + m_actuatedAxesPositionBuffer.resize(nrOfActuatedAxes); + m_actuatedAxesVelocityBuffer.resize(nrOfActuatedAxes); + m_actuatedAxesTorqueBuffer.resize(nrOfActuatedAxes); + size_t nrOfPhysicalJoints{0}; + ok = m_controlBoardData.ijointcoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints); + if(!ok) + { + yError() << "Failed to get number of physical joints"; + return; + } + m_controlBoardData.physicalJoints.resize(nrOfPhysicalJoints); + m_physicalJointsPositionBuffer.resize(nrOfPhysicalJoints); + m_physicalJointsVelocityBuffer.resize(nrOfPhysicalJoints); + m_physicalJointsTorqueBuffer.resize(nrOfPhysicalJoints); } + } } // namespace gzyarp diff --git a/plugins/controlboard/src/ControlBoardData.cpp b/plugins/controlboard/src/ControlBoardData.cpp index a1a61784..7d7e42b7 100644 --- a/plugins/controlboard/src/ControlBoardData.cpp +++ b/plugins/controlboard/src/ControlBoardData.cpp @@ -1,9 +1,9 @@ -#include "ControlBoardData.h" +#include "ControlBoardData.hh" -namespace yarp::dev::gzyarp +namespace gzyarp { -bool ControlBoardData::setInteractionMode(int axis, InteractionModeEnum mode) +bool ControlBoardData::setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) { yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; try @@ -41,6 +41,7 @@ bool ControlBoardData::setInteractionMode(int axis, InteractionModeEnum mode) + e.what(); return false; } + return true; } bool ControlBoardData::setControlMode(int j, int mode) { @@ -66,7 +67,7 @@ bool ControlBoardData::setControlMode(int j, int mode) { } // If joint is in hw fault, only a force idle command can recover it - if (this->actuatedAxes.at(j).controlMode == VOCAB_CM_HW_FAULT + if (this->actuatedAxes.at(j).commonJointProperties.controlMode == VOCAB_CM_HW_FAULT && mode != VOCAB_CM_FORCE_IDLE) { return true; @@ -85,7 +86,7 @@ bool ControlBoardData::setControlMode(int j, int mode) { this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); // If the joint is coupled, we have to change the interaction mode for all the coupled joints - if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) + if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), j) != coupledActuatedAxes.end()) { for (auto& actuatedAxis : coupledActuatedAxes) { @@ -98,16 +99,16 @@ bool ControlBoardData::setControlMode(int j, int mode) { } // if the joint is not coupled, we change the interaction mode only for the selected joint else { - this->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; - this->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; + this->actuatedAxes.at(j).commonJointProperties.controlMode = desired_mode; + this->physicalJoints.at(j).commonJointProperties.controlMode = desired_mode; } } // No coupling, we change the interaction mode only for the selected joint else { - this->physicalJoints.at(axis).commonJointProperties.controlMode = desired_mode; - this->actuatedAxes.at(axis).commonJointProperties.controlMode = desired_mode; + this->physicalJoints.at(j).commonJointProperties.controlMode = desired_mode; + this->actuatedAxes.at(j).commonJointProperties.controlMode = desired_mode; } - + return true; } -} // namespace yarp::dev::gzyarp +} // namespace gzyarp diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp index 32401aa8..715b90cf 100644 --- a/plugins/controlboard/src/ControlBoardDriver.cpp +++ b/plugins/controlboard/src/ControlBoardDriver.cpp @@ -273,7 +273,7 @@ bool ControlBoardDriver::getAxisName(int axis, std::string& name) { // TODO integrate with IJointCoupled interface - name = m_controlBoardData->actuatedAxes.at(axis).name; + name = m_controlBoardData->actuatedAxes.at(axis).commonJointProperties.name; return true; } @@ -634,7 +634,7 @@ bool ControlBoardDriver::setPosition(int j, double ref) return false; } - if (m_controlBoardData->actuatedAxes.at(j).controlMode != VOCAB_CM_POSITION_DIRECT) + if (m_controlBoardData->actuatedAxes.at(j).commonJointProperties.controlMode != VOCAB_CM_POSITION_DIRECT) { yError() << "Error while setting position: joint " + std::to_string(j) + " is not in position direct mode"; From 36cda99edf647129ed2ee8a892c0f3b1a88aefbf Mon Sep 17 00:00:00 2001 From: Nicogene Date: Thu, 12 Dec 2024 16:51:55 +0100 Subject: [PATCH 09/11] ControlBoard: fix trajectoryGenerator segfault --- plugins/controlboard/src/ControlBoard.cpp | 36 +++++++++++++---------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index c5a393ec..294aa42c 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -175,7 +175,7 @@ void ControlBoard::Configure(const Entity& _entity, yError() << "gz-sim-yarp-controlboard-system: failed setting joint properties"; return; } - configureBuffers(); + resetPositionsAndTrajectoryGenerators(_ecm); configureHelper.setConfigureIsSuccessful(true); @@ -264,6 +264,8 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm) yInfo() << "Joint " << jointFromConfigName << " added to the control board data."; } + // Let's initialize all the buffers/vectors + configureBuffers(); if (!initializeJointPositionLimits(_ecm)) { @@ -938,7 +940,6 @@ bool ControlBoard::parseInitialConfiguration(std::vector& initialConfigu void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponentManager& ecm) { std::lock_guard lock(m_controlBoardData.mutex); - std::vector initialConfigurations(m_controlBoardData.physicalJoints.size()); if (parseInitialConfiguration(initialConfigurations)) { @@ -958,9 +959,8 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen Joint(Model(m_modelEntity).JointByName(ecm, joint.commonJointProperties.name)) .ResetPosition(ecm, std::vector{gzPos}); } - - } else - { + } + else { yWarning() << "No initial configuration found, initializing trajectory generator with " "current values"; for (size_t i = 0; i < m_controlBoardData.physicalJoints.size(); i++) @@ -1026,31 +1026,37 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen } void ControlBoard::configureBuffers() { + size_t nrOfActuatedAxes{0}; + size_t nrOfPhysicalJoints{0}; if(m_controlBoardData.ijointcoupling) { - size_t nrOfActuatedAxes{0}; bool ok = m_controlBoardData.ijointcoupling->getNrOfActuatedAxes(nrOfActuatedAxes); if(!ok) { yError() << "Failed to get number of actuated axes"; return; } - m_controlBoardData.actuatedAxes.resize(nrOfActuatedAxes); - m_actuatedAxesPositionBuffer.resize(nrOfActuatedAxes); - m_actuatedAxesVelocityBuffer.resize(nrOfActuatedAxes); - m_actuatedAxesTorqueBuffer.resize(nrOfActuatedAxes); - size_t nrOfPhysicalJoints{0}; ok = m_controlBoardData.ijointcoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints); if(!ok) { yError() << "Failed to get number of physical joints"; return; } - m_controlBoardData.physicalJoints.resize(nrOfPhysicalJoints); - m_physicalJointsPositionBuffer.resize(nrOfPhysicalJoints); - m_physicalJointsVelocityBuffer.resize(nrOfPhysicalJoints); - m_physicalJointsTorqueBuffer.resize(nrOfPhysicalJoints); + + } + else { + nrOfPhysicalJoints = m_controlBoardData.physicalJoints.size(); + nrOfActuatedAxes = nrOfPhysicalJoints; } + m_controlBoardData.actuatedAxes.resize(nrOfActuatedAxes); + m_actuatedAxesPositionBuffer.resize(nrOfActuatedAxes); + m_actuatedAxesVelocityBuffer.resize(nrOfActuatedAxes); + m_actuatedAxesTorqueBuffer.resize(nrOfActuatedAxes); + m_controlBoardData.physicalJoints.resize(nrOfPhysicalJoints); + m_physicalJointsPositionBuffer.resize(nrOfPhysicalJoints); + m_physicalJointsVelocityBuffer.resize(nrOfPhysicalJoints); + m_physicalJointsTorqueBuffer.resize(nrOfPhysicalJoints); + return; } From 9381d0e84330242d10a2561d4540513fa3991d8e Mon Sep 17 00:00:00 2001 From: Nicogene Date: Fri, 13 Dec 2024 16:53:32 +0100 Subject: [PATCH 10/11] ControlBoard: fix limits and joint names for the actuated axes --- plugins/controlboard/src/ControlBoard.cpp | 24 +++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index 294aa42c..e1f6dfb9 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -267,6 +267,21 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm) // Let's initialize all the buffers/vectors configureBuffers(); + + for (size_t i=0; igetActuatedAxisName(i, m_controlBoardData.actuatedAxes[i].commonJointProperties.name); + if (!ok) { + yError() << "Error while getting actuated axis name"; + return false; + } + } + else { + m_controlBoardData.actuatedAxes[i].commonJointProperties.name = m_controlBoardData.physicalJoints[i].commonJointProperties.name; + } + } + if (!initializeJointPositionLimits(_ecm)) { yError() << "Error while setting joint position limits"; @@ -769,6 +784,15 @@ bool ControlBoard::initializeJointPositionLimits(const gz::sim::EntityComponentM } } + else { + // If no coupling is present, the actuated axes are the same as the physical joints + for (size_t i = 0; i < m_controlBoardData.actuatedAxes.size(); ++i) + { + auto& actuatedAxis = m_controlBoardData.actuatedAxes[i]; + actuatedAxis.commonJointProperties.positionLimitMin = m_controlBoardData.physicalJoints[i].commonJointProperties.positionLimitMin; + actuatedAxis.commonJointProperties.positionLimitMax = m_controlBoardData.physicalJoints[i].commonJointProperties.positionLimitMax; + } + } return true; } From 90d2bd36795e902ddbea25b02c0a0c195661d9b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Nicol=C3=B2=20Genesio=20=28=E4=BA=8C=E3=82=B3=E3=82=B2?= =?UTF-8?q?=E3=83=8D=29?= Date: Mon, 16 Dec 2024 14:43:54 +0100 Subject: [PATCH 11/11] Address review requests --- plugins/controlboard/include/ControlBoard.hh | 2 +- .../controlboard/include/ControlBoardData.hh | 9 +++-- plugins/controlboard/src/ControlBoard.cpp | 34 +++++++++++++------ plugins/controlboard/src/ControlBoardData.cpp | 18 ++++++---- 4 files changed, 41 insertions(+), 22 deletions(-) diff --git a/plugins/controlboard/include/ControlBoard.hh b/plugins/controlboard/include/ControlBoard.hh index f96815bc..f3568c43 100644 --- a/plugins/controlboard/include/ControlBoard.hh +++ b/plugins/controlboard/include/ControlBoard.hh @@ -105,7 +105,7 @@ private: bool initializeTrajectoryGeneratorReferences(yarp::os::Bottle& trajectoryGeneratorsGroup); bool parseInitialConfiguration(std::vector& initialConfigurations); void resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponentManager& ecm); - void configureBuffers(); + bool configureBuffers(); }; } // namespace gzyarp diff --git a/plugins/controlboard/include/ControlBoardData.hh b/plugins/controlboard/include/ControlBoardData.hh index 8ceb844c..91c55f72 100644 --- a/plugins/controlboard/include/ControlBoardData.hh +++ b/plugins/controlboard/include/ControlBoardData.hh @@ -62,7 +62,7 @@ struct PhysicalJointProperties std::string positionControlLaw; // TODO: verify usefulness of this field }; -struct ActuatedAxesProperties +struct ActuatedAxisProperties { CommonJointProperties commonJointProperties; std::unique_ptr trajectoryGenerator; @@ -77,14 +77,17 @@ class ControlBoardData public: std::mutex mutex; std::vector physicalJoints; - std::vector actuatedAxes; + std::vector actuatedAxes; yarp::os::Stamp simTime; yarp::dev::IJointCoupling* ijointcoupling{nullptr}; // TODO (xela95): read this value from configuration file std::chrono::milliseconds controlUpdatePeriod = std::chrono::milliseconds(1); - + + bool initCoupledJoints(); bool setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode); bool setControlMode(int j, int mode); +private: + yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; }; class IControlBoardData diff --git a/plugins/controlboard/src/ControlBoard.cpp b/plugins/controlboard/src/ControlBoard.cpp index e1f6dfb9..124bd474 100644 --- a/plugins/controlboard/src/ControlBoard.cpp +++ b/plugins/controlboard/src/ControlBoard.cpp @@ -134,18 +134,22 @@ void ControlBoard::Configure(const Entity& _entity, auto& coupling_group_bottle = m_pluginParameters.findGroup("COUPLING"); if (!coupling_group_bottle.isNull()) { - m_pluginParameters.unput("device"); + yarp::os::Property couplingParameters; + couplingParameters.fromString(m_pluginParameters.toString()); + couplingParameters.unput("device"); + // We need to remove the device name from the coupling parameters and set the one for the coupling driver + // The coupling driver needs some parameters stored in the controlboard configuration, then we copy it auto coupling_device_str = coupling_group_bottle.find("device").asString(); - m_pluginParameters.put("device", coupling_device_str); - if(!m_coupling_driver.open(m_pluginParameters)) + couplingParameters.put("device", coupling_device_str); + if(!m_coupling_driver.open(couplingParameters)) { yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in opening yarp " - "coupling driver"; + "coupling driver with device name " << coupling_device_str; return; } if(!m_coupling_driver.view(m_controlBoardData.ijointcoupling)) { yError() << "gz-sim-yarp-controlboard-system Plugin failed: error in getting " - "IJointCoupling interface"; + "IJointCoupling interface from device with name " << coupling_device_str; return; } @@ -264,8 +268,17 @@ bool ControlBoard::setJointProperties(EntityComponentManager& _ecm) yInfo() << "Joint " << jointFromConfigName << " added to the control board data."; } + if (!m_controlBoardData.initCoupledJoints()) + { + yError() << "Error while initializing coupled joints"; + return false; + } + // Let's initialize all the buffers/vectors - configureBuffers(); + if (!configureBuffers()) { + yError() << "Error while configuring buffers"; + return false; + } for (size_t i=0; icomputeTrajectory(); joint.isMotionDone = joint.trajectoryGenerator->isMotionDone(); @@ -1049,7 +1061,7 @@ void ControlBoard::resetPositionsAndTrajectoryGenerators(gz::sim::EntityComponen } } -void ControlBoard::configureBuffers() { +bool ControlBoard::configureBuffers() { size_t nrOfActuatedAxes{0}; size_t nrOfPhysicalJoints{0}; if(m_controlBoardData.ijointcoupling) @@ -1058,13 +1070,13 @@ void ControlBoard::configureBuffers() { if(!ok) { yError() << "Failed to get number of actuated axes"; - return; + return false; } ok = m_controlBoardData.ijointcoupling->getNrOfPhysicalJoints(nrOfPhysicalJoints); if(!ok) { yError() << "Failed to get number of physical joints"; - return; + return false; } } @@ -1080,7 +1092,7 @@ void ControlBoard::configureBuffers() { m_physicalJointsPositionBuffer.resize(nrOfPhysicalJoints); m_physicalJointsVelocityBuffer.resize(nrOfPhysicalJoints); m_physicalJointsTorqueBuffer.resize(nrOfPhysicalJoints); - return; + return true; } diff --git a/plugins/controlboard/src/ControlBoardData.cpp b/plugins/controlboard/src/ControlBoardData.cpp index 7d7e42b7..94cc6da3 100644 --- a/plugins/controlboard/src/ControlBoardData.cpp +++ b/plugins/controlboard/src/ControlBoardData.cpp @@ -1,18 +1,25 @@ -#include "ControlBoardData.hh" +#include namespace gzyarp { + + +bool ControlBoardData::initCoupledJoints() { + if (this->ijointcoupling) { + bool ok = this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); + ok = ok & this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); + return ok; + } + return true; +} bool ControlBoardData::setInteractionMode(int axis, yarp::dev::InteractionModeEnum mode) { - yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; try { // If there is coupling let's check if we have to change the interaction mode for all the coupled joints if (this->ijointcoupling) { - this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); - this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); // If the joint is coupled, we have to change the interaction mode for all the coupled joints if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), axis) != coupledActuatedAxes.end()) { @@ -79,12 +86,9 @@ bool ControlBoardData::setControlMode(int j, int mode) { desired_mode = VOCAB_CM_IDLE; } - yarp::sig::VectorOf coupledActuatedAxes, coupledPhysicalJoints; // If there is coupling let's check if we have to change the interaction mode for all the coupled joints if (this->ijointcoupling) { - this->ijointcoupling->getCoupledActuatedAxes(coupledActuatedAxes); - this->ijointcoupling->getCoupledPhysicalJoints(coupledPhysicalJoints); // If the joint is coupled, we have to change the interaction mode for all the coupled joints if(std::find(coupledActuatedAxes.begin(), coupledActuatedAxes.end(), j) != coupledActuatedAxes.end()) {