diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md deleted file mode 100644 index 3d89f64143..0000000000 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ /dev/null @@ -1,29 +0,0 @@ ---- -name: Bug report -about: Report a bug -labels: bug ---- - - - -## Environment -* OS Version: -* Source or binary build? - - - -## Description -* Expected behavior: -* Actual behavior: - -## Steps to reproduce - - -1. -2. -3. - -## Output - diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md deleted file mode 100644 index f49727a0ce..0000000000 --- a/.github/ISSUE_TEMPLATE/feature_request.md +++ /dev/null @@ -1,23 +0,0 @@ ---- -name: Feature request -about: Request a new feature -labels: enhancement ---- - - - -## Desired behavior - - -## Alternatives considered - - -## Implementation suggestion - - -## Additional context - diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md deleted file mode 100644 index 6c88bd5465..0000000000 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ /dev/null @@ -1,52 +0,0 @@ - - -# Bug Report - -Fixes issue # - -## Summary - - -## Checklist -- [ ] Signed all commits for DCO -- [ ] Added tests -- [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] `codecheck` passed (See - [contributing](https://ignitionrobotics.org/docs/all/contributing#contributing-code)) -- [ ] All tests passed (See - [test coverage](https://ignitionrobotics.org/docs/all/contributing#test-coverage)) -- [ ] While waiting for a review on your PR, please help review -[another open pull request](https://github.com/pulls?q=is%3Aopen+is%3Apr+user%3Aignitionrobotics+archived%3Afalse+) -to support the maintainers - -**Note to maintainers**: Remember to use **Squash-Merge** - ---- - -# New feature - -Closes # - -## Summary - - -## Test it - - -## Checklist -- [ ] Signed all commits for DCO -- [ ] Added tests -- [ ] Added example world and/or tutorial -- [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] `codecheck` passed (See [contributing](https://ignitionrobotics.org/docs/all/contributing#contributing-code)) -- [ ] All tests passed (See [test coverage](https://ignitionrobotics.org/docs/all/contributing#test-coverage)) -- [ ] While waiting for a review on your PR, please help review -[another open pull request](https://github.com/pulls?q=is%3Aopen+is%3Apr+user%3Aignitionrobotics+archived%3Afalse+) -to support the maintainers - -**Note to maintainers**: Remember to use **Squash-Merge** diff --git a/.github/PULL_REQUEST_TEMPLATE/port.md b/.github/PULL_REQUEST_TEMPLATE/port.md deleted file mode 100644 index 758127f585..0000000000 --- a/.github/PULL_REQUEST_TEMPLATE/port.md +++ /dev/null @@ -1,6 +0,0 @@ -Port to - -Branch comparison: https://github.com/ignitionrobotics/ign-gazebo/compare/... - -**Note to maintainers**: Remember to **Merge** with commit (not squash-merge -or rebase) diff --git a/CMakeLists.txt b/CMakeLists.txt index f11b8106de..b616eadfec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport9_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs6 REQUIRED) +ign_find_package(ignition-msgs6 REQUIRED VERSION 6.2) set(IGN_MSGS_VER ${ignition-msgs6_VERSION_MAJOR}) #-------------------------------------- diff --git a/examples/worlds/joint_trajectory_controller.sdf b/examples/worlds/joint_trajectory_controller.sdf new file mode 100644 index 0000000000..29b68f69a5 --- /dev/null +++ b/examples/worlds/joint_trajectory_controller.sdf @@ -0,0 +1,722 @@ + + + + + + + + + + + + + 0.4 0.4 0.4 + false + + + + + + + + + + 3D View + false + docked + + ogre + scene + 1 0 0 0 0 3.1416 + + + + + + World control + false + false + 50 + 100 + 1 + floating + + + + + + true + true + true + + + + + + World stats + false + false + 250 + 110 + 1 + floating + + + + + + true + true + true + true + + + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + -0.1 0 0 0 1.5708 0 + true + + + + + 0 0 1 + 5 5 + + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + + + + + 0 0 0.25 -2.3561945 0 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + RR_position_control_joint1 + 0.7854 + 20 + 0.4 + 1.0 + -1 + 1 + -20 + 20 + + RR_position_control_joint2 + -1.5708 + 10 + 0.2 + 0.5 + -1 + 1 + -10 + 10 + + + + + + + 0 -0.5 -0.25 0 0 0 + + + world + RR_velocity_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_velocity_control_link0 + RR_velocity_control_link1 + + 1 0 0 + + + 0.02 + + + + 0 0 0.1 0 0 0 + RR_velocity_control_link1 + RR_velocity_control_link2 + + 1 0 0 + + + 0.01 + + + + + + 0.6 + 175 + -10 + 10 + + 0.1 + 200 + -5 + 5 + + + + + + + 0 0.5 -0.25 -0.7854 0 0 + + + world + RR_effort_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0 0.5 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0 0.5 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_effort_control_link0 + RR_effort_control_link1 + + 1 0 0 + + 0.02 + + + + + 0 0 0.1 0 0 0 + RR_effort_control_link1 + RR_effort_control_link2 + + 1 0 0 + + 0.01 + + + + + + + custom_topic_effort_control + + + + + diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 6f88a441fe..1925efa8a5 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -79,7 +79,16 @@ void GuiRunner::RequestState() } reqSrv = reqSrvValid; - this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this); + auto advertised = this->node.AdvertisedServices(); + if (std::find(advertised.begin(), advertised.end(), reqSrv) == + advertised.end()) + { + if (!this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this)) + { + ignerr << "Failed to advertise [" << reqSrv << "]" << std::endl; + } + } + ignition::msgs::StringMsg req; req.set_data(reqSrv); @@ -98,7 +107,7 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) return; } - plugin->Update(this->updateInfo, this->ecm); + this->RequestState(); } ///////////////////////////////////////////////// diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index d8b61a4056..07b88dfba4 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -86,6 +86,7 @@ add_subdirectory(imu) add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) +add_subdirectory(joint_trajectory_controller) add_subdirectory(kinetic_energy_monitor) add_subdirectory(lift_drag) add_subdirectory(log) diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index cd35d55805..9e78162403 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -142,7 +142,16 @@ void JointController::Configure(const Entity &_entity, } if (_sdf->HasElement("topic")) { - topic = _sdf->Get("topic"); + topic = transport::TopicUtils::AsValidTopic( + _sdf->Get("topic")); + + if (topic.empty()) + { + ignerr << "Failed to create topic [" << this->dataPtr->jointName + << "]" << " for joint [" << _sdf->Get("topic") + << "]" << std::endl; + return; + } } this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, this->dataPtr.get()); diff --git a/src/systems/joint_trajectory_controller/CMakeLists.txt b/src/systems/joint_trajectory_controller/CMakeLists.txt new file mode 100644 index 0000000000..c4150b372e --- /dev/null +++ b/src/systems/joint_trajectory_controller/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(joint-trajectory-controller + SOURCES + JointTrajectoryController.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/joint_trajectory_controller/JointTrajectoryController.cc b/src/systems/joint_trajectory_controller/JointTrajectoryController.cc new file mode 100644 index 0000000000..d7cb345e60 --- /dev/null +++ b/src/systems/joint_trajectory_controller/JointTrajectoryController.cc @@ -0,0 +1,1067 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "JointTrajectoryController.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Helper class that contains all parameters required to create and +/// configure an instance of ActuatedJoint +class JointParameters +{ + /// \brief Parse all parameters required for creation of ActuatedJoint and + /// return them in a map + /// \param[in] _sdf SDF reference used to obtain the parameters + /// \param[in] _ecm Ignition Entity Component Manager + /// \param[in] _enabledJoints List of joint entities that are enabled and + /// need to be created + /// \return Map of parameters for each joint, the first entry of pair + /// indicates the joint name + public: static std::map ParseAll( + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + std::vector _enabledJoints); + + /// \brief Parse all values of a single parameter that is specified multiple + /// times in SDF + /// \param[in] _sdf SDF reference used to obtain the parameters + /// \param[in] _parameterName Name of the repeated parameter to parse all + /// values for + /// \return Ordered list of all values for a given repeated parameter + public: template static std::vector Parse( + const std::shared_ptr &_sdf, + const std::string &_parameterName); + + /// \brief Return value from `_vec` at `_index`, or `_alternative_value` if + /// `_vec` is not large enough + /// \param[in] _vec Vector that contains desired value of type T + /// \param[in] _index Index at which the value is stored + /// \param[in] _alternative_value Alternative or default value of type T + /// \return Value from `_vec` at `_index`, or `_alternative_value` if `_vec` + /// is not large enough + public: template static T NthElementOr( + const std::vector &_vec, + const size_t &_index, + const T &_alternative_value); + + /// \brief Initial position of the joint + public: double initialPosition; + /// \brief Default value for initial position of the joint + public: static constexpr double initialPositionDefault = 0.0; + + /// \brief Parameters required for creation of new PID controller + public: struct PID + { + /// \brief Proportional gain + double pGain; + /// \brief Default value for proportional gain + static constexpr double pGainDefault = 0.0; + + /// \brief Integral gain + double iGain; + /// \brief Default value for integral gain + static constexpr double iGainDefault = 0.0; + + /// \brief Derivative gain + double dGain; + /// \brief Default value for derivative gain + static constexpr double dGainDefault = 0.0; + + /// \brief Integral lower limit + double iMin; + /// \brief Default value for integral lower limit + static constexpr double iMinDefault = 0.0; + + /// \brief Integral upper limit + double iMax; + /// \brief Default value for integral upper limit + static constexpr double iMaxDefault = -1.0; + + /// \brief Output min value + double cmdMin; + /// \brief Default value for output min value + static constexpr double cmdMinDefault = 0.0; + + /// \brief Output max value + double cmdMax; + /// \brief Default value for output max value + static constexpr double cmdMaxDefault = -1.0; + + /// \brief Output offset + double cmdOffset; + /// \brief Default value for output offset + static constexpr double cmdOffsetDefault = 0.0; + } positionPID, velocityPID; +}; + +/// \brief A single 1-axis joint that is controlled by JointTrajectoryController +/// plugin +class ActuatedJoint +{ + /// \brief Default contructor + public: ActuatedJoint() = default; + + /// \brief Constructor that properly configures the actuated joint + /// \param[in] _entity Entity of the joint + /// \param[in] _params All parameters of the joint required for its + /// configuration + public: ActuatedJoint(const Entity &_entity, + const JointParameters &_params); + + /// \brief Setup components required for control of this joint + /// \param[in,out] _ecm Ignition Entity Component Manager + public: void SetupComponents( + ignition::gazebo::EntityComponentManager &_ecm) const; + + /// \brief Set target of the joint that the controller will attempt to reach + /// \param[in] _targetPoint Targets of all controlled joint + /// \param[in] _jointIndex Index of the joint, used to determine what index + /// of `_targetPoint` to use + public: void SetTarget( + const ignition::msgs::JointTrajectoryPoint &_targetPoint, + const size_t &_jointIndex); + + /// \brief Update command force that is applied on the joint + /// \param[in,out] _ecm Ignition Entity Component Manager + /// \param[in] _dt Time difference to update for + public: void Update(ignition::gazebo::EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_dt); + + /// \brief Reset the target of the joint + public: void ResetTarget(); + + /// \brief Reset the position and velocity PID error on the joint + public: void ResetPIDs(); + + /// \brief Entity of the joint + public: Entity entity; + + /// \brief Target state that the joint controller should reach + public: struct TargetState + { + /// \brief Target position of the joint + double position; + /// \brief Target position of the joint + double velocity; + /// \brief Target acceleration of the joint + /// \attention Acceleration control is NOT implemented at the moment + double acceleration; + /// \brief Target force or torque of the joint + double effort; + } target; + + /// \brief Initial position of the joint + public: double initialPosition; + + /// \brief List of PID controllers used for the control of this actuated joint + public: struct PIDs + { + /// \brief Position PID controller + ignition::math::PID position; + /// \brief Velocity PID controller + ignition::math::PID velocity; + } pids; +}; + +/// \brief Information about trajectory that is followed by +/// JointTrajectoryController plugin +class Trajectory +{ + /// \brief Update index of trajectory points, such that it directs to a point + /// that needs to be currently followed + /// \param[in] _simTime Current simulation time + /// \return True if index of the trajectory point was updated, False otherwise + public: bool UpdateCurrentPoint( + const std::chrono::steady_clock::duration &_simTime); + + /// \brief Determine if the trajectory goal was reached + /// \return True if trajectory goal was reached, False otherwise + public: bool IsGoalReached() const; + + /// \brief Compute progress of the current trajectory + /// \return Fraction of the completed points in range (0.0, 1.0] + public: float ComputeProgress() const; + + /// \brief Reset trajectory internals, i.e. clean list of joint names, points + /// and reset index of the current point + public: void Reset(); + + /// \brief Status of the trajectory + public: enum TrajectoryStatus + { + /// \brief Trajectory is new and needs to be configure on the next update + /// loop + New, + /// \brief Trajectory is currently being followed + Active, + /// \brief Trajectory goal is reached + Reached, + } status; + + /// \brief Start time of trajectory + public: std::chrono::steady_clock::duration startTime; + + /// \brief Index of the current trajectory point + public: unsigned int pointIndex; + + /// \brief Ordered joints that need to be actuated to follow the current + /// trajectory + public: std::vector jointNames; + + /// \brief Trajectory defined in terms of temporal points, whose members are + /// ordered according to `jointNames` + public: std::vector points; +}; + +/// \brief Private data of the JointTrajectoryController plugin +class ignition::gazebo::systems::JointTrajectoryControllerPrivate +{ + /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no + /// joint names are specified in the plugin configuration, all valid 1-axis + /// joints are returned + /// \param[in] _entity Entity of the model that the plugin is being + /// configured for + /// \param[in] _sdf SDF reference used to determine enabled joints + /// \param[in] _ecm Ignition Entity Component Manager + /// \return List of entities containinig all enabled joints + public: std::vector GetEnabledJoints( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm) const; + + /// \brief Callback for joint trajectory subscription + /// \param[in] _msg A new message describing a joint trajectory that needs + /// to be followed + public: void JointTrajectoryCallback( + const ignition::msgs::JointTrajectory &_msg); + + /// \brief Reset internals of the plugin, without affecting already created + /// components + public: void Reset(); + + /// \brief Ignition communication node + public: transport::Node node; + + /// \brief Publisher of the progress for currently followed trajectory + public: transport::Node::Publisher progressPub; + + /// \brief Map of actuated joints, where the first entry of pair is the name + /// of the joint + public: std::map actuatedJoints; + + /// \brief Mutex projecting trajectory + public: std::mutex trajectoryMutex; + + /// \brief Information about trajectory that should be followed + public: Trajectory trajectory; + + /// \brief Flag that determines whether to use message header timestamp as + /// the trajectory start, where simulation time at the beginning of execution + /// is used otherwise + public: bool useHeaderStartTime; + + /// \brief Flag that determines if all components required for control are + /// already setup + public: bool componentSetupFinished; +}; + +///////////////////////////////// +/// JointTrajectoryController /// +///////////////////////////////// + +////////////////////////////////////////////////// +JointTrajectoryController::JointTrajectoryController() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void JointTrajectoryController::Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager & /*_eventManager*/) +{ + // Make sure the controller is attached to a valid model + const auto model = Model(_entity); + if (!model.Valid(_ecm)) + { + ignerr << "[JointTrajectoryController] Failed to initialize because [" + << model.Name(_ecm) << "(Entity=" << _entity + << ")] is not a model. Please make sure that" + " JointTrajectoryController is attached to a valid model.\n"; + return; + } + ignmsg << "[JointTrajectoryController] Setting up controller for [" + << model.Name(_ecm) << "(Entity=" << _entity << ")].\n"; + + // Get list of enabled joints + const auto enabledJoints = this->dataPtr->GetEnabledJoints(_entity, + _sdf, + _ecm); + + // For each enabled joint, parse all of its parameters from SDF + auto jointParameters = JointParameters::ParseAll(_sdf, _ecm, enabledJoints); + + // Iterate over all enabled joints and create/configure them + for (const auto &jointEntity : enabledJoints) + { + const auto jointName = + _ecm.Component(jointEntity)->Data(); + this->dataPtr->actuatedJoints[jointName] = + ActuatedJoint(jointEntity, jointParameters[jointName]); + ignmsg << "[JointTrajectoryController] Configured joint [" + << jointName << "(Entity=" << jointEntity << ")].\n"; + } + + // Make sure at least one joint is configured + if (this->dataPtr->actuatedJoints.empty()) + { + ignerr << "[JointTrajectoryController] Failed to initialize because [" + << model.Name(_ecm) << "(Entity=" << _entity + << ")] has no supported joints.\n"; + return; + } + + // Get additional parameters from SDF + if (_sdf->HasAttribute("use_header_start_time")) + { + this->dataPtr->useHeaderStartTime = + _sdf->Get("use_header_start_time"); + } + else + { + this->dataPtr->useHeaderStartTime = false; + } + + // Subscribe to joint trajectory commands + auto trajectoryTopic = _sdf->Get("topic"); + if (trajectoryTopic.empty()) + { + // If not specified, use the default topic based on model name + trajectoryTopic = "/model/" + model.Name(_ecm) + "/joint_trajectory"; + } + // Make sure the topic is valid + const auto validTrajectoryTopic = transport::TopicUtils::AsValidTopic( + trajectoryTopic); + if (validTrajectoryTopic.empty()) + { + ignerr << "[JointTrajectoryController] Cannot subscribe to invalid topic [" + << trajectoryTopic << "].\n"; + return; + } + // Subscribe + ignmsg << "[JointTrajectoryController] Subscribing to joint trajectory" + " commands on topic [" << validTrajectoryTopic << "].\n"; + this->dataPtr->node.Subscribe( + validTrajectoryTopic, + &JointTrajectoryControllerPrivate::JointTrajectoryCallback, + this->dataPtr.get()); + + // Advertise progress + const auto progressTopic = validTrajectoryTopic + "_progress"; + ignmsg << "[JointTrajectoryController] Advertising joint trajectory progress" + " on topic [" << progressTopic << "].\n"; + this->dataPtr->progressPub = + this->dataPtr->node.Advertise(progressTopic); +} + +////////////////////////////////////////////////// +void JointTrajectoryController::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("JointTrajectoryController::PreUpdate"); + + // Create required components for each joint (only once) + if (!this->dataPtr->componentSetupFinished) + { + for (auto &actuatedJoint : this->dataPtr->actuatedJoints) + { + ActuatedJoint *joint = &actuatedJoint.second; + joint->SetupComponents(_ecm); + } + this->dataPtr->componentSetupFinished = true; + } + + // Reset plugin if jump back in time is detected + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignmsg << "[JointTrajectoryController] Resetting plugin because jump back" + " in time [" + << std::chrono::duration_cast(_info.dt).count() + << " s] was detected.\n"; + this->dataPtr->Reset(); + } + + // Nothing else to do if paused + if (_info.paused) + { + return; + } + + // Update joint targets based on the current trajectory + { + auto isTargetUpdateRequired = false; + + // Lock mutex before accessing trajectory + std::lock_guard lock(this->dataPtr->trajectoryMutex); + + if (this->dataPtr->trajectory.status == Trajectory::New) + { + // Set trajectory start time if not set before + if (this->dataPtr->trajectory.startTime.count() == 0) + { + this->dataPtr->trajectory.startTime = _info.simTime; + this->dataPtr->trajectory.status = Trajectory::Active; + } + + // If the new trajectory has no points, consider it reached + if (this->dataPtr->trajectory.points.empty()) + { + this->dataPtr->trajectory.status = Trajectory::Reached; + } + + // Update is always needed for a new trajectory + isTargetUpdateRequired = true; + } + + if (this->dataPtr->trajectory.status == Trajectory::Active) + { + // Determine what point needs to be reached at the current time + if (this->dataPtr->trajectory.UpdateCurrentPoint(_info.simTime)) + { + // Update is needed if point was updated + isTargetUpdateRequired = true; + } + } + + // Update the target for each joint that is defined in the + // trajectory, if needed + if (isTargetUpdateRequired && + this->dataPtr->trajectory.status != Trajectory::Reached) + { + const auto targetPoint = + this->dataPtr->trajectory.points[this->dataPtr->trajectory + .pointIndex]; + for (auto jointIndex = 0u; + jointIndex < this->dataPtr->trajectory.jointNames.size(); + ++jointIndex) + { + const auto jointName = this->dataPtr->trajectory.jointNames[jointIndex]; + if (this->dataPtr->actuatedJoints.count(jointName) == 0) + { + // Warning about unconfigured joint is already logged above + continue; + } + auto *joint = &this->dataPtr->actuatedJoints[jointName]; + joint->SetTarget(targetPoint, jointIndex); + } + + // If there are no more points after the current one, set the trajectory + // to Reached + if (this->dataPtr->trajectory.IsGoalReached()) + { + this->dataPtr->trajectory.status = Trajectory::Reached; + } + + // Publish current progress of the trajectory + ignition::msgs::Float progressMsg; + progressMsg.set_data(this->dataPtr->trajectory.ComputeProgress()); + this->dataPtr->progressPub.Publish(progressMsg); + } + } + + // Control loop + for (auto &actuatedJoint : this->dataPtr->actuatedJoints) + { + auto *joint = &actuatedJoint.second; + joint->Update(_ecm, _info.dt); + } +} + +//////////////////////////////////////// +/// JointTrajectoryControllerPrivate /// +//////////////////////////////////////// + +////////////////////////////////////////////////// +std::vector JointTrajectoryControllerPrivate::GetEnabledJoints( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm) const +{ + std::vector output; + + // Get list of user-enabled joint names. If empty, enable all 1-axis joints + const auto enabledJoints = JointParameters::Parse(_sdf, + "joint_name"); + + // Get list of joint entities of the model + // If there are joints explicitely enabled by the user, get only those + std::vector jointEntities; + if (!enabledJoints.empty()) + { + for (const auto &enabledJointName : enabledJoints) + { + auto enabledJointEntity = _ecm.ChildrenByComponents( + _entity, components::Joint(), components::Name(enabledJointName)); + // Check that model has exactly one joint that matches the name + if (enabledJointEntity.empty()) + { + ignerr << "[JointTrajectoryController] Model does not contain joint [" + << enabledJointName << "], which was explicitly enabled.\n"; + continue; + } + else if (enabledJointEntity.size() > 1) + { + ignwarn << "[JointTrajectoryController] Model has " + << enabledJointEntity.size() << " duplicate joints named [" + << enabledJointName << "]. Only the first (Entity=" + << enabledJointEntity[0] << ") will be configured.\n"; + } + // Add entity to the list of enabled joints + jointEntities.push_back(enabledJointEntity[0]); + } + } + else + { + jointEntities = _ecm.ChildrenByComponents(_entity, components::Joint()); + } + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto &jointEntity : jointEntities) + { + const auto jointName = _ecm.Component( + jointEntity)->Data(); + + // Ignore duplicate joints + for (const auto &actuatedJoint : this->actuatedJoints) + { + if (actuatedJoint.second.entity == jointEntity) + { + ignwarn << "[JointTrajectoryController] Ignoring duplicate joint [" + << jointName << "(Entity=" << jointEntity << ")].\n"; + continue; + } + } + + // Make sure the joint type is supported, i.e. it has exactly one + // actuated axis + const auto *jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) + { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + { + // Supported joint type + break; + } + case sdf::JointType::FIXED: + { + igndbg << "[JointTrajectoryController] Fixed joint [" << jointName + << "(Entity=" << jointEntity << ")] is skipped.\n"; + continue; + } + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: + { + ignwarn << "[JointTrajectoryController] Joint [" << jointName + << "(Entity=" << jointEntity + << ")] is of unsupported type. Only joints with a single axis" + " are supported.\n"; + continue; + } + default: + { + ignwarn << "[JointTrajectoryController] Joint [" << jointName + << "(Entity=" << jointEntity << ")] is of unknown type.\n"; + continue; + } + } + output.push_back(jointEntity); + } + + return output; +} + +////////////////////////////////////////////////// +void JointTrajectoryControllerPrivate::JointTrajectoryCallback( + const ignition::msgs::JointTrajectory &_msg) +{ + // Make sure the message is valid + if (_msg.joint_names_size() == 0) + { + ignwarn << "[JointTrajectoryController] JointTrajectory message does not" + " contain any joint names.\n"; + return; + } + + // Warn user that accelerations are currently ignored if the first point + // contains them + if (_msg.points(0).accelerations_size() > 0) + { + ignwarn << "[JointTrajectoryController] JointTrajectory message contains" + " acceleration commands, which are currently ignored.\n"; + } + + // Lock mutex guarding the trajectory + std::lock_guard lock(this->trajectoryMutex); + + if (this->trajectory.status != Trajectory::Reached) + { + ignwarn << "[JointTrajectoryController] A new JointTrajectory message was" + " received while executing a previous trajectory.\n"; + } + + // Get start time of the trajectory from message header if desired + // If not enabled or there is no header, set start time to 0 and determine + // it later from simTime + if (this->useHeaderStartTime && _msg.has_header()) + { + if (_msg.header().has_stamp()) + { + const auto stamp = _msg.header().stamp(); + this->trajectory.startTime = std::chrono::seconds(stamp.sec()) + + std::chrono::nanoseconds(stamp.nsec()); + } + } + else + { + this->trajectory.startTime = std::chrono::nanoseconds(0); + } + + // Reset for a new trajectory + this->trajectory.Reset(); + + // Extract joint names and points + for (const auto &joint_name : _msg.joint_names()) + { + this->trajectory.jointNames.push_back(joint_name); + } + for (const auto &point : _msg.points()) + { + this->trajectory.points.push_back(point); + } +} + +////////////////////////////////////////////////// +void JointTrajectoryControllerPrivate::Reset() +{ + for (auto &actuatedJoint : this->actuatedJoints) + { + auto *joint = &actuatedJoint.second; + // Reset joint target + joint->ResetTarget(); + // Reset PIDs + joint->ResetPIDs(); + } + + // Reset trajectory + this->trajectory.Reset(); +} + +/////////////////////// +/// JointParameters /// +/////////////////////// + +////////////////////////////////////////////////// +std::map JointParameters::ParseAll( + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + std::vector _enabledJoints) +{ + std::map output; + + const auto initialPositionAll = JointParameters::Parse(_sdf, + "initial_position"); + + const auto positionPGainAll = JointParameters::Parse(_sdf, + "position_p_gain"); + const auto positionIGainAll = JointParameters::Parse(_sdf, + "position_i_gain"); + const auto positionDGainAll = JointParameters::Parse(_sdf, + "position_d_gain"); + const auto positionIMinAll = JointParameters::Parse(_sdf, + "position_i_min"); + const auto positionIMaxAll = JointParameters::Parse(_sdf, + "position_i_max"); + const auto positionCmdMinAll = JointParameters::Parse(_sdf, + "position_cmd_min"); + const auto positionCmdMaxAll = JointParameters::Parse(_sdf, + "position_cmd_max"); + const auto positionCmdOffsetAll = JointParameters::Parse(_sdf, + "position_cmd_offset"); + + const auto velocityPGainAll = JointParameters::Parse(_sdf, + "velocity_p_gain"); + const auto velocityIGainAll = JointParameters::Parse(_sdf, + "velocity_i_gain"); + const auto velocityDGainAll = JointParameters::Parse(_sdf, + "velocity_d_gain"); + const auto velocityIMinAll = JointParameters::Parse(_sdf, + "velocity_i_min"); + const auto velocityIMaxAll = JointParameters::Parse(_sdf, + "velocity_i_max"); + const auto velocityCmdMinAll = JointParameters::Parse(_sdf, + "velocity_cmd_min"); + const auto velocityCmdMaxAll = JointParameters::Parse(_sdf, + "velocity_cmd_max"); + const auto velocityCmdOffsetAll = JointParameters::Parse(_sdf, + "velocity_cmd_offset"); + + for (std::size_t i = 0; i < _enabledJoints.size(); ++i) + { + JointParameters params; + params.initialPosition = params.NthElementOr(initialPositionAll, i, + params.initialPositionDefault); + + params.positionPID.pGain = params.NthElementOr(positionPGainAll, i, + params.positionPID.pGainDefault); + params.positionPID.iGain = params.NthElementOr(positionIGainAll, i, + params.positionPID.iGainDefault); + params.positionPID.dGain = params.NthElementOr(positionDGainAll, i, + params.positionPID.dGainDefault); + params.positionPID.iMin = params.NthElementOr(positionIMinAll, i, + params.positionPID.iMinDefault); + params.positionPID.iMax = params.NthElementOr(positionIMaxAll, i, + params.positionPID.iMaxDefault); + params.positionPID.cmdMin = params.NthElementOr(positionCmdMinAll, i, + params.positionPID.cmdMinDefault); + params.positionPID.cmdMax = params.NthElementOr(positionCmdMaxAll, i, + params.positionPID.cmdMaxDefault); + params.positionPID.cmdOffset = params.NthElementOr(positionCmdOffsetAll, i, + params.positionPID.cmdOffsetDefault); + + params.velocityPID.pGain = params.NthElementOr(velocityPGainAll, i, + params.velocityPID.pGainDefault); + params.velocityPID.iGain = params.NthElementOr(velocityIGainAll, i, + params.velocityPID.iGainDefault); + params.velocityPID.dGain = params.NthElementOr(velocityDGainAll, i, + params.velocityPID.dGainDefault); + params.velocityPID.iMin = params.NthElementOr(velocityIMinAll, i, + params.velocityPID.iMinDefault); + params.velocityPID.iMax = params.NthElementOr(velocityIMaxAll, i, + params.velocityPID.iMaxDefault); + params.velocityPID.cmdMin = params.NthElementOr(velocityCmdMinAll, i, + params.velocityPID.cmdMinDefault); + params.velocityPID.cmdMax = params.NthElementOr(velocityCmdMaxAll, i, + params.velocityPID.cmdMaxDefault); + params.velocityPID.cmdOffset = params.NthElementOr(velocityCmdOffsetAll, i, + params.velocityPID.cmdOffsetDefault); + + const auto jointName = _ecm.Component( + _enabledJoints[i])->Data(); + output[jointName] = params; + } + + return output; +} + +////////////////////////////////////////////////// +template +std::vector JointParameters::Parse( + const std::shared_ptr &_sdf, + const std::string &_parameterName) +{ + std::vector output; + + if (_sdf->HasElement(_parameterName)) + { + sdf::ElementPtr param = const_cast( + _sdf.get())->GetElement(_parameterName); + while (param) + { + output.push_back(param->Get()); + param = param->GetNextElement(_parameterName); + } + } + + return output; +} + +//////////////////////////////////////////////// +template +T JointParameters::NthElementOr(const std::vector &_vec, + const size_t &_index, + const T &_alternative_value) +{ + if (_index < _vec.size()) + { + return _vec[_index]; + } + else + { + return _alternative_value; + } +} + +///////////////////// +/// ActuatedJoint /// +///////////////////// + +////////////////////////////////////////////////// +ActuatedJoint::ActuatedJoint(const Entity &_entity, + const JointParameters &_params) +{ + this->entity = _entity; + + this->initialPosition = _params.initialPosition; + this->target.position = _params.initialPosition; + this->target.velocity = 0.0; + this->target.acceleration = 0.0; + this->target.effort = 0.0; + + this->pids.position = ignition::math::PID(_params.positionPID.pGain, + _params.positionPID.iGain, + _params.positionPID.dGain, + _params.positionPID.iMax, + _params.positionPID.iMin, + _params.positionPID.cmdMax, + _params.positionPID.cmdMin, + _params.positionPID.cmdOffset); + + this->pids.velocity = ignition::math::PID(_params.velocityPID.pGain, + _params.velocityPID.iGain, + _params.velocityPID.dGain, + _params.velocityPID.iMax, + _params.velocityPID.iMin, + _params.velocityPID.cmdMax, + _params.velocityPID.cmdMin, + _params.velocityPID.cmdOffset); + + igndbg << "[JointTrajectoryController] Parameters for joint (Entity=" + << _entity << "):\n" + << "initial_position: [" << _params.initialPosition << "]\n" + << "position_p_gain: [" << _params.positionPID.pGain << "]\n" + << "position_i_gain: [" << _params.positionPID.iGain << "]\n" + << "position_d_gain: [" << _params.positionPID.dGain << "]\n" + << "position_i_min: [" << _params.positionPID.iMax << "]\n" + << "position_i_max: [" << _params.positionPID.iMax << "]\n" + << "position_cmd_min: [" << _params.positionPID.cmdMin << "]\n" + << "position_cmd_max: [" << _params.positionPID.cmdMax << "]\n" + << "position_cmd_offset: [" << _params.positionPID.cmdOffset << "]\n" + << "velocity_p_gain: [" << _params.velocityPID.pGain << "]\n" + << "velocity_i_gain: [" << _params.velocityPID.iGain << "]\n" + << "velocity_d_gain: [" << _params.velocityPID.dGain << "]\n" + << "velocity_i_min: [" << _params.velocityPID.iMax << "]\n" + << "velocity_i_max: [" << _params.velocityPID.iMax << "]\n" + << "velocity_cmd_min: [" << _params.velocityPID.cmdMin << "]\n" + << "velocity_cmd_max: [" << _params.velocityPID.cmdMax << "]\n" + << "velocity_cmd_offset: [" << _params.velocityPID.cmdOffset << "]\n"; +} + +////////////////////////////////////////////////// +void ActuatedJoint::SetupComponents( + ignition::gazebo::EntityComponentManager &_ecm) const +{ + // Create JointPosition component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointPosition()); + } + + // Create JointVelocity component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointVelocity()); + } + + // Create JointForceCmd component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointForceCmd({0.0})); + } +} + +////////////////////////////////////////////////// +void ActuatedJoint::SetTarget( + const ignition::msgs::JointTrajectoryPoint &_targetPoint, + const size_t &_jointIndex) +{ + if ((signed)_jointIndex < _targetPoint.positions_size()) + { + this->target.position = _targetPoint.positions(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.velocities_size()) + { + this->target.velocity = _targetPoint.velocities(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.accelerations_size()) + { + this->target.acceleration = _targetPoint.accelerations(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.effort_size()) + { + this->target.effort = _targetPoint.effort(_jointIndex); + } +} + +////////////////////////////////////////////////// +void ActuatedJoint::Update(ignition::gazebo::EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_dt) +{ + // Get JointPosition and JointVelocity components + const auto jointPositionComponent = _ecm.Component( + this->entity); + const auto jointVelocityComponent = _ecm.Component( + this->entity); + + // Compute control errors and force for each PID controller + double forcePosition = 0.0, forceVelocity = 0.0; + if (!jointPositionComponent->Data().empty()) + { + double errorPosition = jointPositionComponent->Data()[0] - + this->target.position; + forcePosition = this->pids.position.Update(errorPosition, _dt); + } + if (!jointVelocityComponent->Data().empty()) + { + double errorVelocity = jointVelocityComponent->Data()[0] - + this->target.velocity; + forceVelocity = this->pids.velocity.Update(errorVelocity, _dt); + } + + // Sum all forces + const double force = forcePosition + forceVelocity + this->target.effort; + + // Get JointForceCmd component and apply command force + auto jointForceCmdComponent = _ecm.Component( + this->entity); + jointForceCmdComponent->Data()[0] = force; +} + +////////////////////////////////////////////////// +void ActuatedJoint::ResetTarget() +{ + this->target.position = this->initialPosition; + this->target.velocity = 0.0; + this->target.acceleration = 0.0; + this->target.effort = 0.0; +} + +////////////////////////////////////////////////// +void ActuatedJoint::ResetPIDs() +{ + this->pids.position.Reset(); + this->pids.velocity.Reset(); +} + +////////////////// +/// Trajectory /// +////////////////// + +////////////////////////////////////////////////// +bool Trajectory::UpdateCurrentPoint( + const std::chrono::steady_clock::duration &_simTime) +{ + bool isUpdated = false; + + const auto trajectoryTime = _simTime - this->startTime; + while (true) + { + // Break if end of trajectory is reached (there are no more points after + // the current one) + if (this->IsGoalReached()) + { + break; + } + + // Break if point needs to be followed + const auto pointTFS = this->points[this->pointIndex].time_from_start(); + const auto pointTime = std::chrono::seconds(pointTFS.sec()) + + std::chrono::nanoseconds(pointTFS.nsec()); + if (pointTime >= trajectoryTime) + { + break; + } + + // Otherwise increment and try again (joint targets need to be updated) + ++this->pointIndex; + isUpdated = true; + }; + + // Return true if a point index was updated + return isUpdated; +} + +////////////////////////////////////////////////// +bool Trajectory::IsGoalReached() const +{ + return this->pointIndex + 1 >= this->points.size(); +} + +////////////////////////////////////////////////// +float Trajectory::ComputeProgress() const +{ + if (this->points.size() == 0) + { + return 1.0; + } + else + { + return static_cast(this->pointIndex + 1) / + static_cast(this->points.size()); + } +} + +////////////////////////////////////////////////// +void Trajectory::Reset() +{ + this->status = Trajectory::New; + this->pointIndex = 0; + this->jointNames.clear(); + this->points.clear(); +} + +// Register plugin +IGNITION_ADD_PLUGIN(JointTrajectoryController, + ignition::gazebo::System, + JointTrajectoryController::ISystemConfigure, + JointTrajectoryController::ISystemPreUpdate) +IGNITION_ADD_PLUGIN_ALIAS( + JointTrajectoryController, + "ignition::gazebo::systems::JointTrajectoryController") diff --git a/src/systems/joint_trajectory_controller/JointTrajectoryController.hh b/src/systems/joint_trajectory_controller/JointTrajectoryController.hh new file mode 100644 index 0000000000..8f895b77e0 --- /dev/null +++ b/src/systems/joint_trajectory_controller/JointTrajectoryController.hh @@ -0,0 +1,164 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_JOINT_TRAJECTORY_CONTROLLER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_JOINT_TRAJECTORY_CONTROLLER_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class IGNITION_GAZEBO_HIDDEN JointTrajectoryControllerPrivate; + + /// \brief Joint trajectory controller, which can be attached to a model with + /// reference to one or more 1-axis joints in order to follow a trajectory. + /// + /// Joint trajectories can be sent to this plugin via Ignition Transport. + /// The default topic name is "/model/${MODEL_NAME}/joint_trajectory" that + /// can be configured with the `` system parameter. + /// + /// This topic accepts ignition::msgs::JointTrajectory messages that represent + /// a full trajectory, defined as temporal `points` with their fields ordered + /// according to `joint_names` field. The fields under `points` are + /// `positions` - Controlled by position PID controller for each joint + /// `velocities` - Controlled by velocity PID controller for each joint + /// `accelerations` - This field is currently ignored + /// `effort` - Controlled directly for each joint + /// `time_from_start` - Temporal information about the point + /// + /// Forces/torques from `position`, `velocity` and `effort` are summed and + /// applied to the joint. Each PID controller can be configured with system + /// parameters described below. + /// + /// Input trajectory can be produced by a motion planning framework such as + /// MoveIt2. For smooth execution of the trajectory, its points should to be + /// interpolated before sending them via Ignition Transport (interpolation + /// might already be implemented in the motion planning framework of your + /// choice). + /// + /// The progress of the current trajectory can be tracked on topic whose name + /// is derived as `_progress`. This progress is indicated in the range + /// of (0.0, 1.0] and is currently based purely on `time_from_start` contained + /// in the trajectory points. + /// + /// ## System Parameters + /// + /// `` The name of the topic that this plugin subscribes to. + /// Optional parameter. + /// Defaults to "/model/${MODEL_NAME}/joint_trajectory". + /// + /// `` If enabled, trajectory execution begins at the + /// timestamp contained in the header of received trajectory messages. + /// Optional parameter. + /// Defaults to false. + /// + /// `` Name of a joint to control. + /// This parameter can be specified multiple times, i.e. once for each joint. + /// Optional parameter. + /// Defaults to all 1-axis joints contained in the model SDF (order is kept). + /// + /// `` Initial position of a joint (for position control). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// Defaults to 0 for all joints. + /// + /// `<%s_p_gain>` The proportional gain of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_i_gain>` The integral gain of the PID. Optional parameter. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_d_gain>` The derivative gain of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_i_min>` The integral lower limit of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no limit if higher than `%s_i_max`). + /// + /// `<%s_i_max>` The integral upper limit of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is -1 (no limit if lower than `%s_i_min`). + /// + /// `<%s_cmd_min>` Output min value of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no limit if higher than `%s_i_max`). + /// + /// `<%s_cmd_max>` Output max value of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is -1 (no limit if lower than `%s_i_min`). + /// + /// `<%s_cmd_offset>` Command offset (feed-forward) of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no offset). + class IGNITION_GAZEBO_VISIBLE JointTrajectoryController + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: JointTrajectoryController(); + + /// \brief Destructor + public: ~JointTrajectoryController() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 5e2233fbd0..c306ec83bc 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -22,6 +22,7 @@ set(tests joint_controller_system.cc joint_position_controller_system.cc joint_state_publisher_system.cc + joint_trajectory_controller_system.cc kinetic_energy_monitor_system.cc lift_drag_system.cc level_manager.cc diff --git a/test/integration/joint_trajectory_controller_system.cc b/test/integration/joint_trajectory_controller_system.cc new file mode 100644 index 0000000000..70ca0aad3f --- /dev/null +++ b/test/integration/joint_trajectory_controller_system.cc @@ -0,0 +1,399 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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 + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointVelocity.hh" +#include "ignition/gazebo/components/Name.hh" + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +#define TOL 1e-4 + +using namespace ignition; +using namespace gazebo; + +/// \brief Test fixture for JointTrajectoryController system +class JointTrajectoryControllerTestFixture : public ::testing::Test +{ + // Documentation inherited +protected: + void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } +}; + +///////////////////////////////////////////////// +// Tests that JointTrajectoryController accepts position-controlled joint +// trajectory +TEST_F(JointTrajectoryControllerTestFixture, + JointTrajectoryControllerPositionControl) +{ + using namespace std::chrono_literals; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_trajectory_controller.sdf"; + + // Define joints of the model to investigate + const size_t kNumberOfJoints = 2; + const std::string jointNames[kNumberOfJoints] = {"RR_position_control_joint1", + "RR_position_control_joint2"}; + + // Define names of Ignition Transport topics + const std::string trajectoryTopic = + "/model/RR_position_control/joint_trajectory"; + const std::string feedbackTopic = + "/model/RR_position_control/joint_trajectory_feedback"; + + // Define initial joint positions + const std::array initialPositions = {0.0, 0.0}; + + // Define a trajectory to follow + msgs::Duration timeFromStart; + std::vector trajectoryTimes; + std::vector> trajectoryPositions; + // Point1 + timeFromStart.set_sec(0); + timeFromStart.set_nsec(666000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({-0.7854, 0.7854}); + // Point2 + timeFromStart.set_sec(1); + timeFromStart.set_nsec(333000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({1.5708, -0.7854}); + // Point3 + timeFromStart.set_sec(2); + timeFromStart.set_nsec(0); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({3.1416, -1.5708}); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Setup test system + test::Relay testSystem; + double currentPositions[kNumberOfJoints]; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + // Create a JointPosition component for each joint if it doesn't exist + for (const auto &jointName : jointNames) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointName)); + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + } + }); + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get the current position of each joint + for (std::size_t i = 0; i < kNumberOfJoints; ++i) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointNames[i])); + const auto jointPositionComponent = + _ecm.Component(joint); + if (nullptr != jointPositionComponent) + { + currentPositions[i] = jointPositionComponent->Data()[0]; + } + } + }); + server.AddSystem(testSystem.systemPtr); + + // Step few iterations and assert that the initial position is kept + const std::size_t initIters = 10; + server.Run(true, initIters, false); + for (size_t i = 0; i < kNumberOfJoints; ++i) + { + EXPECT_NEAR(currentPositions[i], initialPositions[i], TOL); + } + + // Create new JointTrajectory message based on the defined trajectory + ignition::msgs::JointTrajectory msg; + for (const auto &jointName : jointNames) + { + msg.add_joint_names(jointName); + } + for (size_t i = 0; i < trajectoryPositions.size(); ++i) + { + ignition::msgs::JointTrajectoryPoint point; + + // Set the temporal information for the point + auto time = point.mutable_time_from_start(); + time->set_sec(trajectoryTimes[i].sec()); + time->set_nsec(trajectoryTimes[i].nsec()); + + // Add target positions to the point + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + point.add_positions(trajectoryPositions[i][j]); + } + + // Add point to the trajectory + msg.add_points(); + msg.mutable_points(i)->CopyFrom(point); + } + + // Verify that feedback is strictly increasing and reaches value of 1.0 + size_t count = 0; + std::function feedbackCallback = + [&](const ignition::msgs::Float &_msg) { + count++; + if (trajectoryPositions.size() == count) + { + EXPECT_FLOAT_EQ(_msg.data(), 1.0f); + } + else + { + EXPECT_FLOAT_EQ(_msg.data(), static_cast(count) / + static_cast(trajectoryPositions.size())); + } + }; + transport::Node node; + node.Subscribe(feedbackTopic, feedbackCallback); + + // Publish joint trajectory + auto pub = node.Advertise(trajectoryTopic); + pub.Publish(msg); + + // Wait for message to be published + std::this_thread::sleep_for(100ms); + + // Run the simulation while asserting the target position of all joints at + // each trajectory point + auto previousIterFromStart = 0; + for (size_t i = 0; i < trajectoryPositions.size(); ++i) + { + // Number of iters required to reach time_from_start of the current + // point (1ms step size) + auto iterFromStart = trajectoryTimes[i].sec() * 1000 + + trajectoryTimes[i].nsec() / 1000000; + auto neededIters = iterFromStart - previousIterFromStart; + + // Run the simulation + server.Run(true, neededIters, false); + + // Assert that each joint reached its target position + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + EXPECT_NEAR(currentPositions[j], trajectoryPositions[i][j], TOL); + } + + // Keep track of how many iterations have already passed + previousIterFromStart = iterFromStart; + } +} + +///////////////////////////////////////////////// +// Tests that JointTrajectoryController accepts velocity-controlled joint +// trajectory +TEST_F(JointTrajectoryControllerTestFixture, + JointTrajectoryControllerVelocityControl) +{ + using namespace std::chrono_literals; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_trajectory_controller.sdf"; + + // Define joints of the model to investigate + const size_t kNumberOfJoints = 2; + const std::string jointNames[kNumberOfJoints] = {"RR_velocity_control_joint1", + "RR_velocity_control_joint2"}; + + // Define names of Ignition Transport topics + const std::string trajectoryTopic = "/test_custom_topic/velocity_control"; + const std::string feedbackTopic = + "/test_custom_topic/velocity_control_feedback"; + + // Define initial joint velocities + const std::array initialVelocities = {0.0, 0.0}; + + // Define a trajectory to follow + msgs::Duration timeFromStart; + std::vector trajectoryTimes; + std::vector> trajectoryVelocities; + // Point1 + timeFromStart.set_sec(0); + timeFromStart.set_nsec(500000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryVelocities.push_back({0.5, 0.5}); + // Point2 + timeFromStart.set_sec(1); + timeFromStart.set_nsec(0); + trajectoryTimes.push_back(timeFromStart); + trajectoryVelocities.push_back({-1.0, 1.0}); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Setup test system + test::Relay testSystem; + double currentVelocities[kNumberOfJoints]; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + // Create a JointVelocity component for each joint if it doesn't exist + for (const auto &jointName : jointNames) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointName)); + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointVelocity()); + } + } + }); + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get the current velocity of each joint + for (std::size_t i = 0; i < kNumberOfJoints; ++i) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointNames[i])); + const auto jointVelocityComponent = + _ecm.Component(joint); + if (nullptr != jointVelocityComponent) + { + currentVelocities[i] = jointVelocityComponent->Data()[0]; + } + } + }); + server.AddSystem(testSystem.systemPtr); + + // Step few iterations and assert that the initial velocity is kept + const std::size_t initIters = 10; + server.Run(true, initIters, false); + for (size_t i = 0; i < kNumberOfJoints; ++i) + { + EXPECT_NEAR(currentVelocities[i], initialVelocities[i], TOL); + } + + // Create new JointTrajectory message based on the defined trajectory + ignition::msgs::JointTrajectory msg; + for (const auto &jointName : jointNames) + { + msg.add_joint_names(jointName); + } + for (size_t i = 0; i < trajectoryVelocities.size(); ++i) + { + ignition::msgs::JointTrajectoryPoint point; + + // Set the temporal information for the point + auto time = point.mutable_time_from_start(); + time->set_sec(trajectoryTimes[i].sec()); + time->set_nsec(trajectoryTimes[i].nsec()); + + // Add target velocities to the point + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + point.add_velocities(trajectoryVelocities[i][j]); + } + + // Add point to the trajectory + msg.add_points(); + msg.mutable_points(i)->CopyFrom(point); + } + + // Verify that feedback is strictly increasing and reaches value of 1.0 + size_t count = 0; + std::function feedbackCallback = + [&](const ignition::msgs::Float &_msg) { + count++; + if (trajectoryVelocities.size() == count) + { + EXPECT_FLOAT_EQ(_msg.data(), 1.0f); + } + else + { + EXPECT_FLOAT_EQ(_msg.data(), static_cast(count) / + static_cast(trajectoryVelocities.size())); + } + }; + transport::Node node; + node.Subscribe(feedbackTopic, feedbackCallback); + + // Publish joint trajectory + auto pub = node.Advertise(trajectoryTopic); + pub.Publish(msg); + + // Wait for message to be published + std::this_thread::sleep_for(100ms); + + // Run the simulation while asserting the target velocity of all joints at + // each trajectory point + auto previousIterFromStart = 0; + for (size_t i = 0; i < trajectoryVelocities.size(); ++i) + { + // Number of iters required to reach time_from_start of the current + // point (1ms step size) + auto iterFromStart = trajectoryTimes[i].sec() * 1000 + + trajectoryTimes[i].nsec() / 1000000; + auto neededIters = iterFromStart - previousIterFromStart; + + // Run the simulation + server.Run(true, neededIters, false); + + // Assert that each joint reached its target velocity + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + EXPECT_NEAR(currentVelocities[j], trajectoryVelocities[i][j], TOL); + } + + // Keep track of how many iterations have already passed + previousIterFromStart = iterFromStart; + } +} diff --git a/test/worlds/joint_trajectory_controller.sdf b/test/worlds/joint_trajectory_controller.sdf new file mode 100644 index 0000000000..4aaee68ca5 --- /dev/null +++ b/test/worlds/joint_trajectory_controller.sdf @@ -0,0 +1,420 @@ + + + + + + + + + + + + 0.4 0.4 0.4 + false + + + + + + + + + + 3D View + false + docked + + ogre + scene + 0 0 1 0 1.5708 0 + + + + + + World control + false + false + 50 + 100 + 1 + floating + + + + + + true + true + true + + + + + + World stats + false + false + 250 + 110 + 1 + floating + + + + + + true + true + true + true + + + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + + 0 -0.5 0 0 1.5708 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + + RR_position_control_joint2 + 25 + 400 + 0.1 + -0.5 + 0.5 + -5 + 5 + + RR_position_control_joint1 + 50 + 600 + 0.8 + -1 + 1 + -10 + 10 + + + + + + + 0 0.5 0 0 1.5708 0 + + + world + RR_velocity_control_link0 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0.025 + + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_velocity_control_link0 + RR_velocity_control_link1 + + 1 0 0 + + + 0.02 + + + + 0 0 0.1 0 0 0 + RR_velocity_control_link1 + RR_velocity_control_link2 + + 1 0 0 + + + 0.01 + + + + + + test_custom_topic/velocity_control + + + 0.6 + 175 + -10 + 10 + + 0.1 + 200 + -5 + 5 + + + + + diff --git a/tutorials/video_recorder.md b/tutorials/video_recorder.md index 50bbef75d9..47454230cf 100644 --- a/tutorials/video_recorder.md +++ b/tutorials/video_recorder.md @@ -105,3 +105,10 @@ going to the world SDF file, locate the * **bitrate**: Video encoding bitrate in bps. This affects the quality of the generated video. The default bitrate is 2Mbps. + +## Hardware-accelerated encoding + +Since Ignition Common 3.10.2, there is support for utilizing the power of GPUs +to speed up the video encoding process. See the +[Hardware-accelerated Video Encoding tutorial](https://ignitionrobotics.org/api/common/3.10/hw-encoding.html) +for more details.