From 7709620d081da159b599ebbec0278ec816bcd8d3 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 26 Mar 2020 17:25:46 +0100 Subject: [PATCH] [squash] update APIs related to joint forces --- .../gazebo/include/scenario/gazebo/Joint.h | 16 +- .../gazebo/include/scenario/gazebo/Model.h | 10 +- .../{JointEffortLimit.h => MaxJointForce.h} | 0 cpp/scenario/gazebo/src/Joint.cpp | 282 ++++++++++-------- cpp/scenario/gazebo/src/Model.cpp | 43 +-- 5 files changed, 197 insertions(+), 154 deletions(-) rename cpp/scenario/gazebo/include/scenario/gazebo/components/{JointEffortLimit.h => MaxJointForce.h} (100%) diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h b/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h index 47287a0cd..dd24d9d0a 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h @@ -100,9 +100,6 @@ class scenario::gazebo::Joint double controllerPeriod() const; - double maxEffort() const; - bool setMaxEffort(const double maxEffort); - base::PID pid() const; bool setPID(const base::PID& pid); @@ -110,18 +107,21 @@ class scenario::gazebo::Joint // Single DOF methods // ================== - double force(const size_t dof = 0) const; + double maxGeneralizedForce(const size_t dof = 0) const; + bool setMaxGeneralizedForce(const double maxForce, const size_t dof = 0); + double position(const size_t dof = 0) const; double velocity(const size_t dof = 0) const; - bool setForce(const double force, const size_t dof = 0); bool setPositionTarget(const double position, const size_t dof = 0); bool setVelocityTarget(const double velocity, const size_t dof = 0); bool setAccelerationTarget(const double acceleration, const size_t dof = 0); + bool setGeneralizedForceTarget(const double force, const size_t dof = 0); double positionTarget(const size_t dof = 0) const; double velocityTarget(const size_t dof = 0) const; double accelerationTarget(const size_t dof = 0) const; + double generalizedForceTarget(const size_t dof = 0) const; bool resetPosition(const double position, const size_t dof = 0); bool resetVelocity(const double velocity, const size_t dof = 0); @@ -133,18 +133,20 @@ class scenario::gazebo::Joint // Multi DOF methods // ================= - std::vector jointForce() const; + std::vector jointMaxGeneralizedForce() const; + bool setJointMaxGeneralizedForce(const std::vector& maxForce); std::vector jointPosition() const; std::vector jointVelocity() const; - bool setJointForce(const std::vector& force); bool setJointPositionTarget(const std::vector& position); bool setJointVelocityTarget(const std::vector& velocity); bool setJointAccelerationTarget(const std::vector& acceleration); + bool setJointGeneralizedForceTarget(const std::vector& force); std::vector jointPositionTarget() const; std::vector jointVelocityTarget() const; std::vector jointAccelerationTarget() const; + std::vector jointGeneralizedForceTarget() const; bool resetJointPosition(const std::vector& position); bool resetJointVelocity(const std::vector& velocity); diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Model.h b/cpp/scenario/gazebo/include/scenario/gazebo/Model.h index b6b7f9985..ac70bf3ae 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/Model.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Model.h @@ -122,8 +122,6 @@ class scenario::gazebo::Model // Vectorized Methods // ================== - std::vector jointForces( // - const std::vector& jointNames = {}) const; std::vector jointPositions( // const std::vector& jointNames = {}) const; std::vector jointVelocities( // @@ -141,9 +139,6 @@ class scenario::gazebo::Model // Vectorized Target Methods // ========================= - bool setJointForces( // - const std::vector forces, - const std::vector& jointNames = {}); bool setJointPositionTargets( // const std::vector positions, const std::vector& jointNames = {}); @@ -153,6 +148,9 @@ class scenario::gazebo::Model bool setJointAccelerationTargets( // const std::vector velocities, const std::vector& jointNames = {}); + bool setJointGeneralizedForceTargets( // + const std::vector forces, + const std::vector& jointNames = {}); bool resetJointPositions( // const std::vector positions, @@ -167,6 +165,8 @@ class scenario::gazebo::Model const std::vector& jointNames = {}) const; std::vector jointAccelerationTargets( // const std::vector& jointNames = {}) const; + std::vector jointGeneralizedForceTargets( // + const std::vector& jointNames = {}) const; // ========= // Base Link diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointEffortLimit.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/MaxJointForce.h similarity index 100% rename from cpp/scenario/gazebo/include/scenario/gazebo/components/JointEffortLimit.h rename to cpp/scenario/gazebo/include/scenario/gazebo/components/MaxJointForce.h diff --git a/cpp/scenario/gazebo/src/Joint.cpp b/cpp/scenario/gazebo/src/Joint.cpp index 5685d8b3b..5dcfa2fc7 100644 --- a/cpp/scenario/gazebo/src/Joint.cpp +++ b/cpp/scenario/gazebo/src/Joint.cpp @@ -31,12 +31,12 @@ #include "scenario/gazebo/components/JointAccelerationTarget.h" #include "scenario/gazebo/components/JointControlMode.h" #include "scenario/gazebo/components/JointControllerPeriod.h" -#include "scenario/gazebo/components/JointEffortLimit.h" #include "scenario/gazebo/components/JointPID.h" #include "scenario/gazebo/components/JointPositionReset.h" #include "scenario/gazebo/components/JointPositionTarget.h" #include "scenario/gazebo/components/JointVelocityReset.h" #include "scenario/gazebo/components/JointVelocityTarget.h" +#include "scenario/gazebo/components/MaxJointForce.h" #include "scenario/gazebo/helpers.h" #include @@ -104,6 +104,9 @@ bool Joint::createECMResources() using namespace ignition::gazebo; const std::vector zero(this->dofs(), 0.0); + const std::vector infinity( // + this->dofs(), + std::numeric_limits::infinity()); // Create required components pImpl->ecm->CreateComponent(pImpl->jointEntity, @@ -117,9 +120,8 @@ bool Joint::createECMResources() pImpl->ecm->CreateComponent( pImpl->jointEntity, components::JointControlMode(base::JointControlMode::Idle)); - pImpl->ecm->CreateComponent( - pImpl->jointEntity, - components::JointEffortLimit(std::numeric_limits::infinity())); + pImpl->ecm->CreateComponent(pImpl->jointEntity, + components::MaxJointForce(infinity)); return true; } @@ -205,34 +207,6 @@ scenario::base::JointType Joint::type() const return type; } -scenario::base::Limit Joint::positionLimit() const -{ - base::Limit limit; - - switch (this->type()) { - case base::JointType::Revolute: - case base::JointType::Prismatic: { - sdf::JointAxis& axis = utils::getExistingComponentData< // - ignition::gazebo::components::JointAxis>(pImpl->ecm, - pImpl->jointEntity); - limit.min = axis.Lower(); - limit.max = axis.Upper(); - break; - } - case base::JointType::Fixed: - limit.min = 0; - limit.max = 0; - break; - case base::JointType::Invalid: - case base::JointType::Ball: - gymppWarning << "Type of Joint '" << this->name() - << "' has no limits" << std::endl; - break; - } - - return limit; -} - scenario::base::JointControlMode Joint::controlMode() const { base::JointControlMode jointControlMode = utils::getExistingComponentData< @@ -302,32 +276,6 @@ double Joint::controllerPeriod() const return utils::steadyClockDurationToDouble(duration); } -double Joint::maxEffort() const -{ - double jointEffortLimit = utils::getExistingComponentData< // - ignition::gazebo::components::JointEffortLimit>(pImpl->ecm, - pImpl->jointEntity); - - return jointEffortLimit; -} - -bool Joint::setMaxEffort(const double maxEffort) -{ - utils::setExistingComponentData< - ignition::gazebo::components::JointEffortLimit>( - pImpl->ecm, pImpl->jointEntity, maxEffort); - - // Get the PID - ignition::math::PID& pid = utils::getExistingComponentData< // - ignition::gazebo::components::JointPID>(pImpl->ecm, pImpl->jointEntity); - - // Update the PID - pid.SetCmdMax(maxEffort); - pid.SetCmdMin(-maxEffort); - - return true; -} - scenario::base::PID Joint::pid() const { const ignition::math::PID& pid = utils::getExistingComponentData< // @@ -354,22 +302,26 @@ bool Joint::setPID(const scenario::base::PID& pid) return equal; }; + if (this->dofs() > 1) { + gymppError << "Setting PIDs of joints with more than 1 DoF is not " + << "currently supported" << std::endl; + return false; + } + scenario::base::PID pidParams = pid; - auto minEffort = -this->maxEffort(); - auto maxEffort = this->maxEffort(); + auto minForce = -this->maxGeneralizedForce(0); + auto maxForce = this->maxGeneralizedForce(0); - if (pidParams.cmdMin > minEffort || pidParams.cmdMax < maxEffort) { - gymppWarning << "The output bounds of the PID are more limiting than " - << "the configured effort limit, they will be ignored" - << std::endl; - pidParams.cmdMin = minEffort; - pidParams.cmdMax = maxEffort; + if (pidParams.cmdMin < minForce || pidParams.cmdMax > maxForce) { + gymppWarning << "The output limits of the PID are less limiting than " + << "the maximum force that can be exerted on the joint. " + << "Ignoring the specified PID limits." << std::endl; + pidParams.cmdMin = minForce; + pidParams.cmdMax = maxForce; } // Create the new PID ignition::math::PID pidIgnitionMath = utils::toIgnitionPID(pidParams); - pidIgnitionMath.SetCmdMax(this->maxEffort()); - pidIgnitionMath.SetCmdMin(-this->maxEffort()); // Store the new PID in the ECM utils::setExistingComponentData force = this->jointForce(); - return force[dof]; -} + base::Limit limit; -double Joint::position(const size_t dof) const -{ - std::vector position = this->jointPosition(); - return position[dof]; + switch (this->type()) { + case base::JointType::Revolute: + case base::JointType::Prismatic: { + sdf::JointAxis& axis = utils::getExistingComponentData< // + ignition::gazebo::components::JointAxis>(pImpl->ecm, + pImpl->jointEntity); + limit.min = axis.Lower(); + limit.max = axis.Upper(); + break; + } + case base::JointType::Fixed: + limit.min = 0; + limit.max = 0; + break; + case base::JointType::Invalid: + case base::JointType::Ball: + gymppWarning << "Type of Joint '" << this->name() + << "' has no limits" << std::endl; + break; + } + + return limit; } -double Joint::velocity(const size_t dof) const +double Joint::maxGeneralizedForce(const size_t dof) const { - std::vector velocity = this->jointVelocity(); - return velocity[dof]; + std::vector maxForce = this->jointMaxGeneralizedForce(); + return maxForce[dof]; } -bool Joint::setForce(const double force, const size_t dof) +bool Joint::setMaxGeneralizedForce(const double maxForce, const size_t dof) { - if (this->controlMode() != base::JointControlMode::Force) { - gymppError << "The active joint control mode does not accept a force" - << std::endl; - return false; - } - if (dof >= this->dofs()) { gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof << std::endl; return false; } - auto& jointForce = utils::getComponentData< // - ignition::gazebo::components::JointForceCmd>(pImpl->ecm, + auto& maxJointForce = utils::getComponentData< // + ignition::gazebo::components::MaxJointForce>(pImpl->ecm, pImpl->jointEntity); - if (jointForce.size() != this->dofs()) { - assert(jointForce.size() == 0); - jointForce = std::vector(this->dofs(), 0.0); + if (maxJointForce.size() != this->dofs()) { + assert(maxJointForce.size() == 0); + maxJointForce = std::vector(this->dofs(), 0.0); } - double forceClipped = force; - double maxEffort = this->maxEffort(); + maxJointForce[dof] = maxForce; + return true; +} - forceClipped = std::min(forceClipped, maxEffort); - forceClipped = std::max(forceClipped, -maxEffort); +double Joint::position(const size_t dof) const +{ + std::vector position = this->jointPosition(); + return position[dof]; +} - jointForce[dof] = forceClipped; - return true; +double Joint::velocity(const size_t dof) const +{ + std::vector velocity = this->jointVelocity(); + return velocity[dof]; } bool Joint::setPositionTarget(const double position, const size_t dof) @@ -527,6 +495,39 @@ bool Joint::setAccelerationTarget(const double acceleration, const size_t dof) return true; } +bool Joint::setGeneralizedForceTarget(const double force, const size_t dof) +{ + if (this->controlMode() != base::JointControlMode::Force) { + gymppError << "The active joint control mode does not accept a force" + << std::endl; + return false; + } + + if (dof >= this->dofs()) { + gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; + return false; + } + + auto& jointForce = utils::getComponentData< // + ignition::gazebo::components::JointForceCmd>(pImpl->ecm, + pImpl->jointEntity); + + if (jointForce.size() != this->dofs()) { + assert(jointForce.size() == 0); + jointForce = std::vector(this->dofs(), 0.0); + } + + double forceClipped = force; + double maxForce = this->maxGeneralizedForce(dof); + + forceClipped = std::min(forceClipped, maxForce); + forceClipped = std::max(forceClipped, -maxForce); + + jointForce[dof] = forceClipped; + return true; +} + double Joint::positionTarget(const size_t dof) const { std::vector positionTarget = this->jointPositionTarget(); @@ -545,6 +546,12 @@ double Joint::accelerationTarget(const size_t dof) const return accelerationTarget[dof]; } +double Joint::generalizedForceTarget(const size_t dof) const +{ + std::vector force = this->jointGeneralizedForceTarget(); + return force[dof]; +} + bool Joint::resetPosition(const double position, size_t dof) { if (dof >= this->dofs()) { @@ -608,18 +615,29 @@ bool Joint::reset(const double position, const double velocity, size_t dof) return true; } -std::vector Joint::jointForce() const +std::vector Joint::jointMaxGeneralizedForce() const { - std::vector& jointForce = utils::getExistingComponentData< // - ignition::gazebo::components::JointForce>(pImpl->ecm, - pImpl->jointEntity); + std::vector& maxJointForce = utils::getExistingComponentData< // + ignition::gazebo::components::MaxJointForce>(pImpl->ecm, + pImpl->jointEntity); - if (jointForce.size() != this->dofs()) { - throw exceptions::DOFMismatch( - this->dofs(), jointForce.size(), this->name()); + return maxJointForce; +} + +bool Joint::setJointMaxGeneralizedForce(const std::vector& maxForce) +{ + if (maxForce.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; } - return jointForce; + auto& maxJointForce = utils::getComponentData< // + ignition::gazebo::components::MaxJointForce>(pImpl->ecm, + pImpl->jointEntity); + + maxJointForce = maxForce; + return true; } std::vector Joint::jointPosition() const @@ -650,22 +668,6 @@ std::vector Joint::jointVelocity() const return jointVelocity; } -bool Joint::setJointForce(const std::vector& force) -{ - if (force.size() != this->dofs()) { - gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() - << ")" << std::endl; - return false; - } - - auto& jointForce = utils::getComponentData< // - ignition::gazebo::components::JointForceCmd>(pImpl->ecm, - pImpl->jointEntity); - - jointForce = force; - return true; -} - bool Joint::setJointPositionTarget(const std::vector& position) { if (position.size() != this->dofs()) { @@ -714,6 +716,30 @@ bool Joint::setJointAccelerationTarget(const std::vector& acceleration) return true; } +bool Joint::setJointGeneralizedForceTarget(const std::vector& force) +{ + if (force.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; + } + + auto& jointForceTarget = utils::getComponentData< // + ignition::gazebo::components::JointForceCmd>(pImpl->ecm, + pImpl->jointEntity); + + std::vector clippedForce = std::move(force); + const std::vector maxForce = this->jointMaxGeneralizedForce(); + + for (size_t dof = 0; dof < this->dofs(); ++dof) { + clippedForce[dof] = std::min(clippedForce[dof], maxForce[dof]); + clippedForce[dof] = std::max(clippedForce[dof], -maxForce[dof]); + } + + jointForceTarget = std::move(clippedForce); + return true; +} + std::vector Joint::jointPositionTarget() const { std::vector& jointPositionTarget = utils::getExistingComponentData< @@ -756,6 +782,20 @@ std::vector Joint::jointAccelerationTarget() const return jointAccelTarget; } +std::vector Joint::jointGeneralizedForceTarget() const +{ + std::vector& jointForceTarget = utils::getExistingComponentData< // + ignition::gazebo::components::JointForce>(pImpl->ecm, + pImpl->jointEntity); + + if (jointForceTarget.size() != this->dofs()) { + throw exceptions::DOFMismatch( + this->dofs(), jointForceTarget.size(), this->name()); + } + + return jointForceTarget; +} + bool Joint::resetJointPosition(const std::vector& position) { if (position.size() != this->dofs()) { diff --git a/cpp/scenario/gazebo/src/Model.cpp b/cpp/scenario/gazebo/src/Model.cpp index cacac9db3..238ea76c4 100644 --- a/cpp/scenario/gazebo/src/Model.cpp +++ b/cpp/scenario/gazebo/src/Model.cpp @@ -494,16 +494,6 @@ Model::contacts(const std::vector& linkNames) const return allContacts; } -std::vector -Model::jointForces(const std::vector& jointNames) const -{ - auto lambda = [](JointPtr joint, const size_t dof) -> double { - return joint->force(dof); - }; - - return Impl::getJointDataSerialized(this, jointNames, lambda); -} - std::vector Model::jointPositions(const std::vector& jointNames) const { @@ -569,17 +559,6 @@ Model::joints(const std::vector& jointNames) const return joints; } -bool Model::setJointForces(const std::vector forces, - const std::vector& jointNames) -{ - auto lambda = - [](JointPtr joint, const double force, const size_t dof) -> bool { - return joint->setForce(force, dof); - }; - - return Impl::setJointDataSerialized(this, forces, jointNames, lambda); -} - bool Model::setJointPositionTargets(const std::vector positions, const std::vector& jointNames) { @@ -614,6 +593,18 @@ bool Model::setJointAccelerationTargets( return Impl::setJointDataSerialized(this, velocities, jointNames, lambda); } +bool Model::setJointGeneralizedForceTargets( + const std::vector forces, + const std::vector& jointNames) +{ + auto lambda = + [](JointPtr joint, const double force, const size_t dof) -> bool { + return joint->setGeneralizedForceTarget(force, dof); + }; + + return Impl::setJointDataSerialized(this, forces, jointNames, lambda); +} + bool Model::resetJointPositions(const std::vector positions, const std::vector& jointNames) { @@ -666,6 +657,16 @@ std::vector Model::jointAccelerationTargets( return Impl::getJointDataSerialized(this, jointNames, lambda); } +std::vector Model::jointGeneralizedForceTargets( + const std::vector& jointNames) const +{ + auto lambda = [](JointPtr joint, const size_t dof) -> double { + return joint->generalizedForceTarget(dof); + }; + + return Impl::getJointDataSerialized(this, jointNames, lambda); +} + std::string Model::baseFrame() const { // Get all the canonical links of the model