-
Notifications
You must be signed in to change notification settings - Fork 26
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #166 from diegoferigo/refactor/joint_controller_pl…
…ugin New JointController plugin to implement Position and Velocity joint control modes
- Loading branch information
Showing
5 changed files
with
288 additions
and
181 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
271 changes: 271 additions & 0 deletions
271
cpp/scenario/plugins/JointController/JointController.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,271 @@ | ||
| /* | ||
| * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) | ||
| * All rights reserved. | ||
| * | ||
| * This project is dual licensed under LGPL v2.1+ or Apachi License. | ||
| * | ||
| * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * | ||
| * | ||
| * This software may be modified and distributed under the terms of the | ||
| * GNU Lesser General Public License v2.1 or any later version. | ||
| * | ||
| * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * | ||
| * | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| */ | ||
|
|
||
| #include "JointController.h" | ||
| #include "scenario/gazebo/Joint.h" | ||
| #include "scenario/gazebo/Log.h" | ||
| #include "scenario/gazebo/Model.h" | ||
| #include "scenario/gazebo/components/JointControlMode.h" | ||
| #include "scenario/gazebo/components/JointPID.h" | ||
| #include "scenario/gazebo/components/JointPositionTarget.h" | ||
| #include "scenario/gazebo/components/JointVelocityTarget.h" | ||
| #include "scenario/gazebo/helpers.h" | ||
|
|
||
| #include <ignition/gazebo/Entity.hh> | ||
| #include <ignition/gazebo/components/Joint.hh> | ||
| #include <ignition/gazebo/components/JointForceCmd.hh> | ||
| #include <ignition/gazebo/components/JointPosition.hh> | ||
| #include <ignition/gazebo/components/JointType.hh> | ||
| #include <ignition/gazebo/components/JointVelocity.hh> | ||
| #include <ignition/gazebo/components/Model.hh> | ||
| #include <ignition/gazebo/components/Name.hh> | ||
| #include <ignition/gazebo/components/ParentEntity.hh> | ||
| #include <ignition/plugin/Register.hh> | ||
| #include <sdf/Joint.hh> | ||
|
|
||
| #include <cassert> | ||
| #include <chrono> | ||
| #include <ostream> | ||
| #include <string> | ||
|
|
||
| using namespace scenario::gazebo; | ||
| using namespace scenario::plugins::gazebo; | ||
|
|
||
| class JointController::Impl | ||
| { | ||
| public: | ||
| scenario::gazebo::Model model; | ||
| ignition::gazebo::Entity modelEntity; | ||
| std::chrono::steady_clock::duration prevUpdateTime{0}; | ||
|
|
||
| static bool runPIDController(scenario::gazebo::Joint& joint, | ||
| const bool computeNewForce, | ||
| ignition::math::PID& pid, | ||
| const std::chrono::steady_clock::duration& dt, | ||
| const std::vector<double>& reference, | ||
| const std::vector<double>& current); | ||
| }; | ||
|
|
||
| JointController::JointController() | ||
| : System() | ||
| , pImpl{std::make_unique<Impl>()} | ||
| {} | ||
|
|
||
| JointController::~JointController() = default; | ||
|
|
||
| void JointController::Configure( | ||
| const ignition::gazebo::Entity& entity, | ||
| const std::shared_ptr<const sdf::Element>& /*sdf*/, | ||
| ignition::gazebo::EntityComponentManager& ecm, | ||
| ignition::gazebo::EventManager& eventMgr) | ||
| { | ||
| // Store the model entity | ||
| pImpl->modelEntity = entity; | ||
|
|
||
| // Create a model and check its validity | ||
| pImpl->model.initialize(entity, &ecm, &eventMgr); | ||
|
|
||
| if (!pImpl->model.valid()) { | ||
| gymppError << "Failed to create a model from Entity [" << entity << "]" | ||
| << std::endl; | ||
| return; | ||
| } | ||
| } | ||
|
|
||
| void JointController::PreUpdate(const ignition::gazebo::UpdateInfo& info, | ||
| ignition::gazebo::EntityComponentManager& ecm) | ||
| { | ||
| if (info.paused) { | ||
| return; | ||
| } | ||
|
|
||
| using namespace std::chrono; | ||
|
|
||
| // Update the controller only if enough time is passed | ||
| duration<double> elapsedFromLastUpdate = | ||
| info.simTime - pImpl->prevUpdateTime; | ||
| assert(elapsedFromLastUpdate.count() > 0); | ||
|
|
||
| // Handle first iteration | ||
| if (pImpl->prevUpdateTime.count() == 0) { | ||
| elapsedFromLastUpdate = | ||
| duration<double>(pImpl->model.controllerPeriod()); | ||
| } | ||
|
|
||
| // If enough time has passed, store the time of this actuation step. In this | ||
| // case the state of the robot is read and new force references are computed | ||
| // and actuated. Otherwise, the same force of the last step is actuated. | ||
| bool computeNewForce; | ||
|
|
||
| // Due to numerical floating point approximations, sometimes a comparison of | ||
| // chrono durations has an error in the 1e-18 order | ||
| auto greaterThan = [](const duration<double>& a, | ||
| const duration<double>& b) -> bool { | ||
| return a.count() >= b.count() - std::numeric_limits<double>::epsilon(); | ||
| }; | ||
|
|
||
| if (greaterThan(elapsedFromLastUpdate, | ||
| duration<double>(pImpl->model.controllerPeriod()))) { | ||
| // Store the current update time | ||
| pImpl->prevUpdateTime = info.simTime; | ||
|
|
||
| // Enable using the PID to compute the new force | ||
| computeNewForce = true; | ||
| } | ||
| else { | ||
| // Disable the PID and send the same force reference as last update | ||
| computeNewForce = false; | ||
| } | ||
|
|
||
| auto positionControlledJoints = ecm.EntitiesByComponents( | ||
| ignition::gazebo::components::Joint(), | ||
| ignition::gazebo::components::ParentEntity(pImpl->modelEntity), | ||
| ignition::gazebo::components::JointControlMode( | ||
| base::JointControlMode::Position)); | ||
|
|
||
| auto positionInterpolatedControlledJoints = ecm.EntitiesByComponents( | ||
| ignition::gazebo::components::Joint(), | ||
| ignition::gazebo::components::ParentEntity(pImpl->modelEntity), | ||
| ignition::gazebo::components::JointControlMode( | ||
| base::JointControlMode::PositionInterpolated)); | ||
|
|
||
| auto velocityControlledJoints = ecm.EntitiesByComponents( | ||
| ignition::gazebo::components::Joint(), | ||
| ignition::gazebo::components::ParentEntity(pImpl->modelEntity), | ||
| ignition::gazebo::components::JointControlMode( | ||
| base::JointControlMode::Velocity)); | ||
|
|
||
| // Update PIDs for Revolute and Prismatic joints controlled in Position | ||
| for (const auto jointEntity : positionControlledJoints) { | ||
|
|
||
| ignition::math::PID& pid = utils::getExistingComponentData< // | ||
| ignition::gazebo::components::JointPID>(&ecm, jointEntity); | ||
|
|
||
| std::string& jointName = utils::getExistingComponentData< // | ||
| ignition::gazebo::components::Name>(&ecm, jointEntity); | ||
|
|
||
| auto joint = pImpl->model.getJoint(jointName); | ||
| const std::vector<double>& position = joint->jointPosition(); | ||
|
|
||
| std::vector<double>& positionTarget = utils::getExistingComponentData< | ||
| ignition::gazebo::components::JointPositionTarget>(&ecm, | ||
| jointEntity); | ||
|
|
||
| if (!Impl::runPIDController(*joint, | ||
| computeNewForce, | ||
| pid, | ||
| info.dt, | ||
| positionTarget, | ||
| position)) { | ||
| gymppError << "Failed to run PID controller of joint " | ||
| << joint->name() << " [" << jointEntity << "]" | ||
| << std::endl; | ||
| } | ||
| } | ||
|
|
||
| // TODO: Update PIDs for Revolute and Prismatic joints controlled in | ||
| // PositionInterpolated | ||
|
|
||
| // Update PIDs for Revolute and Prismatic joints controlled in Velocity | ||
| for (const auto jointEntity : velocityControlledJoints) { | ||
|
|
||
| ignition::math::PID& pid = utils::getExistingComponentData< | ||
| ignition::gazebo::components::JointPID>(&ecm, jointEntity); | ||
|
|
||
| std::string& jointName = | ||
| utils::getExistingComponentData<ignition::gazebo::components::Name>( | ||
| &ecm, jointEntity); | ||
|
|
||
| auto joint = pImpl->model.getJoint(jointName); | ||
| const std::vector<double>& velocity = joint->jointVelocity(); | ||
|
|
||
| std::vector<double>& velocityTarget = utils::getExistingComponentData< | ||
| ignition::gazebo::components::JointVelocityTarget>(&ecm, | ||
| jointEntity); | ||
|
|
||
| if (!Impl::runPIDController(*joint, | ||
| computeNewForce, | ||
| pid, | ||
| info.dt, | ||
| velocityTarget, | ||
| velocity)) { | ||
| gymppError << "Failed to run PID controller of joint " | ||
| << joint->name() << " [" << jointEntity << "]" | ||
| << std::endl; | ||
| } | ||
| } | ||
| } | ||
|
|
||
| bool JointController::Impl::runPIDController( | ||
| scenario::gazebo::Joint& joint, | ||
| const bool computeNewForce, | ||
| ignition::math::PID& pid, | ||
| const std::chrono::steady_clock::duration& dt, | ||
| const std::vector<double>& reference, | ||
| const std::vector<double>& current) | ||
| { | ||
| switch (joint.type()) { | ||
|
|
||
| case base::JointType::Revolute: | ||
| case base::JointType::Prismatic: { | ||
|
|
||
| double force; | ||
|
|
||
| if (computeNewForce) { | ||
| gymppWarning << "updating" << std::endl; | ||
| assert(current.size() == 1); | ||
| assert(reference.size() == 1); | ||
|
|
||
| double error = current[0] - reference[0]; | ||
| force = pid.Update(error, dt); | ||
| } | ||
| else { | ||
| force = pid.Cmd(); | ||
| } | ||
|
|
||
| if (!joint.setForce(force)) { | ||
| gymppError << "Failed to set force of joint " << joint.name() | ||
| << std::endl; | ||
| return false; | ||
| } | ||
| return true; | ||
| } | ||
| case base::JointType::Fixed: | ||
| case base::JointType::Ball: | ||
| case base::JointType::Invalid: | ||
| gymppWarning << "Type of joint '" << joint.name() | ||
| << " not supported" << std::endl; | ||
| return true; | ||
| } | ||
|
|
||
| return false; | ||
| }; | ||
|
|
||
| IGNITION_ADD_PLUGIN( | ||
| scenario::plugins::gazebo::JointController, | ||
| scenario::plugins::gazebo::JointController::System, | ||
| scenario::plugins::gazebo::JointController::ISystemConfigure, | ||
| scenario::plugins::gazebo::JointController::ISystemPreUpdate) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.