From 4325f2275fce173859c1617dd60b0c34ffede0ae Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 6 Oct 2020 23:30:50 -0700 Subject: [PATCH 01/26] set link vel Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- src/systems/physics/Physics.cc | 80 ++++++++++++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 763026945c..0ca1791b71 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1467,6 +1467,86 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Update link angular velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::AngularVelocityCmd *_angularVelocityCmd) + { + auto linkIt = this->entityLinkMap.find(_entity); + if (linkIt == this->entityLinkMap.end()) + return true; + + auto freeGroup = linkIt->second->FindFreeGroup(); + if (!freeGroup) + return true; + + const components::Pose *poseComp = + _ecm.Component(_entity); + math::Vector3d worldAngularVel = poseComp->Data().Rot() * + _angularVelocityCmd->Data(); + + auto worldAngularVelFeature = entityCast(_entity, freeGroup, + this->entityWorldVelocityCommandMap); + if (!worldAngularVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + + worldAngularVelFeature->SetWorldAngularVelocity( + math::eigen3::convert(worldAngularVel)); + + return true; + }); + + // Update link linear velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::LinearVelocityCmd *_linearVelocityCmd) + { + auto linkIt = this->entityLinkMap.find(_entity); + if (linkIt == this->entityLinkMap.end()) + return true; + + auto freeGroup = linkIt->second->FindFreeGroup(); + if (!freeGroup) + return true; + + const components::Pose *poseComp = + _ecm.Component(_entity); + math::Vector3d worldLinearVel = poseComp->Data().Rot() * + _linearVelocityCmd->Data(); + + auto worldLinearVelFeature = entityCast(_entity, freeGroup, + this->entityWorldVelocityCommandMap); + if (!worldLinearVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + + worldLinearVelFeature->SetWorldLinearVelocity( + math::eigen3::convert(worldLinearVel)); + + return true; + }); + // Clear pending commands // Note: Removing components from inside an Each call can be dangerous. // Instead, we collect all the entities that have the desired components and From a4f43dbf3137c141971814ac0cf7038734653d47 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Wed, 14 Oct 2020 11:11:34 -0700 Subject: [PATCH 02/26] add sdf Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/two_links.sdf | 132 ++++++++++++++++++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100644 examples/worlds/two_links.sdf diff --git a/examples/worlds/two_links.sdf b/examples/worlds/two_links.sdf new file mode 100644 index 0000000000..9c32cdb19a --- /dev/null +++ b/examples/worlds/two_links.sdf @@ -0,0 +1,132 @@ + + + + + 0.001 + 1.0 + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/car_world/control + /world/car_world/stats + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/car_world/stats + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + \ No newline at end of file From eba1aaef683d316370cb5b2b164501ce05f3459e Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Wed, 21 Oct 2020 14:14:57 -0700 Subject: [PATCH 03/26] add link controller demo Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/double_pendulum.sdf | 313 ++++++++++++++++++ .../link_position_controller/CMakeLists.txt | 0 .../LinkPositionController.cc | 0 .../LinkPositionController.hh | 0 4 files changed, 313 insertions(+) create mode 100644 examples/worlds/double_pendulum.sdf create mode 100644 src/systems/link_position_controller/CMakeLists.txt create mode 100644 src/systems/link_position_controller/LinkPositionController.cc create mode 100644 src/systems/link_position_controller/LinkPositionController.hh diff --git a/examples/worlds/double_pendulum.sdf b/examples/worlds/double_pendulum.sdf new file mode 100644 index 0000000000..a6fc8e8821 --- /dev/null +++ b/examples/worlds/double_pendulum.sdf @@ -0,0 +1,313 @@ + + + + + 0.001 + 1.0 + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/car_world/control + /world/car_world/stats + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + upper_pendulum_link + lower_pendulum_link + + + + \ No newline at end of file diff --git a/src/systems/link_position_controller/CMakeLists.txt b/src/systems/link_position_controller/CMakeLists.txt new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/systems/link_position_controller/LinkPositionController.cc b/src/systems/link_position_controller/LinkPositionController.cc new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/systems/link_position_controller/LinkPositionController.hh b/src/systems/link_position_controller/LinkPositionController.hh new file mode 100644 index 0000000000..e69de29bb2 From dcde2ee0547675aeb5104dade1eea96e51795572 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 22 Oct 2020 00:00:12 -0700 Subject: [PATCH 04/26] add link position controller for link positions demo and testing with tpe Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- ...dulum.sdf => link_position_controller.sdf} | 0 src/systems/CMakeLists.txt | 1 + .../link_position_controller/CMakeLists.txt | 7 ++ .../LinkPositionController.cc | 114 ++++++++++++++++++ .../LinkPositionController.hh | 73 +++++++++++ 5 files changed, 195 insertions(+) rename examples/worlds/{double_pendulum.sdf => link_position_controller.sdf} (100%) diff --git a/examples/worlds/double_pendulum.sdf b/examples/worlds/link_position_controller.sdf similarity index 100% rename from examples/worlds/double_pendulum.sdf rename to examples/worlds/link_position_controller.sdf diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index b85928dd94..0c47fbe861 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -85,6 +85,7 @@ add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) add_subdirectory(lift_drag) +add_subdirectory(link_position_controller) add_subdirectory(log) add_subdirectory(log_video_recorder) add_subdirectory(logical_camera) diff --git a/src/systems/link_position_controller/CMakeLists.txt b/src/systems/link_position_controller/CMakeLists.txt index e69de29bb2..cc12bd0102 100644 --- a/src/systems/link_position_controller/CMakeLists.txt +++ b/src/systems/link_position_controller/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(link-position-controller + SOURCES + LinkPositionController.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/link_position_controller/LinkPositionController.cc b/src/systems/link_position_controller/LinkPositionController.cc index e69de29bb2..a8954ceaf9 100644 --- a/src/systems/link_position_controller/LinkPositionController.cc +++ b/src/systems/link_position_controller/LinkPositionController.cc @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2020 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/Model.hh" + +#include "LinkPositionController.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::LinkPositionControllerPrivate +{ + /// \brief Callback for position subscription + /// \param[in] _msg Position message + public: void OnCmdPos(const ignition::msgs::Double &_msg); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Link Entity + public: Entity linkEntity; + + /// \brief Link name + public: std::string linkName; + + /// \brief Commanded Link position + public: double linkPosCmd; + + /// \brief mutex to protect Link commands + public: std::mutex linkCmdMutex; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Position PID controller. + public: ignition::math::PID posPid; + + /// \brief Link index to be used. + public: unsigned int linkIndex = 0u; +}; + +////////////////////////////////////////////////// +LinkPositionController::LinkPositionController() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void LinkPositionController::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "LinkPositionController plugin should be attached to a model " + << "entity. Failed to initialize." << std::endl; + return; + } +} + +////////////////////////////////////////////////// +void LinkPositionController::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("JointPositionController::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } +} + +////////////////////////////////////////////////// +void LinkPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg) +{ + std::lock_guard lock(this->linkCmdMutex); + this->linkPosCmd = _msg.data(); +} + +IGNITION_ADD_PLUGIN(LinkPositionController, + ignition::gazebo::System, + LinkPositionController::ISystemConfigure, + LinkPositionController::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(LinkPositionController, + "ignition::gazebo::systems::LinkPositionController") diff --git a/src/systems/link_position_controller/LinkPositionController.hh b/src/systems/link_position_controller/LinkPositionController.hh index e69de29bb2..df595f0d64 100644 --- a/src/systems/link_position_controller/LinkPositionController.hh +++ b/src/systems/link_position_controller/LinkPositionController.hh @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2020 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_LINKPOSITIONCONTROLLER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_LINKPOSITIONCONTROLLER_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class LinkPositionControllerPrivate; + + /// \brief Link position controller which can be attached to a model with a + /// reference to a single link. + /// + /// A new Ignition Transport topic is created to send target link positions. + /// The topic name is + /// "/model//link///cmd_pos" + /// + /// This topic accepts ignition::msgs::Double values representing the target + /// position. + class IGNITION_GAZEBO_VISIBLE LinkPositionController + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: LinkPositionController(); + + /// \brief Destructor + public: ~LinkPositionController() 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; + }; + } +} +} +} + +#endif From ddd009282be75e196073c9a4d25afe74eea102e4 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 22 Oct 2020 00:11:21 -0700 Subject: [PATCH 05/26] change name to reflect velocity Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- ...oller.sdf => link_velocity_controller.sdf} | 0 .../CMakeLists.txt | 4 +- .../LinkVelocityController.cc} | 37 +++++++++---------- .../LinkVelocityController.hh} | 18 ++++----- 4 files changed, 29 insertions(+), 30 deletions(-) rename examples/worlds/{link_position_controller.sdf => link_velocity_controller.sdf} (100%) rename src/systems/{link_position_controller => link_velocity_controller}/CMakeLists.txt (72%) rename src/systems/{link_position_controller/LinkPositionController.cc => link_velocity_controller/LinkVelocityController.cc} (71%) rename src/systems/{link_position_controller/LinkPositionController.hh => link_velocity_controller/LinkVelocityController.hh} (80%) diff --git a/examples/worlds/link_position_controller.sdf b/examples/worlds/link_velocity_controller.sdf similarity index 100% rename from examples/worlds/link_position_controller.sdf rename to examples/worlds/link_velocity_controller.sdf diff --git a/src/systems/link_position_controller/CMakeLists.txt b/src/systems/link_velocity_controller/CMakeLists.txt similarity index 72% rename from src/systems/link_position_controller/CMakeLists.txt rename to src/systems/link_velocity_controller/CMakeLists.txt index cc12bd0102..0cd3f177ce 100644 --- a/src/systems/link_position_controller/CMakeLists.txt +++ b/src/systems/link_velocity_controller/CMakeLists.txt @@ -1,6 +1,6 @@ -gz_add_system(link-position-controller +gz_add_system(link-velocity-controller SOURCES - LinkPositionController.cc + LinkVelocityController.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/link_position_controller/LinkPositionController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc similarity index 71% rename from src/systems/link_position_controller/LinkPositionController.cc rename to src/systems/link_velocity_controller/LinkVelocityController.cc index a8954ceaf9..4d31944f5e 100644 --- a/src/systems/link_position_controller/LinkPositionController.cc +++ b/src/systems/link_velocity_controller/LinkVelocityController.cc @@ -23,16 +23,16 @@ #include "ignition/gazebo/Model.hh" -#include "LinkPositionController.hh" +#include "LinkVelocityController.hh" using namespace ignition; using namespace gazebo; using namespace systems; -class ignition::gazebo::systems::LinkPositionControllerPrivate +class ignition::gazebo::systems::LinkVelocityControllerPrivate { - /// \brief Callback for position subscription - /// \param[in] _msg Position message + /// \brief Callback for Velocity subscription + /// \param[in] _msg Velocity message public: void OnCmdPos(const ignition::msgs::Double &_msg); /// \brief Ignition communication node. @@ -44,7 +44,7 @@ class ignition::gazebo::systems::LinkPositionControllerPrivate /// \brief Link name public: std::string linkName; - /// \brief Commanded Link position + /// \brief Commanded Link Velocity public: double linkPosCmd; /// \brief mutex to protect Link commands @@ -53,7 +53,7 @@ class ignition::gazebo::systems::LinkPositionControllerPrivate /// \brief Model interface public: Model model{kNullEntity}; - /// \brief Position PID controller. + /// \brief Velocity PID controller. public: ignition::math::PID posPid; /// \brief Link index to be used. @@ -61,13 +61,13 @@ class ignition::gazebo::systems::LinkPositionControllerPrivate }; ////////////////////////////////////////////////// -LinkPositionController::LinkPositionController() - : dataPtr(std::make_unique()) +LinkVelocityController::LinkVelocityController() + : dataPtr(std::make_unique()) { } ////////////////////////////////////////////////// -void LinkPositionController::Configure(const Entity &_entity, +void LinkVelocityController::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, EventManager &/*_eventMgr*/) @@ -76,20 +76,19 @@ void LinkPositionController::Configure(const Entity &_entity, if (!this->dataPtr->model.Valid(_ecm)) { - ignerr << "LinkPositionController plugin should be attached to a model " + ignerr << "LinkVelocityController plugin should be attached to a model " << "entity. Failed to initialize." << std::endl; return; } } ////////////////////////////////////////////////// -void LinkPositionController::PreUpdate( +void LinkVelocityController::PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { - IGN_PROFILE("JointPositionController::PreUpdate"); + IGN_PROFILE("JointVelocityController::PreUpdate"); - // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { ignwarn << "Detected jump back in time [" @@ -99,16 +98,16 @@ void LinkPositionController::PreUpdate( } ////////////////////////////////////////////////// -void LinkPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg) +void LinkVelocityControllerPrivate::OnCmdPos(const msgs::Double &_msg) { std::lock_guard lock(this->linkCmdMutex); this->linkPosCmd = _msg.data(); } -IGNITION_ADD_PLUGIN(LinkPositionController, +IGNITION_ADD_PLUGIN(LinkVelocityController, ignition::gazebo::System, - LinkPositionController::ISystemConfigure, - LinkPositionController::ISystemPreUpdate) + LinkVelocityController::ISystemConfigure, + LinkVelocityController::ISystemPreUpdate) -IGNITION_ADD_PLUGIN_ALIAS(LinkPositionController, - "ignition::gazebo::systems::LinkPositionController") +IGNITION_ADD_PLUGIN_ALIAS(LinkVelocityController, + "ignition::gazebo::systems::LinkVelocityController") diff --git a/src/systems/link_position_controller/LinkPositionController.hh b/src/systems/link_velocity_controller/LinkVelocityController.hh similarity index 80% rename from src/systems/link_position_controller/LinkPositionController.hh rename to src/systems/link_velocity_controller/LinkVelocityController.hh index df595f0d64..03809c45ba 100644 --- a/src/systems/link_position_controller/LinkPositionController.hh +++ b/src/systems/link_velocity_controller/LinkVelocityController.hh @@ -14,8 +14,8 @@ * limitations under the License. * */ -#ifndef IGNITION_GAZEBO_SYSTEMS_LINKPOSITIONCONTROLLER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_LINKPOSITIONCONTROLLER_HH_ +#ifndef IGNITION_GAZEBO_SYSTEMS_LINKVELOCITYCONTROLLER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_LINKVELOCITYCONTROLLER_HH_ #include #include @@ -29,27 +29,27 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Forward declaration - class LinkPositionControllerPrivate; + class LinkVelocityontrollerPrivate; - /// \brief Link position controller which can be attached to a model with a + /// \brief Link velocity controller which can be attached to a model with a /// reference to a single link. /// - /// A new Ignition Transport topic is created to send target link positions. + /// A new Ignition Transport topic is created to send target link velocity. /// The topic name is /// "/model//link///cmd_pos" /// /// This topic accepts ignition::msgs::Double values representing the target /// position. - class IGNITION_GAZEBO_VISIBLE LinkPositionController + class IGNITION_GAZEBO_VISIBLE LinkVelocityontroller : public System, public ISystemConfigure, public ISystemPreUpdate { /// \brief Constructor - public: LinkPositionController(); + public: LinkVelocityController(); /// \brief Destructor - public: ~LinkPositionController() override = default; + public: ~LinkVelocityController() override = default; // Documentation inherited public: void Configure(const Entity &_entity, @@ -63,7 +63,7 @@ namespace systems ignition::gazebo::EntityComponentManager &_ecm) override; /// \brief Private data pointer - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; }; } } From e65e92f2a42424e43cfc128e69efaaf3ec6f70fa Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 22 Oct 2020 00:13:11 -0700 Subject: [PATCH 06/26] remove two_links Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/two_links.sdf | 132 ---------------------------------- 1 file changed, 132 deletions(-) delete mode 100644 examples/worlds/two_links.sdf diff --git a/examples/worlds/two_links.sdf b/examples/worlds/two_links.sdf deleted file mode 100644 index 9c32cdb19a..0000000000 --- a/examples/worlds/two_links.sdf +++ /dev/null @@ -1,132 +0,0 @@ - - - - - 0.001 - 1.0 - - - - - - - - - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/car_world/control - /world/car_world/stats - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/car_world/stats - - - - - - - - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - \ No newline at end of file From 9fa7e787562f5c1ac6804520b874c81e03ce3978 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 22 Oct 2020 21:57:03 -0700 Subject: [PATCH 07/26] add link vel control system Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- src/systems/CMakeLists.txt | 2 +- .../LinkVelocityController.cc | 83 +++++++++++++++++-- .../LinkVelocityController.hh | 12 ++- 3 files changed, 84 insertions(+), 13 deletions(-) diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index f3f0eb996f..a189203ea4 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -85,7 +85,7 @@ add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) add_subdirectory(lift_drag) -add_subdirectory(link_position_controller) +add_subdirectory(link_velocity_controller) add_subdirectory(log) add_subdirectory(log_video_recorder) add_subdirectory(logical_camera) diff --git a/src/systems/link_velocity_controller/LinkVelocityController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc index 4d31944f5e..f7195cc1b7 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.cc +++ b/src/systems/link_velocity_controller/LinkVelocityController.cc @@ -21,6 +21,8 @@ #include #include +#include "ignition/gazebo/components/AngularVelocity.hh" +// #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/Model.hh" #include "LinkVelocityController.hh" @@ -33,7 +35,7 @@ class ignition::gazebo::systems::LinkVelocityControllerPrivate { /// \brief Callback for Velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdPos(const ignition::msgs::Double &_msg); + public: void OnCmdVel(const ignition::msgs::Double &_msg); /// \brief Ignition communication node. public: transport::Node node; @@ -44,8 +46,11 @@ class ignition::gazebo::systems::LinkVelocityControllerPrivate /// \brief Link name public: std::string linkName; - /// \brief Commanded Link Velocity - public: double linkPosCmd; + // /// \brief Commanded Link Linear Velocity + // public: math::Vector3d linkLinearVelCmd; + + /// \brief Commanded Link Angular Velocity + public: double linkAngularVelCmd; /// \brief mutex to protect Link commands public: std::mutex linkCmdMutex; @@ -53,9 +58,6 @@ class ignition::gazebo::systems::LinkVelocityControllerPrivate /// \brief Model interface public: Model model{kNullEntity}; - /// \brief Velocity PID controller. - public: ignition::math::PID posPid; - /// \brief Link index to be used. public: unsigned int linkIndex = 0u; }; @@ -80,6 +82,23 @@ void LinkVelocityController::Configure(const Entity &_entity, << "entity. Failed to initialize." << std::endl; return; } + + // Get params from SDF + this->dataPtr->linkName = _sdf->Get("link_name"); + if (this->dataPtr->linkName == "") + { + ignerr << "LinkVelocityController found an empty linkName parameter. " + << "Failed to initialize."; + return; + } + + // Subscribe to commands + std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + this->dataPtr->linkName + "/" + + std::to_string(this->dataPtr->linkIndex) + "/cmd_vel"}; + this->dataPtr->node.Subscribe( + topic, &linkVelocityControllerPrivate::OnCmdVel, this->dataPtr.get()); + igndbg << "Topic: [" << topic << "]" << std::endl; } ////////////////////////////////////////////////// @@ -87,7 +106,7 @@ void LinkVelocityController::PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { - IGN_PROFILE("JointVelocityController::PreUpdate"); + IGN_PROFILE("LinkVelocityController::PreUpdate"); if (_info.dt < std::chrono::steady_clock::duration::zero()) { @@ -95,13 +114,61 @@ void LinkVelocityController::PreUpdate( << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; } + // If the link hasn't been identified yet, look for it + if (this->dataPtr->linkEntity == kNullEntity) + { + this->dataPtr->linkEntity = + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); + } + + if (this->dataPtr->linkEntity == kNullEntity) + return; + + // Nothing left to do if paused + if (_info.paused) + return; + + // // Create link velocity componenet if one doesn't exist + // auto linearVelocityComp = + // _ecm.Component(this->dataPtr->linkEntity); + // if (linearVelocityComp == nullptr) + // { + // _ecm.CreateComponent( + // this->dataPtr->linkEntity, components::LinearVelocity()); + // } + auto angularVelocityComp = + _ecm.Component(this->dataPtr->linkEntity); + if (angularVelocityComp == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->linkEntity, components::AngularVelocity()); + } + if (angularVelocityComp == nullptr) + return; + + // Sanity Check: make sure the link index is valid + if (this->dataPtr->linkIndex >= angularVelocityComp->Data().size()) + { + static bool invalidLinkReported = false; + if (!invalidLinkReported) + { + ignerr << "[LinkPositionController]: Detected an invalid " + << "parameter. The index specified is [" + << this->dataPtr->linkIndex << "] but the link only has [" + << angularVelocityComp->Data().size() << "] index[es]. " + << "This controller will be ignored" << std::endl; + invalidLinkReported = true; + } + return; + } } ////////////////////////////////////////////////// void LinkVelocityControllerPrivate::OnCmdPos(const msgs::Double &_msg) { std::lock_guard lock(this->linkCmdMutex); - this->linkPosCmd = _msg.data(); + // this->linkLinearVelCmd = _msg.data(); + this->linkAngularVelCmd = msg.data(); } IGNITION_ADD_PLUGIN(LinkVelocityController, diff --git a/src/systems/link_velocity_controller/LinkVelocityController.hh b/src/systems/link_velocity_controller/LinkVelocityController.hh index 03809c45ba..ff00ef1046 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.hh +++ b/src/systems/link_velocity_controller/LinkVelocityController.hh @@ -29,18 +29,22 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Forward declaration - class LinkVelocityontrollerPrivate; + class LinkVelocityControllerPrivate; /// \brief Link velocity controller which can be attached to a model with a /// reference to a single link. /// /// A new Ignition Transport topic is created to send target link velocity. /// The topic name is - /// "/model//link///cmd_pos" + /// "/model//link///cmd_vel" /// /// This topic accepts ignition::msgs::Double values representing the target - /// position. - class IGNITION_GAZEBO_VISIBLE LinkVelocityontroller + /// velocity. + /// + /// ## System Parameters + /// + /// `` The name of the link to control. Required parameter. + class IGNITION_GAZEBO_VISIBLE LinkVelocityController : public System, public ISystemConfigure, public ISystemPreUpdate From 6f61b2e51d6d97ed5264d1529f11dc0913e46ad6 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Fri, 23 Oct 2020 18:20:12 -0700 Subject: [PATCH 08/26] add vector3d msg, need to resolve link name parsing issue Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/link_velocity_controller.sdf | 13 ++++++- .../LinkVelocityController.cc | 39 ++++++------------- 2 files changed, 22 insertions(+), 30 deletions(-) diff --git a/examples/worlds/link_velocity_controller.sdf b/examples/worlds/link_velocity_controller.sdf index a6fc8e8821..a9872ce1a9 100644 --- a/examples/worlds/link_velocity_controller.sdf +++ b/examples/worlds/link_velocity_controller.sdf @@ -1,3 +1,12 @@ + + @@ -303,8 +312,8 @@ + filename="ignition-gazebo-link-velocity-controller-system" + name="ignition::gazebo::systems::LinkVelocityController"> upper_pendulum_link lower_pendulum_link diff --git a/src/systems/link_velocity_controller/LinkVelocityController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc index f7195cc1b7..99e87b7206 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.cc +++ b/src/systems/link_velocity_controller/LinkVelocityController.cc @@ -15,9 +15,10 @@ * */ -#include +#include #include #include +#include #include #include @@ -35,7 +36,7 @@ class ignition::gazebo::systems::LinkVelocityControllerPrivate { /// \brief Callback for Velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Double &_msg); + public: void OnCmdVel(const ignition::msgs::Vector3d &_msg); /// \brief Ignition communication node. public: transport::Node node; @@ -50,16 +51,13 @@ class ignition::gazebo::systems::LinkVelocityControllerPrivate // public: math::Vector3d linkLinearVelCmd; /// \brief Commanded Link Angular Velocity - public: double linkAngularVelCmd; + public: math::Vector3d linkAngularVelCmd; /// \brief mutex to protect Link commands public: std::mutex linkCmdMutex; /// \brief Model interface public: Model model{kNullEntity}; - - /// \brief Link index to be used. - public: unsigned int linkIndex = 0u; }; ////////////////////////////////////////////////// @@ -84,7 +82,8 @@ void LinkVelocityController::Configure(const Entity &_entity, } // Get params from SDF - this->dataPtr->linkName = _sdf->Get("link_name"); + if (_sdf->HasElement("link")) + this->dataPtr->linkName = _sdf->Get("link_name"); if (this->dataPtr->linkName == "") { ignerr << "LinkVelocityController found an empty linkName parameter. " @@ -94,10 +93,9 @@ void LinkVelocityController::Configure(const Entity &_entity, // Subscribe to commands std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/link/" + this->dataPtr->linkName + "/" + - std::to_string(this->dataPtr->linkIndex) + "/cmd_vel"}; + "/link/" + this->dataPtr->linkName + "/cmd_vel"}; this->dataPtr->node.Subscribe( - topic, &linkVelocityControllerPrivate::OnCmdVel, this->dataPtr.get()); + topic, &LinkVelocityControllerPrivate::OnCmdVel, this->dataPtr.get()); igndbg << "Topic: [" << topic << "]" << std::endl; } @@ -145,30 +143,15 @@ void LinkVelocityController::PreUpdate( } if (angularVelocityComp == nullptr) return; - - // Sanity Check: make sure the link index is valid - if (this->dataPtr->linkIndex >= angularVelocityComp->Data().size()) - { - static bool invalidLinkReported = false; - if (!invalidLinkReported) - { - ignerr << "[LinkPositionController]: Detected an invalid " - << "parameter. The index specified is [" - << this->dataPtr->linkIndex << "] but the link only has [" - << angularVelocityComp->Data().size() << "] index[es]. " - << "This controller will be ignored" << std::endl; - invalidLinkReported = true; - } - return; - } } ////////////////////////////////////////////////// -void LinkVelocityControllerPrivate::OnCmdPos(const msgs::Double &_msg) +void LinkVelocityControllerPrivate::OnCmdVel(const msgs::Vector3d &_msg) { std::lock_guard lock(this->linkCmdMutex); // this->linkLinearVelCmd = _msg.data(); - this->linkAngularVelCmd = msg.data(); + std::cout << _msg.x() << ", " << _msg.y() << ", " << _msg.z() << std::endl; + this->linkAngularVelCmd = math::Vector3d(_msg.x(), _msg.y(), _msg.z()); } IGNITION_ADD_PLUGIN(LinkVelocityController, From c73a5f8ff47e408a3d295a8f4c11e1eb9f719103 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Fri, 23 Oct 2020 23:23:58 -0700 Subject: [PATCH 09/26] correct link name in sdf Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/link_velocity_controller.sdf | 4 ++-- .../LinkVelocityController.cc | 21 ++++++++++++------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/examples/worlds/link_velocity_controller.sdf b/examples/worlds/link_velocity_controller.sdf index a9872ce1a9..9da5db86de 100644 --- a/examples/worlds/link_velocity_controller.sdf +++ b/examples/worlds/link_velocity_controller.sdf @@ -314,8 +314,8 @@ - upper_pendulum_link - lower_pendulum_link + upper_link + lower_link diff --git a/src/systems/link_velocity_controller/LinkVelocityController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc index 99e87b7206..88d1770127 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.cc +++ b/src/systems/link_velocity_controller/LinkVelocityController.cc @@ -44,8 +44,11 @@ class ignition::gazebo::systems::LinkVelocityControllerPrivate /// \brief Link Entity public: Entity linkEntity; - /// \brief Link name - public: std::string linkName; + /// \brief Link 1 name + public: std::string link1Name; + + /// \brief Link 2 name + public: std::string link2Name; // /// \brief Commanded Link Linear Velocity // public: math::Vector3d linkLinearVelCmd; @@ -82,18 +85,20 @@ void LinkVelocityController::Configure(const Entity &_entity, } // Get params from SDF - if (_sdf->HasElement("link")) - this->dataPtr->linkName = _sdf->Get("link_name"); - if (this->dataPtr->linkName == "") + this->dataPtr->link1Name = _sdf->Get("link1_name"); + // ignerr << _sdf->ToString("") << std::endl; + // // if (_sdf->HasElement("link")) + // // this->dataPtr->linkName = _sdf->GetAttribute("link"); + if (this->dataPtr->link1Name == "") { - ignerr << "LinkVelocityController found an empty linkName parameter. " + ignerr << "LinkVelocityController found an empty link1Name parameter. " << "Failed to initialize."; return; } // Subscribe to commands std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/link/" + this->dataPtr->linkName + "/cmd_vel"}; + "/link/" + this->dataPtr->link1Name + "/cmd_vel"}; this->dataPtr->node.Subscribe( topic, &LinkVelocityControllerPrivate::OnCmdVel, this->dataPtr.get()); igndbg << "Topic: [" << topic << "]" << std::endl; @@ -116,7 +121,7 @@ void LinkVelocityController::PreUpdate( if (this->dataPtr->linkEntity == kNullEntity) { this->dataPtr->linkEntity = - this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->link1Name); } if (this->dataPtr->linkEntity == kNullEntity) From d73fb28b5a927cdc91fead0c44f0cddb311e98ff Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Wed, 16 Dec 2020 16:42:29 -0800 Subject: [PATCH 10/26] set one link vel Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/link_velocity_controller.sdf | 30 ++++- .../LinkVelocityController.cc | 123 +++++++++++------- .../LinkVelocityController.hh | 12 +- src/systems/physics/Physics.cc | 12 +- 4 files changed, 120 insertions(+), 57 deletions(-) diff --git a/examples/worlds/link_velocity_controller.sdf b/examples/worlds/link_velocity_controller.sdf index 9da5db86de..9f01855cac 100644 --- a/examples/worlds/link_velocity_controller.sdf +++ b/examples/worlds/link_velocity_controller.sdf @@ -2,9 +2,10 @@ Ignition Gazebo link velocity control demo Try sending commands: - ign topic -t "/model/double_pendulum/link/upper_link/cmd_vel" -m ignition.msgs.Vector3d -p "x: 0, y: 0, z: 0.05" + ign topic -t "/model/double_pendulum_with_base/link/upper_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {z: 0.05}" + + ign topic -t "/model/double_pendulum_with_base/link/lower_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {z: -0.10}" - ign topic -t "/model/double_pendulum/link/lower_link/cmd_vel" -m ignition.msgs.Vector3d -p "x: 0, y: 0, z: 0.01" --> @@ -67,6 +68,31 @@ /world/car_world/stats + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/velocity_control/stats + + + diff --git a/src/systems/link_velocity_controller/LinkVelocityController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc index 88d1770127..b84775c931 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.cc +++ b/src/systems/link_velocity_controller/LinkVelocityController.cc @@ -22,7 +22,7 @@ #include #include -#include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/AngularVelocityCmd.hh" // #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/Model.hh" @@ -34,30 +34,34 @@ using namespace systems; class ignition::gazebo::systems::LinkVelocityControllerPrivate { - /// \brief Callback for Velocity subscription + /// \brief Callback for velocity subscription /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Vector3d &_msg); + public: void OnCmdVel(const ignition::msgs::Twist &_msg); + + /// \brief Update link angular velocity. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); /// \brief Ignition communication node. public: transport::Node node; - /// \brief Link Entity + /// \brief Link entity public: Entity linkEntity; - /// \brief Link 1 name - public: std::string link1Name; - - /// \brief Link 2 name - public: std::string link2Name; + /// \brief Link name + public: std::string linkName; - // /// \brief Commanded Link Linear Velocity - // public: math::Vector3d linkLinearVelCmd; + /// \brief Angular velocity of link + public: math::Vector3d linkAngularVel {0, 0, 0}; - /// \brief Commanded Link Angular Velocity - public: math::Vector3d linkAngularVelCmd; + /// \brief Mutex to protect link commands + public: std::mutex mutex; - /// \brief mutex to protect Link commands - public: std::mutex linkCmdMutex; + /// \brief Last target velocity requested. + public: msgs::Twist targetVel; /// \brief Model interface public: Model model{kNullEntity}; @@ -85,11 +89,9 @@ void LinkVelocityController::Configure(const Entity &_entity, } // Get params from SDF - this->dataPtr->link1Name = _sdf->Get("link1_name"); - // ignerr << _sdf->ToString("") << std::endl; - // // if (_sdf->HasElement("link")) - // // this->dataPtr->linkName = _sdf->GetAttribute("link"); - if (this->dataPtr->link1Name == "") + // TODO: add link2_name + this->dataPtr->linkName = _sdf->Get("link1_name"); + if (this->dataPtr->linkName == "") { ignerr << "LinkVelocityController found an empty link1Name parameter. " << "Failed to initialize."; @@ -98,10 +100,11 @@ void LinkVelocityController::Configure(const Entity &_entity, // Subscribe to commands std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/link/" + this->dataPtr->link1Name + "/cmd_vel"}; + "/link/" + this->dataPtr->linkName + "/cmd_vel"}; this->dataPtr->node.Subscribe( topic, &LinkVelocityControllerPrivate::OnCmdVel, this->dataPtr.get()); - igndbg << "Topic: [" << topic << "]" << std::endl; + ignmsg << "LinkVelocityController subscribing to twist messages on [" << topic << "]" + << std::endl; } ////////////////////////////////////////////////// @@ -117,52 +120,78 @@ void LinkVelocityController::PreUpdate( << std::chrono::duration_cast(_info.dt).count() << "s]. System may not work properly." << std::endl; } + + // Nothing left to do if paused. + if (_info.paused) + return; + // If the link hasn't been identified yet, look for it if (this->dataPtr->linkEntity == kNullEntity) { this->dataPtr->linkEntity = - this->dataPtr->model.LinkByName(_ecm, this->dataPtr->link1Name); + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); } if (this->dataPtr->linkEntity == kNullEntity) return; - - // Nothing left to do if paused - if (_info.paused) - return; - - // // Create link velocity componenet if one doesn't exist - // auto linearVelocityComp = - // _ecm.Component(this->dataPtr->linkEntity); - // if (linearVelocityComp == nullptr) - // { - // _ecm.CreateComponent( - // this->dataPtr->linkEntity, components::LinearVelocity()); - // } - auto angularVelocityComp = - _ecm.Component(this->dataPtr->linkEntity); - if (angularVelocityComp == nullptr) + + // update angular velocity of link + auto angularVel = + _ecm.Component(this->dataPtr->linkEntity); + if (angularVel == nullptr) { _ecm.CreateComponent( - this->dataPtr->linkEntity, components::AngularVelocity()); + this->dataPtr->linkEntity, + components::AngularVelocityCmd({this->dataPtr->linkAngularVel})); + } + else + { + *angularVel = + components::AngularVelocityCmd({this->dataPtr->linkAngularVel}); } - if (angularVelocityComp == nullptr) +} + +////////////////////////////////////////////////// +void LinkVelocityController::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("LinkVelocityController::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) return; + + this->dataPtr->UpdateVelocity(_info, _ecm); +} + +////////////////////////////////////////////////// +void LinkVelocityControllerPrivate::UpdateVelocity( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + IGN_PROFILE("LinkVelocityController::UpdateVelocity"); + + double angVel; + { + std::lock_guard lock(this->mutex); + angVel = this->targetVel.angular().z(); + } + + this->linkAngularVel = math::Vector3d( + this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); } ////////////////////////////////////////////////// -void LinkVelocityControllerPrivate::OnCmdVel(const msgs::Vector3d &_msg) +void LinkVelocityControllerPrivate::OnCmdVel(const msgs::Twist &_msg) { - std::lock_guard lock(this->linkCmdMutex); - // this->linkLinearVelCmd = _msg.data(); - std::cout << _msg.x() << ", " << _msg.y() << ", " << _msg.z() << std::endl; - this->linkAngularVelCmd = math::Vector3d(_msg.x(), _msg.y(), _msg.z()); + std::lock_guard lock(this->mutex); + this->targetVel = _msg; } IGNITION_ADD_PLUGIN(LinkVelocityController, ignition::gazebo::System, LinkVelocityController::ISystemConfigure, - LinkVelocityController::ISystemPreUpdate) + LinkVelocityController::ISystemPreUpdate, + LinkVelocityController::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(LinkVelocityController, "ignition::gazebo::systems::LinkVelocityController") diff --git a/src/systems/link_velocity_controller/LinkVelocityController.hh b/src/systems/link_velocity_controller/LinkVelocityController.hh index ff00ef1046..5b794c4833 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.hh +++ b/src/systems/link_velocity_controller/LinkVelocityController.hh @@ -38,16 +38,17 @@ namespace systems /// The topic name is /// "/model//link///cmd_vel" /// - /// This topic accepts ignition::msgs::Double values representing the target + /// This topic accepts ignition::msgs::Twist values representing the target /// velocity. /// /// ## System Parameters - /// + /// /// `` The name of the link to control. Required parameter. class IGNITION_GAZEBO_VISIBLE LinkVelocityController : public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemPostUpdate { /// \brief Constructor public: LinkVelocityController(); @@ -66,6 +67,11 @@ namespace systems const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1c58451922..ff3af2b6da 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1649,16 +1649,18 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) auto linkIt = this->entityLinkMap.find(_entity); if (linkIt == this->entityLinkMap.end()) return true; - + std::cout << "Found link" << std::endl; auto freeGroup = linkIt->second->FindFreeGroup(); if (!freeGroup) return true; - + std::cout << "Found free group" << std::endl; const components::Pose *poseComp = _ecm.Component(_entity); + // std::cout << "rotation: " << poseComp->Data().Rot() << std::endl; math::Vector3d worldAngularVel = poseComp->Data().Rot() * _angularVelocityCmd->Data(); - + // std::cout << "angular velocity cmd: " << _angularVelocityCmd->Data()[2] << std::endl; + // std::cout << "world angular velocity: " << worldAngularVel[0] << ", " << worldAngularVel[1] << ", " << worldAngularVel[2] << std::endl; auto worldAngularVelFeature = entityCast(_entity, freeGroup, this->entityWorldVelocityCommandMap); if (!worldAngularVelFeature) @@ -1689,11 +1691,11 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) auto linkIt = this->entityLinkMap.find(_entity); if (linkIt == this->entityLinkMap.end()) return true; - + auto freeGroup = linkIt->second->FindFreeGroup(); if (!freeGroup) return true; - + const components::Pose *poseComp = _ecm.Component(_entity); math::Vector3d worldLinearVel = poseComp->Data().Rot() * From df88d1a448f7cc4f5bde712a5bca874a944b57dc Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Wed, 16 Dec 2020 16:43:59 -0800 Subject: [PATCH 11/26] remove profiler Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- .../link_velocity_controller/LinkVelocityController.cc | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/systems/link_velocity_controller/LinkVelocityController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc index b84775c931..0d65108507 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.cc +++ b/src/systems/link_velocity_controller/LinkVelocityController.cc @@ -16,14 +16,12 @@ */ #include -#include #include #include #include #include #include "ignition/gazebo/components/AngularVelocityCmd.hh" -// #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/Model.hh" #include "LinkVelocityController.hh" @@ -112,8 +110,6 @@ void LinkVelocityController::PreUpdate( const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { - IGN_PROFILE("LinkVelocityController::PreUpdate"); - if (_info.dt < std::chrono::steady_clock::duration::zero()) { ignwarn << "Detected jump back in time [" @@ -155,7 +151,6 @@ void LinkVelocityController::PreUpdate( void LinkVelocityController::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { - IGN_PROFILE("LinkVelocityController::PostUpdate"); // Nothing left to do if paused. if (_info.paused) return; @@ -168,8 +163,6 @@ void LinkVelocityControllerPrivate::UpdateVelocity( const ignition::gazebo::UpdateInfo &/*_info*/, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { - IGN_PROFILE("LinkVelocityController::UpdateVelocity"); - double angVel; { std::lock_guard lock(this->mutex); From 8d58761d2fba546f8249d2edbc4bd8aff3c7e501 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 17 Dec 2020 18:16:41 -0800 Subject: [PATCH 12/26] remove std::cout Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/link_velocity_controller.sdf | 4 ++-- src/systems/physics/Physics.cc | 5 ----- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/examples/worlds/link_velocity_controller.sdf b/examples/worlds/link_velocity_controller.sdf index 9f01855cac..a650073bf5 100644 --- a/examples/worlds/link_velocity_controller.sdf +++ b/examples/worlds/link_velocity_controller.sdf @@ -2,9 +2,9 @@ Ignition Gazebo link velocity control demo Try sending commands: - ign topic -t "/model/double_pendulum_with_base/link/upper_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {z: 0.05}" + ign topic -t "/model/double_pendulum_with_base/link/upper_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {y: 0.05}" - ign topic -t "/model/double_pendulum_with_base/link/lower_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {z: -0.10}" + ign topic -t "/model/double_pendulum_with_base/link/lower_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {y: -0.10}" --> diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index ff3af2b6da..c506c5f3f1 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1649,18 +1649,13 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) auto linkIt = this->entityLinkMap.find(_entity); if (linkIt == this->entityLinkMap.end()) return true; - std::cout << "Found link" << std::endl; auto freeGroup = linkIt->second->FindFreeGroup(); if (!freeGroup) return true; - std::cout << "Found free group" << std::endl; const components::Pose *poseComp = _ecm.Component(_entity); - // std::cout << "rotation: " << poseComp->Data().Rot() << std::endl; math::Vector3d worldAngularVel = poseComp->Data().Rot() * _angularVelocityCmd->Data(); - // std::cout << "angular velocity cmd: " << _angularVelocityCmd->Data()[2] << std::endl; - // std::cout << "world angular velocity: " << worldAngularVel[0] << ", " << worldAngularVel[1] << ", " << worldAngularVel[2] << std::endl; auto worldAngularVelFeature = entityCast(_entity, freeGroup, this->entityWorldVelocityCommandMap); if (!worldAngularVelFeature) From 7519b1ce6fe805e5ec5549f44f09525ff2899af8 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Fri, 18 Dec 2020 14:52:58 -0800 Subject: [PATCH 13/26] only move lower arm Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/link_velocity_controller.sdf | 10 ++++------ .../link_velocity_controller/LinkVelocityController.cc | 3 +-- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/examples/worlds/link_velocity_controller.sdf b/examples/worlds/link_velocity_controller.sdf index a650073bf5..ce51cec71b 100644 --- a/examples/worlds/link_velocity_controller.sdf +++ b/examples/worlds/link_velocity_controller.sdf @@ -2,9 +2,7 @@ Ignition Gazebo link velocity control demo Try sending commands: - ign topic -t "/model/double_pendulum_with_base/link/upper_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {y: 0.05}" - - ign topic -t "/model/double_pendulum_with_base/link/lower_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {y: -0.10}" + ign topic -t "/model/double_pendulum_with_base/link/lower_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {x: 0.5}" --> @@ -64,8 +62,8 @@ true true true - /world/car_world/control - /world/car_world/stats + /world/link_velocity_controller/control + /world/link_velocity_controller/stats @@ -89,7 +87,7 @@ true true true - /world/velocity_control/stats + /world/link_velocity_controller/stats diff --git a/src/systems/link_velocity_controller/LinkVelocityController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc index 0d65108507..fba117b184 100644 --- a/src/systems/link_velocity_controller/LinkVelocityController.cc +++ b/src/systems/link_velocity_controller/LinkVelocityController.cc @@ -87,8 +87,7 @@ void LinkVelocityController::Configure(const Entity &_entity, } // Get params from SDF - // TODO: add link2_name - this->dataPtr->linkName = _sdf->Get("link1_name"); + this->dataPtr->linkName = _sdf->Get("link2_name"); if (this->dataPtr->linkName == "") { ignerr << "LinkVelocityController found an empty link1Name parameter. " From 628e2791e4903a7cdff817e7e81106b9eb0ed5f9 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Mon, 21 Dec 2020 23:32:38 -0800 Subject: [PATCH 14/26] add camera pose Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/link_velocity_controller.sdf | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/worlds/link_velocity_controller.sdf b/examples/worlds/link_velocity_controller.sdf index ce51cec71b..ee902209f5 100644 --- a/examples/worlds/link_velocity_controller.sdf +++ b/examples/worlds/link_velocity_controller.sdf @@ -40,6 +40,7 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 + -6 0 6 0 0.5 0 From 9651d97e77cb6e794a6f9a00c568582a638d998f Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 22 Dec 2020 22:15:06 -0800 Subject: [PATCH 15/26] add link velocity to velocity control Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- ...city_controller.sdf => pendulum_links.sdf} | 10 +- src/systems/CMakeLists.txt | 1 - .../link_velocity_controller/CMakeLists.txt | 7 - .../LinkVelocityController.cc | 189 ------------- .../LinkVelocityController.hh | 83 ------ .../velocity_control/VelocityControl.cc | 259 ++++++++++++++++-- 6 files changed, 248 insertions(+), 301 deletions(-) rename examples/worlds/{link_velocity_controller.sdf => pendulum_links.sdf} (97%) delete mode 100644 src/systems/link_velocity_controller/CMakeLists.txt delete mode 100644 src/systems/link_velocity_controller/LinkVelocityController.cc delete mode 100644 src/systems/link_velocity_controller/LinkVelocityController.hh diff --git a/examples/worlds/link_velocity_controller.sdf b/examples/worlds/pendulum_links.sdf similarity index 97% rename from examples/worlds/link_velocity_controller.sdf rename to examples/worlds/pendulum_links.sdf index ee902209f5..c2acf85c5e 100644 --- a/examples/worlds/link_velocity_controller.sdf +++ b/examples/worlds/pendulum_links.sdf @@ -63,8 +63,8 @@ true true true - /world/link_velocity_controller/control - /world/link_velocity_controller/stats + /world/velocity_control/control + /world/velocity_control/stats @@ -88,7 +88,7 @@ true true true - /world/link_velocity_controller/stats + /world/velocity_control/stats @@ -337,8 +337,8 @@ + filename="ignition-gazebo-velocity-control-system" + name="ignition::gazebo::systems::VelocityControl"> upper_link lower_link diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 4ac33a4116..529819fbe4 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -85,7 +85,6 @@ add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) add_subdirectory(lift_drag) -add_subdirectory(link_velocity_controller) add_subdirectory(log) add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) diff --git a/src/systems/link_velocity_controller/CMakeLists.txt b/src/systems/link_velocity_controller/CMakeLists.txt deleted file mode 100644 index 0cd3f177ce..0000000000 --- a/src/systems/link_velocity_controller/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -gz_add_system(link-velocity-controller - SOURCES - LinkVelocityController.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/link_velocity_controller/LinkVelocityController.cc b/src/systems/link_velocity_controller/LinkVelocityController.cc deleted file mode 100644 index fba117b184..0000000000 --- a/src/systems/link_velocity_controller/LinkVelocityController.cc +++ /dev/null @@ -1,189 +0,0 @@ -/* - * Copyright (C) 2020 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/AngularVelocityCmd.hh" -#include "ignition/gazebo/Model.hh" - -#include "LinkVelocityController.hh" - -using namespace ignition; -using namespace gazebo; -using namespace systems; - -class ignition::gazebo::systems::LinkVelocityControllerPrivate -{ - /// \brief Callback for velocity subscription - /// \param[in] _msg Velocity message - public: void OnCmdVel(const ignition::msgs::Twist &_msg); - - /// \brief Update link angular velocity. - /// \param[in] _info System update information. - /// \param[in] _ecm The EntityComponentManager of the given simulation - /// instance. - public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); - - /// \brief Ignition communication node. - public: transport::Node node; - - /// \brief Link entity - public: Entity linkEntity; - - /// \brief Link name - public: std::string linkName; - - /// \brief Angular velocity of link - public: math::Vector3d linkAngularVel {0, 0, 0}; - - /// \brief Mutex to protect link commands - public: std::mutex mutex; - - /// \brief Last target velocity requested. - public: msgs::Twist targetVel; - - /// \brief Model interface - public: Model model{kNullEntity}; -}; - -////////////////////////////////////////////////// -LinkVelocityController::LinkVelocityController() - : dataPtr(std::make_unique()) -{ -} - -////////////////////////////////////////////////// -void LinkVelocityController::Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) -{ - this->dataPtr->model = Model(_entity); - - if (!this->dataPtr->model.Valid(_ecm)) - { - ignerr << "LinkVelocityController plugin should be attached to a model " - << "entity. Failed to initialize." << std::endl; - return; - } - - // Get params from SDF - this->dataPtr->linkName = _sdf->Get("link2_name"); - if (this->dataPtr->linkName == "") - { - ignerr << "LinkVelocityController found an empty link1Name parameter. " - << "Failed to initialize."; - return; - } - - // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/link/" + this->dataPtr->linkName + "/cmd_vel"}; - this->dataPtr->node.Subscribe( - topic, &LinkVelocityControllerPrivate::OnCmdVel, this->dataPtr.get()); - ignmsg << "LinkVelocityController subscribing to twist messages on [" << topic << "]" - << std::endl; -} - -////////////////////////////////////////////////// -void LinkVelocityController::PreUpdate( - const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) -{ - if (_info.dt < std::chrono::steady_clock::duration::zero()) - { - ignwarn << "Detected jump back in time [" - << std::chrono::duration_cast(_info.dt).count() - << "s]. System may not work properly." << std::endl; - } - - // Nothing left to do if paused. - if (_info.paused) - return; - - // If the link hasn't been identified yet, look for it - if (this->dataPtr->linkEntity == kNullEntity) - { - this->dataPtr->linkEntity = - this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName); - } - - if (this->dataPtr->linkEntity == kNullEntity) - return; - - // update angular velocity of link - auto angularVel = - _ecm.Component(this->dataPtr->linkEntity); - if (angularVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->linkEntity, - components::AngularVelocityCmd({this->dataPtr->linkAngularVel})); - } - else - { - *angularVel = - components::AngularVelocityCmd({this->dataPtr->linkAngularVel}); - } -} - -////////////////////////////////////////////////// -void LinkVelocityController::PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm) -{ - // Nothing left to do if paused. - if (_info.paused) - return; - - this->dataPtr->UpdateVelocity(_info, _ecm); -} - -////////////////////////////////////////////////// -void LinkVelocityControllerPrivate::UpdateVelocity( - const ignition::gazebo::UpdateInfo &/*_info*/, - const ignition::gazebo::EntityComponentManager &/*_ecm*/) -{ - double angVel; - { - std::lock_guard lock(this->mutex); - angVel = this->targetVel.angular().z(); - } - - this->linkAngularVel = math::Vector3d( - this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); -} - -////////////////////////////////////////////////// -void LinkVelocityControllerPrivate::OnCmdVel(const msgs::Twist &_msg) -{ - std::lock_guard lock(this->mutex); - this->targetVel = _msg; -} - -IGNITION_ADD_PLUGIN(LinkVelocityController, - ignition::gazebo::System, - LinkVelocityController::ISystemConfigure, - LinkVelocityController::ISystemPreUpdate, - LinkVelocityController::ISystemPostUpdate) - -IGNITION_ADD_PLUGIN_ALIAS(LinkVelocityController, - "ignition::gazebo::systems::LinkVelocityController") diff --git a/src/systems/link_velocity_controller/LinkVelocityController.hh b/src/systems/link_velocity_controller/LinkVelocityController.hh deleted file mode 100644 index 5b794c4833..0000000000 --- a/src/systems/link_velocity_controller/LinkVelocityController.hh +++ /dev/null @@ -1,83 +0,0 @@ -/* - * Copyright (C) 2020 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_LINKVELOCITYCONTROLLER_HH_ -#define IGNITION_GAZEBO_SYSTEMS_LINKVELOCITYCONTROLLER_HH_ - -#include -#include - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace systems -{ - // Forward declaration - class LinkVelocityControllerPrivate; - - /// \brief Link velocity controller which can be attached to a model with a - /// reference to a single link. - /// - /// A new Ignition Transport topic is created to send target link velocity. - /// The topic name is - /// "/model//link///cmd_vel" - /// - /// This topic accepts ignition::msgs::Twist values representing the target - /// velocity. - /// - /// ## System Parameters - /// - /// `` The name of the link to control. Required parameter. - class IGNITION_GAZEBO_VISIBLE LinkVelocityController - : public System, - public ISystemConfigure, - public ISystemPreUpdate, - public ISystemPostUpdate - { - /// \brief Constructor - public: LinkVelocityController(); - - /// \brief Destructor - public: ~LinkVelocityController() 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; - - // Documentation inherited - public: void PostUpdate( - const UpdateInfo &_info, - const EntityComponentManager &_ecm) override; - - /// \brief Private data pointer - private: std::unique_ptr dataPtr; - }; - } -} -} -} - -#endif diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index d6e07f48e7..eb453545ec 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -35,10 +35,15 @@ using namespace systems; class ignition::gazebo::systems::VelocityControlPrivate { - /// \brief Callback for velocity subscription + /// \brief Callback for model velocity subscription /// \param[in] _msg Velocity message public: void OnCmdVel(const ignition::msgs::Twist &_msg); + /// \brief Callback for link velocity subscription + /// \param[in] _msg Velocity message + public: void OnLinkCmdVel(const ignition::msgs::Twist &_msg, + const ignition::transport::MessageInfo &_info); + /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation @@ -46,22 +51,66 @@ class ignition::gazebo::systems::VelocityControlPrivate public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Update upper link velocity. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateUpperLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Update lower link velocity. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateLowerLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Ignition communication node. public: transport::Node node; + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Upper link entity of pendulum + public: Entity upperLink{kNullEntity}; + + /// \brief Lower link entity of pendulum + public: Entity lowerLink{kNullEntity}; + + /// \brief Upper link name + public: std::string upperLinkName; + + /// \brief Lower link name + public: std::string lowerLinkName; + /// \brief Angular velocity of a model public: math::Vector3d angularVelocity{0, 0, 0}; /// \brief Linear velocity of a model public: math::Vector3d linearVelocity{0, 0, 0}; - /// \brief Model interface - public: Model model{kNullEntity}; + /// \brief Angular velocity of upper link + public: math::Vector3d upperAngularVelocity{0, 0, 0}; + + /// \brief Linear velocity of a upper link + public: math::Vector3d upperLinearVelocity{0, 0, 0}; + + /// \brief Angular velocity of lower link + public: math::Vector3d lowerAngularVelocity{0, 0, 0}; + + /// \brief Linear velocity of a lower link + public: math::Vector3d lowerLinearVelocity{0, 0, 0}; /// \brief Last target velocity requested. public: msgs::Twist targetVel; - /// \brief A mutex to protect the target velocity command. + /// \brief Last upper link velocity requested. + public: msgs::Twist upperLinkVel; + + /// \brief Last lower link velocity requested. + public: msgs::Twist lowerLinkVel; + + /// \brief A mutex to protect the velocity command. public: std::mutex mutex; }; @@ -86,15 +135,53 @@ void VelocityControl::Configure(const Entity &_entity, return; } - // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; + // Subscribe to model commands + std::string modelTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; if (_sdf->HasElement("topic")) - topic = _sdf->Get("topic"); + modelTopic = _sdf->Get("topic"); this->dataPtr->node.Subscribe( - topic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); - - ignmsg << "VelocityControl subscribing to twist messages on [" << topic << "]" + modelTopic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << modelTopic << "]" << std::endl; + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + if (!ptr->HasElement("link1_name") || !ptr->HasElement("link2_name")) + return; + + sdf::ElementPtr sdfElem = ptr->GetElement("link1_name"); + while (sdfElem) + { + this->dataPtr->upperLinkName = sdfElem->Get(); + sdfElem = sdfElem->GetNextElement("link1_name"); + } + + sdfElem = ptr->GetElement("link2_name"); + while (sdfElem) + { + this->dataPtr->lowerLinkName = sdfElem->Get(); + sdfElem = sdfElem->GetNextElement("link2_name"); + } + + // Subscribe to link commands + std::string upperLinkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + this->dataPtr->upperLinkName + "/cmd_vel"}; + this->dataPtr->node.Subscribe( + upperLinkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << upperLinkTopic << "]" + << std::endl; + + std::string lowerLinkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + this->dataPtr->lowerLinkName + "/cmd_vel"}; + this->dataPtr->node.Subscribe( + lowerLinkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << lowerLinkTopic << "]" + << std::endl; } ////////////////////////////////////////////////// @@ -116,11 +203,11 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, return; // update angular velocity of model - auto angularVel = + auto modelAngularVel = _ecm.Component( this->dataPtr->model.Entity()); - if (angularVel == nullptr) + if (modelAngularVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -128,16 +215,16 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *angularVel = + *modelAngularVel = components::AngularVelocityCmd({this->dataPtr->angularVelocity}); } // update linear velocity of model - auto linearVel = + auto modelLinearVel = _ecm.Component( this->dataPtr->model.Entity()); - if (linearVel == nullptr) + if (modelLinearVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -145,9 +232,103 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *linearVel = + *modelLinearVel = components::LinearVelocityCmd({this->dataPtr->linearVelocity}); } + + // If there are links, create link components + // If the link hasn't been identified yet, look for it + auto modelName = this->dataPtr->model.Name(_ecm); + + if (this->dataPtr->upperLinkName.empty() && + this->dataPtr->lowerLinkName.empty()) + return; + if (this->dataPtr->upperLink == kNullEntity || + this->dataPtr->lowerLink == kNullEntity) + { + Entity link = this->dataPtr->model.LinkByName( + _ecm, this->dataPtr->upperLinkName); + if (link != kNullEntity) + this->dataPtr->upperLink = link; + else + { + ignwarn << "Failed to find upper link [" << this->dataPtr->upperLinkName + << "] for model [" << modelName << "]" << std::endl; + } + link = this->dataPtr->model.LinkByName(_ecm, this->dataPtr->lowerLinkName); + if (link != kNullEntity) + this->dataPtr->lowerLink = link; + else + { + ignwarn << "Failed to find lower link [" << this->dataPtr->lowerLinkName + << "] for model [" << modelName << "]" << std::endl; + } + } + + if (this->dataPtr->upperLink == kNullEntity || this->dataPtr->lowerLink == kNullEntity) + return; + + // update upper link velocity + auto upperAngularVel = + _ecm.Component(this->dataPtr->upperLink); + + if (upperAngularVel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->upperLink, + components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity})); + } + else + { + *upperAngularVel = + components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity}); + } + + auto upperLinearVel = + _ecm.Component(this->dataPtr->upperLink); + + if (upperLinearVel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->upperLink, + components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity})); + } + else + { + *upperLinearVel = + components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity}); + } + + // update lower link velocity + auto lowerAngularVel = + _ecm.Component(this->dataPtr->lowerLink); + + if (lowerAngularVel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->lowerLink, + components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity})); + } + else + { + *lowerAngularVel = + components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity}); + } + + auto lowerLinearVel = + _ecm.Component(this->dataPtr->lowerLink); + + if (lowerLinearVel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->lowerLink, + components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity})); + } + else + { + *lowerLinearVel = + components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity}); + } } ////////////////////////////////////////////////// @@ -160,6 +341,8 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, return; this->dataPtr->UpdateVelocity(_info, _ecm); + this->dataPtr->UpdateUpperLinkVelocity(_info, _ecm); + this->dataPtr->UpdateLowerLinkVelocity(_info, _ecm); } @@ -184,6 +367,36 @@ void VelocityControlPrivate::UpdateVelocity( this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); } +////////////////////////////////////////////////// +void VelocityControlPrivate::UpdateUpperLinkVelocity( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + this->upperLinearVelocity = math::Vector3d( + this->upperLinkVel.linear().x(), + this->upperLinkVel.linear().y(), + this->upperLinkVel.linear().z()); + this->upperAngularVelocity = math::Vector3d( + this->upperLinkVel.angular().x(), + this->upperLinkVel.angular().y(), + this->upperLinkVel.angular().z()); +} + +////////////////////////////////////////////////// +void VelocityControlPrivate::UpdateLowerLinkVelocity( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + this->lowerLinearVelocity = math::Vector3d( + this->lowerLinkVel.linear().x(), + this->lowerLinkVel.linear().y(), + this->lowerLinkVel.linear().z()); + this->lowerAngularVelocity = math::Vector3d( + this->lowerLinkVel.angular().x(), + this->lowerLinkVel.angular().y(), + this->lowerLinkVel.angular().z()); +} + ////////////////////////////////////////////////// void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) { @@ -191,6 +404,20 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) this->targetVel = _msg; } +////////////////////////////////////////////////// +void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, + const transport::MessageInfo &_info) +{ + if (_info.Topic().find(this->upperLinkName) != std::string::npos) + { + this->upperLinkVel = _msg; + } + else if (_info.Topic().find(this->lowerLinkName) != std::string::npos) + { + this->lowerLinkVel = _msg; + } +} + IGNITION_ADD_PLUGIN(VelocityControl, ignition::gazebo::System, VelocityControl::ISystemConfigure, From f2f7847532e7a8a1cec779a7ccf47ecf51ef9ff9 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Tue, 22 Dec 2020 22:18:26 -0800 Subject: [PATCH 16/26] add upper link topic Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/pendulum_links.sdf | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf index c2acf85c5e..37d52269cd 100644 --- a/examples/worlds/pendulum_links.sdf +++ b/examples/worlds/pendulum_links.sdf @@ -4,6 +4,8 @@ Try sending commands: ign topic -t "/model/double_pendulum_with_base/link/lower_link/cmd_vel" -m ignition.msgs.Twist -p "angular: {x: 0.5}" + ign topic -t "/model/double_pendulum_with_base/link/upper_link/cmd_vel" -m ignition.msgs.Twist -p "linear: {x:0.2}, angular: {x: 0.5}" + --> From dec41a8c321df436a7526e830ade8fd28286f9ee Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 28 Jan 2021 14:25:09 -0800 Subject: [PATCH 17/26] accept dup link_name elems Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- examples/worlds/pendulum_links.sdf | 4 +- .../velocity_control/VelocityControl.cc | 439 +++++++++++------- 2 files changed, 274 insertions(+), 169 deletions(-) diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf index 37d52269cd..ff187802c7 100644 --- a/examples/worlds/pendulum_links.sdf +++ b/examples/worlds/pendulum_links.sdf @@ -341,8 +341,8 @@ - upper_link - lower_link + upper_link + lower_link diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index eb453545ec..44be02fa5a 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -42,7 +42,7 @@ class ignition::gazebo::systems::VelocityControlPrivate /// \brief Callback for link velocity subscription /// \param[in] _msg Velocity message public: void OnLinkCmdVel(const ignition::msgs::Twist &_msg, - const ignition::transport::MessageInfo &_info); + const ignition::transport::MessageInfo &_info); /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. @@ -51,19 +51,26 @@ class ignition::gazebo::systems::VelocityControlPrivate public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm); - /// \brief Update upper link velocity. + /// \brief Update link velocity. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation /// instance. - public: void UpdateUpperLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + public: void UpdateLinkVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm); - /// \brief Update lower link velocity. - /// \param[in] _info System update information. - /// \param[in] _ecm The EntityComponentManager of the given simulation - /// instance. - public: void UpdateLowerLinkVelocity(const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm); + // /// \brief Update upper link velocity. + // /// \param[in] _info System update information. + // /// \param[in] _ecm The EntityComponentManager of the given simulation + // /// instance. + // public: void UpdateUpperLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + // const ignition::gazebo::EntityComponentManager &_ecm); + + // /// \brief Update lower link velocity. + // /// \param[in] _info System update information. + // /// \param[in] _ecm The EntityComponentManager of the given simulation + // /// instance. + // public: void UpdateLowerLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + // const ignition::gazebo::EntityComponentManager &_ecm); /// \brief Ignition communication node. public: transport::Node node; @@ -71,47 +78,62 @@ class ignition::gazebo::systems::VelocityControlPrivate /// \brief Model interface public: Model model{kNullEntity}; - /// \brief Upper link entity of pendulum - public: Entity upperLink{kNullEntity}; - - /// \brief Lower link entity of pendulum - public: Entity lowerLink{kNullEntity}; - - /// \brief Upper link name - public: std::string upperLinkName; - - /// \brief Lower link name - public: std::string lowerLinkName; - /// \brief Angular velocity of a model public: math::Vector3d angularVelocity{0, 0, 0}; /// \brief Linear velocity of a model public: math::Vector3d linearVelocity{0, 0, 0}; - /// \brief Angular velocity of upper link - public: math::Vector3d upperAngularVelocity{0, 0, 0}; + /// \brief Last target velocity requested. + public: msgs::Twist targetVel; - /// \brief Linear velocity of a upper link - public: math::Vector3d upperLinearVelocity{0, 0, 0}; + /// \brief A mutex to protect the model velocity command. + public: std::mutex mutex; - /// \brief Angular velocity of lower link - public: math::Vector3d lowerAngularVelocity{0, 0, 0}; + /// \brief link names + public: std::vector linkNames; - /// \brief Linear velocity of a lower link - public: math::Vector3d lowerLinearVelocity{0, 0, 0}; + /// \brief Link entities in a model + public: std::map links; - /// \brief Last target velocity requested. - public: msgs::Twist targetVel; + /// \brief Angular velocities of links + public: std::map angularVelocities; - /// \brief Last upper link velocity requested. - public: msgs::Twist upperLinkVel; + /// \brief Linear velocities of links + public: std::map linearVelocities; - /// \brief Last lower link velocity requested. - public: msgs::Twist lowerLinkVel; + /// \brief all link velocites + public: std::map linkVels; - /// \brief A mutex to protect the velocity command. - public: std::mutex mutex; + // /// \brief Upper link entity of pendulum + // public: Entity upperLink{kNullEntity}; + + // /// \brief Lower link entity of pendulum + // public: Entity lowerLink{kNullEntity}; + + // /// \brief Upper link name + // public: std::string upperLinkName; + + // /// \brief Lower link name + // public: std::string lowerLinkName; + + // /// \brief Angular velocity of upper link + // public: math::Vector3d upperAngularVelocity{0, 0, 0}; + + // /// \brief Linear velocity of a upper link + // public: math::Vector3d upperLinearVelocity{0, 0, 0}; + + // /// \brief Angular velocity of lower link + // public: math::Vector3d lowerAngularVelocity{0, 0, 0}; + + // /// \brief Linear velocity of a lower link + // public: math::Vector3d lowerLinearVelocity{0, 0, 0}; + + // /// \brief Last upper link velocity requested. + // public: msgs::Twist upperLinkVel; + + // /// \brief Last lower link velocity requested. + // public: msgs::Twist lowerLinkVel; }; ////////////////////////////////////////////////// @@ -149,39 +171,42 @@ void VelocityControl::Configure(const Entity &_entity, // function and _sdf is a const shared pointer to a const sdf::Element. auto ptr = const_cast(_sdf.get()); - if (!ptr->HasElement("link1_name") || !ptr->HasElement("link2_name")) + if (!ptr->HasElement("link_name")) return; - sdf::ElementPtr sdfElem = ptr->GetElement("link1_name"); + sdf::ElementPtr sdfElem = ptr->GetElement("link_name"); while (sdfElem) { - this->dataPtr->upperLinkName = sdfElem->Get(); - sdfElem = sdfElem->GetNextElement("link1_name"); + this->dataPtr->linkNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("link_name"); } - sdfElem = ptr->GetElement("link2_name"); - while (sdfElem) - { - this->dataPtr->lowerLinkName = sdfElem->Get(); - sdfElem = sdfElem->GetNextElement("link2_name"); - } + // sdfElem = ptr->GetElement("link2_name"); + // while (sdfElem) + // { + // this->dataPtr->lowerLinkName = sdfElem->Get(); + // sdfElem = sdfElem->GetNextElement("link2_name"); + // } // Subscribe to link commands - std::string upperLinkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/link/" + this->dataPtr->upperLinkName + "/cmd_vel"}; - this->dataPtr->node.Subscribe( - upperLinkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + for (const auto &linkName : this->dataPtr->linkNames) + { + std::string linkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + linkName + "/cmd_vel"}; + this->dataPtr->node.Subscribe( + linkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); ignmsg << "VelocityControl subscribing to twist messages on [" - << upperLinkTopic << "]" + << linkTopic << "]" << std::endl; + } - std::string lowerLinkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/link/" + this->dataPtr->lowerLinkName + "/cmd_vel"}; - this->dataPtr->node.Subscribe( - lowerLinkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); - ignmsg << "VelocityControl subscribing to twist messages on [" - << lowerLinkTopic << "]" - << std::endl; + // std::string lowerLinkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + // "/link/" + this->dataPtr->lowerLinkName + "/cmd_vel"}; + // this->dataPtr->node.Subscribe( + // lowerLinkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + // ignmsg << "VelocityControl subscribing to twist messages on [" + // << lowerLinkTopic << "]" + // << std::endl; } ////////////////////////////////////////////////// @@ -240,95 +265,148 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, // If the link hasn't been identified yet, look for it auto modelName = this->dataPtr->model.Name(_ecm); - if (this->dataPtr->upperLinkName.empty() && - this->dataPtr->lowerLinkName.empty()) - return; - if (this->dataPtr->upperLink == kNullEntity || - this->dataPtr->lowerLink == kNullEntity) + if (this->dataPtr->linkNames.empty()) + return; + + for (const auto &linkName : this->dataPtr->linkNames) { - Entity link = this->dataPtr->model.LinkByName( - _ecm, this->dataPtr->upperLinkName); - if (link != kNullEntity) - this->dataPtr->upperLink = link; - else - { - ignwarn << "Failed to find upper link [" << this->dataPtr->upperLinkName - << "] for model [" << modelName << "]" << std::endl; - } - link = this->dataPtr->model.LinkByName(_ecm, this->dataPtr->lowerLinkName); - if (link != kNullEntity) - this->dataPtr->lowerLink = link; - else + if (this->dataPtr->links.find(linkName) == this->dataPtr->links.end()) { - ignwarn << "Failed to find lower link [" << this->dataPtr->lowerLinkName + Entity link = this->dataPtr->model.LinkByName(_ecm, linkName); + if (link != kNullEntity) + this->dataPtr->links.insert({linkName, link}); + else + { + ignwarn << "Failed to find link [" << linkName << "] for model [" << modelName << "]" << std::endl; + } } } - - if (this->dataPtr->upperLink == kNullEntity || this->dataPtr->lowerLink == kNullEntity) + if (this->dataPtr->links.empty()) return; - // update upper link velocity - auto upperAngularVel = - _ecm.Component(this->dataPtr->upperLink); - - if (upperAngularVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->upperLink, - components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity})); - } - else - { - *upperAngularVel = - components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity}); - } - - auto upperLinearVel = - _ecm.Component(this->dataPtr->upperLink); - - if (upperLinearVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->upperLink, - components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity})); - } - else + // update link velocities + for (const auto& [linkName, link] : this->dataPtr->links) { - *upperLinearVel = - components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity}); - } - - // update lower link velocity - auto lowerAngularVel = - _ecm.Component(this->dataPtr->lowerLink); + // update angular velocity + auto linkAngularVel = _ecm.Component(link); + auto it = this->dataPtr->angularVelocities.find(linkName); + if (it != this->dataPtr->end()) + { + if (linkAngularVel == nullptr) + _ecm.CreateComponent(link, components::AngularVelocityCmd({it->second})); + else + *linkAngularVel = components::AngularVelocityCmd(it->second); + } + else + { + ignwarn << "No angular velocity found for link [" << linkName << "]" << std:;endl; + } - if (lowerAngularVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->lowerLink, - components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity})); - } - else - { - *lowerAngularVel = - components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity}); + // update linear velocity + auto linkLinearVel = _ecm.Component(link); + auto it = this->dataPtr->linearVelocities.find(linkName); + if (it != this->dataPtr->end()) + { + if (linkLinearVel == nullptr) + _ecm.CreateComponent(link, components::LinearVelocityCmd({it->second})); + else + *linkLinearVel = components::LinearVelocityCmd({it->second}); + } + else + { + ignwarn << "No linear velocity found for link [" << linkName << "]" << std:;endl; + } } + // if (this->dataPtr->upperLinkName.empty() && + // this->dataPtr->lowerLinkName.empty()) + // return; + // if (this->dataPtr->upperLink == kNullEntity || + // this->dataPtr->lowerLink == kNullEntity) + // { + // Entity link = this->dataPtr->model.LinkByName( + // _ecm, this->dataPtr->upperLinkName); + // if (link != kNullEntity) + // this->dataPtr->upperLink = link; + // else + // { + // ignwarn << "Failed to find upper link [" << this->dataPtr->upperLinkName + // << "] for model [" << modelName << "]" << std::endl; + // } + // link = this->dataPtr->model.LinkByName(_ecm, this->dataPtr->lowerLinkName); + // if (link != kNullEntity) + // this->dataPtr->lowerLink = link; + // else + // { + // ignwarn << "Failed to find lower link [" << this->dataPtr->lowerLinkName + // << "] for model [" << modelName << "]" << std::endl; + // } + // } + + // if (this->dataPtr->upperLink == kNullEntity || this->dataPtr->lowerLink == kNullEntity) + // return; - auto lowerLinearVel = - _ecm.Component(this->dataPtr->lowerLink); - - if (lowerLinearVel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->lowerLink, - components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity})); - } - else - { - *lowerLinearVel = - components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity}); - } + // update upper link velocity + // auto upperAngularVel = + // _ecm.Component(this->dataPtr->upperLink); + + // if (upperAngularVel == nullptr) + // { + // _ecm.CreateComponent( + // this->dataPtr->upperLink, + // components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity})); + // } + // else + // { + // *upperAngularVel = + // components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity}); + // } + + // auto upperLinearVel = + // _ecm.Component(this->dataPtr->upperLink); + + // if (upperLinearVel == nullptr) + // { + // _ecm.CreateComponent( + // this->dataPtr->upperLink, + // components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity})); + // } + // else + // { + // *upperLinearVel = + // components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity}); + // } + + // // update lower link velocity + // auto lowerAngularVel = + // _ecm.Component(this->dataPtr->lowerLink); + + // if (lowerAngularVel == nullptr) + // { + // _ecm.CreateComponent( + // this->dataPtr->lowerLink, + // components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity})); + // } + // else + // { + // *lowerAngularVel = + // components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity}); + // } + + // auto lowerLinearVel = + // _ecm.Component(this->dataPtr->lowerLink); + + // if (lowerLinearVel == nullptr) + // { + // _ecm.CreateComponent( + // this->dataPtr->lowerLink, + // components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity})); + // } + // else + // { + // *lowerLinearVel = + // components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity}); + // } } ////////////////////////////////////////////////// @@ -340,9 +418,12 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, if (_info.paused) return; + // update model velocities this->dataPtr->UpdateVelocity(_info, _ecm); - this->dataPtr->UpdateUpperLinkVelocity(_info, _ecm); - this->dataPtr->UpdateLowerLinkVelocity(_info, _ecm); + // update link velocities + this->dataPtr->UpdateLinkVelocity(_info, _ecm); + // this->dataPtr->UpdateUpperLinkVelocity(_info, _ecm); + // this->dataPtr->UpdateLowerLinkVelocity(_info, _ecm); } @@ -367,34 +448,50 @@ void VelocityControlPrivate::UpdateVelocity( this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); } -////////////////////////////////////////////////// -void VelocityControlPrivate::UpdateUpperLinkVelocity( - const ignition::gazebo::UpdateInfo &/*_info*/, - const ignition::gazebo::EntityComponentManager &/*_ecm*/) -{ - this->upperLinearVelocity = math::Vector3d( - this->upperLinkVel.linear().x(), - this->upperLinkVel.linear().y(), - this->upperLinkVel.linear().z()); - this->upperAngularVelocity = math::Vector3d( - this->upperLinkVel.angular().x(), - this->upperLinkVel.angular().y(), - this->upperLinkVel.angular().z()); +// ////////////////////////////////////////////////// +// void VelocityControlPrivate::UpdateUpperLinkVelocity( +// const ignition::gazebo::UpdateInfo &/*_info*/, +// const ignition::gazebo::EntityComponentManager &/*_ecm*/) +// { +// this->upperLinearVelocity = math::Vector3d( +// this->upperLinkVel.linear().x(), +// this->upperLinkVel.linear().y(), +// this->upperLinkVel.linear().z()); +// this->upperAngularVelocity = math::Vector3d( +// this->upperLinkVel.angular().x(), +// this->upperLinkVel.angular().y(), +// this->upperLinkVel.angular().z()); +// } + +// ////////////////////////////////////////////////// +// void VelocityControlPrivate::UpdateLowerLinkVelocity( +// const ignition::gazebo::UpdateInfo &/*_info*/, +// const ignition::gazebo::EntityComponentManager &/*_ecm*/) +// { +// this->lowerLinearVelocity = math::Vector3d( +// this->lowerLinkVel.linear().x(), +// this->lowerLinkVel.linear().y(), +// this->lowerLinkVel.linear().z()); +// this->lowerAngularVelocity = math::Vector3d( +// this->lowerLinkVel.angular().x(), +// this->lowerLinkVel.angular().y(), +// this->lowerLinkVel.angular().z()); } ////////////////////////////////////////////////// -void VelocityControlPrivate::UpdateLowerLinkVelocity( +void VelocityControlPrivate::UpdateLinkVelocity( const ignition::gazebo::UpdateInfo &/*_info*/, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { - this->lowerLinearVelocity = math::Vector3d( - this->lowerLinkVel.linear().x(), - this->lowerLinkVel.linear().y(), - this->lowerLinkVel.linear().z()); - this->lowerAngularVelocity = math::Vector3d( - this->lowerLinkVel.angular().x(), - this->lowerLinkVel.angular().y(), - this->lowerLinkVel.angular().z()); + IGN_PROFILE("VeocityControl::UpdateLinkVelocity"); + + for (const auto& [linkName, msg] : this->linkVels) + { + auto linearVel = math::Vector3d(msg.linear().x(), msg.linear().y(), msg.linear().z()); + auto angularVel = math::Vector3d(msg.angular().x(), msg.angular().y(), msg.angular().z()); + this->dataPtr->linearVelocities.insert({linkName, linearVel}); + this->dataPtr->angularVelocities.insert({linkName, angularVel}); + } } ////////////////////////////////////////////////// @@ -408,14 +505,22 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, const transport::MessageInfo &_info) { - if (_info.Topic().find(this->upperLinkName) != std::string::npos) + for (const auto &linkName : this->dataPtr->linkNames) { - this->upperLinkVel = _msg; - } - else if (_info.Topic().find(this->lowerLinkName) != std::string::npos) - { - this->lowerLinkVel = _msg; + if (_info.Topic().find(linkName) != std::string::npos) + { + this->linkVels.insert({linkName, _msg}); + } } + + // if (_info.Topic().find(this->upperLinkName) != std::string::npos) + // { + // this->upperLinkVel = _msg; + // } + // else if (_info.Topic().find(this->lowerLinkName) != std::string::npos) + // { + // this->lowerLinkVel = _msg; + // } } IGNITION_ADD_PLUGIN(VelocityControl, From 83bf607365a93cac4434aa66136e28e8bcfd183a Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 28 Jan 2021 14:32:47 -0800 Subject: [PATCH 18/26] clean code Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- .../velocity_control/VelocityControl.cc | 191 +----------------- 1 file changed, 1 insertion(+), 190 deletions(-) diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 44be02fa5a..49d5267578 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -58,20 +59,6 @@ class ignition::gazebo::systems::VelocityControlPrivate public: void UpdateLinkVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm); - // /// \brief Update upper link velocity. - // /// \param[in] _info System update information. - // /// \param[in] _ecm The EntityComponentManager of the given simulation - // /// instance. - // public: void UpdateUpperLinkVelocity(const ignition::gazebo::UpdateInfo &_info, - // const ignition::gazebo::EntityComponentManager &_ecm); - - // /// \brief Update lower link velocity. - // /// \param[in] _info System update information. - // /// \param[in] _ecm The EntityComponentManager of the given simulation - // /// instance. - // public: void UpdateLowerLinkVelocity(const ignition::gazebo::UpdateInfo &_info, - // const ignition::gazebo::EntityComponentManager &_ecm); - /// \brief Ignition communication node. public: transport::Node node; @@ -104,36 +91,6 @@ class ignition::gazebo::systems::VelocityControlPrivate /// \brief all link velocites public: std::map linkVels; - - // /// \brief Upper link entity of pendulum - // public: Entity upperLink{kNullEntity}; - - // /// \brief Lower link entity of pendulum - // public: Entity lowerLink{kNullEntity}; - - // /// \brief Upper link name - // public: std::string upperLinkName; - - // /// \brief Lower link name - // public: std::string lowerLinkName; - - // /// \brief Angular velocity of upper link - // public: math::Vector3d upperAngularVelocity{0, 0, 0}; - - // /// \brief Linear velocity of a upper link - // public: math::Vector3d upperLinearVelocity{0, 0, 0}; - - // /// \brief Angular velocity of lower link - // public: math::Vector3d lowerAngularVelocity{0, 0, 0}; - - // /// \brief Linear velocity of a lower link - // public: math::Vector3d lowerLinearVelocity{0, 0, 0}; - - // /// \brief Last upper link velocity requested. - // public: msgs::Twist upperLinkVel; - - // /// \brief Last lower link velocity requested. - // public: msgs::Twist lowerLinkVel; }; ////////////////////////////////////////////////// @@ -181,13 +138,6 @@ void VelocityControl::Configure(const Entity &_entity, sdfElem = sdfElem->GetNextElement("link_name"); } - // sdfElem = ptr->GetElement("link2_name"); - // while (sdfElem) - // { - // this->dataPtr->lowerLinkName = sdfElem->Get(); - // sdfElem = sdfElem->GetNextElement("link2_name"); - // } - // Subscribe to link commands for (const auto &linkName : this->dataPtr->linkNames) { @@ -199,14 +149,6 @@ void VelocityControl::Configure(const Entity &_entity, << linkTopic << "]" << std::endl; } - - // std::string lowerLinkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + - // "/link/" + this->dataPtr->lowerLinkName + "/cmd_vel"}; - // this->dataPtr->node.Subscribe( - // lowerLinkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); - // ignmsg << "VelocityControl subscribing to twist messages on [" - // << lowerLinkTopic << "]" - // << std::endl; } ////////////////////////////////////////////////// @@ -318,95 +260,6 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignwarn << "No linear velocity found for link [" << linkName << "]" << std:;endl; } } - // if (this->dataPtr->upperLinkName.empty() && - // this->dataPtr->lowerLinkName.empty()) - // return; - // if (this->dataPtr->upperLink == kNullEntity || - // this->dataPtr->lowerLink == kNullEntity) - // { - // Entity link = this->dataPtr->model.LinkByName( - // _ecm, this->dataPtr->upperLinkName); - // if (link != kNullEntity) - // this->dataPtr->upperLink = link; - // else - // { - // ignwarn << "Failed to find upper link [" << this->dataPtr->upperLinkName - // << "] for model [" << modelName << "]" << std::endl; - // } - // link = this->dataPtr->model.LinkByName(_ecm, this->dataPtr->lowerLinkName); - // if (link != kNullEntity) - // this->dataPtr->lowerLink = link; - // else - // { - // ignwarn << "Failed to find lower link [" << this->dataPtr->lowerLinkName - // << "] for model [" << modelName << "]" << std::endl; - // } - // } - - // if (this->dataPtr->upperLink == kNullEntity || this->dataPtr->lowerLink == kNullEntity) - // return; - - // update upper link velocity - // auto upperAngularVel = - // _ecm.Component(this->dataPtr->upperLink); - - // if (upperAngularVel == nullptr) - // { - // _ecm.CreateComponent( - // this->dataPtr->upperLink, - // components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity})); - // } - // else - // { - // *upperAngularVel = - // components::AngularVelocityCmd({this->dataPtr->upperAngularVelocity}); - // } - - // auto upperLinearVel = - // _ecm.Component(this->dataPtr->upperLink); - - // if (upperLinearVel == nullptr) - // { - // _ecm.CreateComponent( - // this->dataPtr->upperLink, - // components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity})); - // } - // else - // { - // *upperLinearVel = - // components::LinearVelocityCmd({this->dataPtr->upperLinearVelocity}); - // } - - // // update lower link velocity - // auto lowerAngularVel = - // _ecm.Component(this->dataPtr->lowerLink); - - // if (lowerAngularVel == nullptr) - // { - // _ecm.CreateComponent( - // this->dataPtr->lowerLink, - // components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity})); - // } - // else - // { - // *lowerAngularVel = - // components::AngularVelocityCmd({this->dataPtr->lowerAngularVelocity}); - // } - - // auto lowerLinearVel = - // _ecm.Component(this->dataPtr->lowerLink); - - // if (lowerLinearVel == nullptr) - // { - // _ecm.CreateComponent( - // this->dataPtr->lowerLink, - // components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity})); - // } - // else - // { - // *lowerLinearVel = - // components::LinearVelocityCmd({this->dataPtr->lowerLinearVelocity}); - // } } ////////////////////////////////////////////////// @@ -422,11 +275,8 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, this->dataPtr->UpdateVelocity(_info, _ecm); // update link velocities this->dataPtr->UpdateLinkVelocity(_info, _ecm); - // this->dataPtr->UpdateUpperLinkVelocity(_info, _ecm); - // this->dataPtr->UpdateLowerLinkVelocity(_info, _ecm); } - ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateVelocity( const ignition::gazebo::UpdateInfo &/*_info*/, @@ -448,36 +298,6 @@ void VelocityControlPrivate::UpdateVelocity( this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); } -// ////////////////////////////////////////////////// -// void VelocityControlPrivate::UpdateUpperLinkVelocity( -// const ignition::gazebo::UpdateInfo &/*_info*/, -// const ignition::gazebo::EntityComponentManager &/*_ecm*/) -// { -// this->upperLinearVelocity = math::Vector3d( -// this->upperLinkVel.linear().x(), -// this->upperLinkVel.linear().y(), -// this->upperLinkVel.linear().z()); -// this->upperAngularVelocity = math::Vector3d( -// this->upperLinkVel.angular().x(), -// this->upperLinkVel.angular().y(), -// this->upperLinkVel.angular().z()); -// } - -// ////////////////////////////////////////////////// -// void VelocityControlPrivate::UpdateLowerLinkVelocity( -// const ignition::gazebo::UpdateInfo &/*_info*/, -// const ignition::gazebo::EntityComponentManager &/*_ecm*/) -// { -// this->lowerLinearVelocity = math::Vector3d( -// this->lowerLinkVel.linear().x(), -// this->lowerLinkVel.linear().y(), -// this->lowerLinkVel.linear().z()); -// this->lowerAngularVelocity = math::Vector3d( -// this->lowerLinkVel.angular().x(), -// this->lowerLinkVel.angular().y(), -// this->lowerLinkVel.angular().z()); -} - ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateLinkVelocity( const ignition::gazebo::UpdateInfo &/*_info*/, @@ -512,15 +332,6 @@ void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, this->linkVels.insert({linkName, _msg}); } } - - // if (_info.Topic().find(this->upperLinkName) != std::string::npos) - // { - // this->upperLinkVel = _msg; - // } - // else if (_info.Topic().find(this->lowerLinkName) != std::string::npos) - // { - // this->lowerLinkVel = _msg; - // } } IGNITION_ADD_PLUGIN(VelocityControl, From a39f99a69663d5f858f2b63b24add0f602ccb622 Mon Sep 17 00:00:00 2001 From: claireyywang <22240514+claireyywang@users.noreply.github.com> Date: Thu, 28 Jan 2021 15:20:31 -0800 Subject: [PATCH 19/26] correct type Signed-off-by: claireyywang <22240514+claireyywang@users.noreply.github.com> --- src/systems/velocity_control/VelocityControl.cc | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 49d5267578..c14347c6e7 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -233,7 +233,7 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, // update angular velocity auto linkAngularVel = _ecm.Component(link); auto it = this->dataPtr->angularVelocities.find(linkName); - if (it != this->dataPtr->end()) + if (it != this->dataPtr->angularVelocities.end()) { if (linkAngularVel == nullptr) _ecm.CreateComponent(link, components::AngularVelocityCmd({it->second})); @@ -242,13 +242,13 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - ignwarn << "No angular velocity found for link [" << linkName << "]" << std:;endl; + ignwarn << "No angular velocity found for link [" << linkName << "]" << std::endl; } // update linear velocity auto linkLinearVel = _ecm.Component(link); - auto it = this->dataPtr->linearVelocities.find(linkName); - if (it != this->dataPtr->end()) + it = this->dataPtr->linearVelocities.find(linkName); + if (it != this->dataPtr->linearVelocities.end()) { if (linkLinearVel == nullptr) _ecm.CreateComponent(link, components::LinearVelocityCmd({it->second})); @@ -257,7 +257,7 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - ignwarn << "No linear velocity found for link [" << linkName << "]" << std:;endl; + ignwarn << "No linear velocity found for link [" << linkName << "]" << std::endl; } } } @@ -309,8 +309,8 @@ void VelocityControlPrivate::UpdateLinkVelocity( { auto linearVel = math::Vector3d(msg.linear().x(), msg.linear().y(), msg.linear().z()); auto angularVel = math::Vector3d(msg.angular().x(), msg.angular().y(), msg.angular().z()); - this->dataPtr->linearVelocities.insert({linkName, linearVel}); - this->dataPtr->angularVelocities.insert({linkName, angularVel}); + this->linearVelocities.insert({linkName, linearVel}); + this->angularVelocities.insert({linkName, angularVel}); } } @@ -325,7 +325,7 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, const transport::MessageInfo &_info) { - for (const auto &linkName : this->dataPtr->linkNames) + for (const auto &linkName : this->linkNames) { if (_info.Topic().find(linkName) != std::string::npos) { From 546658bdd2b263c747422edba09cc27ac9ffb019 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 19 Feb 2021 15:02:58 -0800 Subject: [PATCH 20/26] fix frame, comments Signed-off-by: Ian Chen --- examples/worlds/pendulum_links.sdf | 3 +- src/systems/physics/Physics.cc | 29 +++--- .../velocity_control/VelocityControl.cc | 97 ++++++++++++------- 3 files changed, 80 insertions(+), 49 deletions(-) diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf index ff187802c7..b212918a3a 100644 --- a/examples/worlds/pendulum_links.sdf +++ b/examples/worlds/pendulum_links.sdf @@ -341,9 +341,10 @@ + base upper_link lower_link - \ No newline at end of file + diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index c506c5f3f1..3abd103389 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -1641,7 +1641,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); - // Update link angular velocity + // Update link angular velocity _ecm.Each( [&](const Entity &_entity, const components::Link *, const components::AngularVelocityCmd *_angularVelocityCmd) @@ -1652,10 +1652,7 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) auto freeGroup = linkIt->second->FindFreeGroup(); if (!freeGroup) return true; - const components::Pose *poseComp = - _ecm.Component(_entity); - math::Vector3d worldAngularVel = poseComp->Data().Rot() * - _angularVelocityCmd->Data(); + auto worldAngularVelFeature = entityCast(_entity, freeGroup, this->entityWorldVelocityCommandMap); if (!worldAngularVelFeature) @@ -1671,7 +1668,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) } return true; } - + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldAngularVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _angularVelocityCmd->Data(); worldAngularVelFeature->SetWorldAngularVelocity( math::eigen3::convert(worldAngularVel)); @@ -1691,11 +1695,6 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) if (!freeGroup) return true; - const components::Pose *poseComp = - _ecm.Component(_entity); - math::Vector3d worldLinearVel = poseComp->Data().Rot() * - _linearVelocityCmd->Data(); - auto worldLinearVelFeature = entityCast(_entity, freeGroup, this->entityWorldVelocityCommandMap); if (!worldLinearVelFeature) @@ -1712,6 +1711,14 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; } + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldLinearVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _linearVelocityCmd->Data(); worldLinearVelFeature->SetWorldLinearVelocity( math::eigen3::convert(worldLinearVel)); diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index c14347c6e7..5c09063ddc 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -81,16 +81,16 @@ class ignition::gazebo::systems::VelocityControlPrivate public: std::vector linkNames; /// \brief Link entities in a model - public: std::map links; + public: std::unordered_map links; /// \brief Angular velocities of links - public: std::map angularVelocities; + public: std::unordered_map angularVelocities; /// \brief Linear velocities of links - public: std::map linearVelocities; + public: std::unordered_map linearVelocities; /// \brief all link velocites - public: std::map linkVels; + public: std::unordered_map linkVels; }; ////////////////////////////////////////////////// @@ -144,7 +144,7 @@ void VelocityControl::Configure(const Entity &_entity, std::string linkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/link/" + linkName + "/cmd_vel"}; this->dataPtr->node.Subscribe( - linkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + linkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); ignmsg << "VelocityControl subscribing to twist messages on [" << linkTopic << "]" << std::endl; @@ -210,54 +210,72 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, if (this->dataPtr->linkNames.empty()) return; - for (const auto &linkName : this->dataPtr->linkNames) + + // find all the link entity ids + if (this->dataPtr->links.size() != this->dataPtr->linkNames.size()) { - if (this->dataPtr->links.find(linkName) == this->dataPtr->links.end()) + for (const auto &linkName : this->dataPtr->linkNames) { - Entity link = this->dataPtr->model.LinkByName(_ecm, linkName); - if (link != kNullEntity) - this->dataPtr->links.insert({linkName, link}); - else + if (this->dataPtr->links.find(linkName) == this->dataPtr->links.end()) { - ignwarn << "Failed to find link [" << linkName - << "] for model [" << modelName << "]" << std::endl; + Entity link = this->dataPtr->model.LinkByName(_ecm, linkName); + if (link != kNullEntity) + this->dataPtr->links.insert({linkName, link}); + else + { + ignwarn << "Failed to find link [" << linkName + << "] for model [" << modelName << "]" << std::endl; + } } } } - if (this->dataPtr->links.empty()) - return; // update link velocities - for (const auto& [linkName, link] : this->dataPtr->links) + for (const auto& [linkName, angularVel] : this->dataPtr->angularVelocities) { - // update angular velocity - auto linkAngularVel = _ecm.Component(link); - auto it = this->dataPtr->angularVelocities.find(linkName); - if (it != this->dataPtr->angularVelocities.end()) + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) { - if (linkAngularVel == nullptr) - _ecm.CreateComponent(link, components::AngularVelocityCmd({it->second})); + auto linkAngularVelComp = + _ecm.Component(it->second); + if (!linkAngularVelComp) + { + _ecm.CreateComponent(it->second, + components::AngularVelocityCmd({angularVel})); + } else - *linkAngularVel = components::AngularVelocityCmd(it->second); + { + *linkAngularVelComp = components::AngularVelocityCmd(angularVel); + } } else { - ignwarn << "No angular velocity found for link [" << linkName << "]" << std::endl; + ignwarn << "No link found for angular velocity cmd [" + << linkName << "]" << std::endl; } + } - // update linear velocity - auto linkLinearVel = _ecm.Component(link); - it = this->dataPtr->linearVelocities.find(linkName); - if (it != this->dataPtr->linearVelocities.end()) + for (const auto& [linkName, linearVel] : this->dataPtr->linearVelocities) + { + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) { - if (linkLinearVel == nullptr) - _ecm.CreateComponent(link, components::LinearVelocityCmd({it->second})); + auto linkLinearVelComp = + _ecm.Component(it->second); + if (!linkLinearVelComp) + { + _ecm.CreateComponent(it->second, + components::LinearVelocityCmd({linearVel})); + } else - *linkLinearVel = components::LinearVelocityCmd({it->second}); + { + *linkLinearVelComp = components::LinearVelocityCmd(linearVel); + } } else { - ignwarn << "No linear velocity found for link [" << linkName << "]" << std::endl; + ignwarn << "No link found for linear velocity cmd [" + << linkName << "]" << std::endl; } } } @@ -305,13 +323,16 @@ void VelocityControlPrivate::UpdateLinkVelocity( { IGN_PROFILE("VeocityControl::UpdateLinkVelocity"); + std::lock_guard lock(this->mutex); for (const auto& [linkName, msg] : this->linkVels) { - auto linearVel = math::Vector3d(msg.linear().x(), msg.linear().y(), msg.linear().z()); - auto angularVel = math::Vector3d(msg.angular().x(), msg.angular().y(), msg.angular().z()); - this->linearVelocities.insert({linkName, linearVel}); - this->angularVelocities.insert({linkName, angularVel}); + auto linearVel = msgs::Convert(msg.linear()); + auto angularVel = msgs::Convert(msg.angular()); + this->linearVelocities[linkName] = linearVel; + this->angularVelocities[linkName] = angularVel; + std::cerr << "got new link vel " << linkName << " " << linearVel << " | " << angularVel << std::endl; } + this->linkVels.clear(); } ////////////////////////////////////////////////// @@ -325,11 +346,13 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, const transport::MessageInfo &_info) { + std::lock_guard lock(this->mutex); for (const auto &linkName : this->linkNames) { - if (_info.Topic().find(linkName) != std::string::npos) + if (_info.Topic().find("/" + linkName + "/cmd_vel") != std::string::npos) { this->linkVels.insert({linkName, _msg}); + break; } } } From 7c2e603a58e1535b741e27ee7e338d36aa45db04 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 19 Feb 2021 15:08:27 -0800 Subject: [PATCH 21/26] tidyup Signed-off-by: Ian Chen --- src/systems/velocity_control/VelocityControl.cc | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 5c09063ddc..30989bb7d4 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -310,10 +310,8 @@ void VelocityControlPrivate::UpdateVelocity( angVel = this->targetVel.angular().z(); } - this->linearVelocity = math::Vector3d( - linVel, this->targetVel.linear().y(), this->targetVel.linear().z()); - this->angularVelocity = math::Vector3d( - this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); + this->linearVelocity = msgs::Convert(this->targetVel.linear()); + this->angularVelocity = msgs::Convert(this->targetVel.angular()); } ////////////////////////////////////////////////// @@ -330,7 +328,6 @@ void VelocityControlPrivate::UpdateLinkVelocity( auto angularVel = msgs::Convert(msg.angular()); this->linearVelocities[linkName] = linearVel; this->angularVelocities[linkName] = angularVel; - std::cerr << "got new link vel " << linkName << " " << linearVel << " | " << angularVel << std::endl; } this->linkVels.clear(); } From e3cd459b387d0f00eae5e7c2f7895d475de3db52 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Feb 2021 12:38:16 -0800 Subject: [PATCH 22/26] indentation, comments Signed-off-by: Ian Chen --- examples/worlds/pendulum_links.sdf | 642 +++++++++--------- .../velocity_control/VelocityControl.cc | 18 +- 2 files changed, 327 insertions(+), 333 deletions(-) diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf index b212918a3a..ca63f87d2c 100644 --- a/examples/worlds/pendulum_links.sdf +++ b/examples/worlds/pendulum_links.sdf @@ -10,341 +10,341 @@ - - - 0.001 - 1.0 - - - - - - - + + + 0.001 + 1.0 + + + + + + + - + - - - - 3D View - false - docked - + + + + 3D View + false + docked + - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + - - - - World control - false - false - 72 - 121 - 1 + + + + World control + false + false + 72 + 121 + 1 - floating - - - - - + floating + + + + + - true - true - true - /world/velocity_control/control - /world/velocity_control/stats - + true + true + true + /world/velocity_control/control + /world/velocity_control/stats + - - - - World stats - false - false - 110 - 290 - 1 + + + + World stats + false + false + 110 + 290 + 1 - floating - - - - - + floating + + + + + - true - true - true - true - /world/velocity_control/stats + true + true + true + true + /world/velocity_control/stats - + - - - + + + - + - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + - - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + - - - - 100 - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 0.01 0 0 0 - - - 0.8 - 0.02 - - - - - -0.275 0 1.1 0 0 0 - - - 0.2 0.2 2.2 - - - - - - - 0 0 2.1 -1.5708 0 0 - 0 - - 0 0 0.5 0 0 0 - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - -0.05 0 0 0 1.5708 0 - - - 0.1 - 0.3 - - - - - 0 0 1.0 0 1.5708 0 - - - 0.1 - 0.2 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - 0.25 1.0 2.1 -2 0 0 - 0 - - 0 0 0.5 0 0 0 - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - 0 0 0 0 1.5708 0 - - - 0.08 - 0.3 - - - - - 0 0 0.5 0 0 0 - - - 0.1 - 0.9 - - - - - - - base - upper_link - - 1.0 0 0 - - - - - upper_link - lower_link - - 1.0 0 0 - - + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + - - base - upper_link - lower_link - - - + + base + upper_link + lower_link + + + diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 30989bb7d4..de38675bdd 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -77,7 +77,7 @@ class ignition::gazebo::systems::VelocityControlPrivate /// \brief A mutex to protect the model velocity command. public: std::mutex mutex; - /// \brief link names + /// \brief Link names public: std::vector linkNames; /// \brief Link entities in a model @@ -89,7 +89,7 @@ class ignition::gazebo::systems::VelocityControlPrivate /// \brief Linear velocities of links public: std::unordered_map linearVelocities; - /// \brief all link velocites + /// \brief All link velocites public: std::unordered_map linkVels; }; @@ -118,6 +118,7 @@ void VelocityControl::Configure(const Entity &_entity, std::string modelTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; if (_sdf->HasElement("topic")) modelTopic = _sdf->Get("topic"); + modelTopic = transport::TopicUtils::AsValidTopic(modelTopic); this->dataPtr->node.Subscribe( modelTopic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); ignmsg << "VelocityControl subscribing to twist messages on [" @@ -143,6 +144,7 @@ void VelocityControl::Configure(const Entity &_entity, { std::string linkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/link/" + linkName + "/cmd_vel"}; + linkTopic = transport::TopicUtils::AsValidTopic(linkTopic); this->dataPtr->node.Subscribe( linkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); ignmsg << "VelocityControl subscribing to twist messages on [" @@ -210,7 +212,6 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, if (this->dataPtr->linkNames.empty()) return; - // find all the link entity ids if (this->dataPtr->links.size() != this->dataPtr->linkNames.size()) { @@ -302,14 +303,7 @@ void VelocityControlPrivate::UpdateVelocity( { IGN_PROFILE("VeocityControl::UpdateVelocity"); - double linVel; - double angVel; - { - std::lock_guard lock(this->mutex); - linVel = this->targetVel.linear().x(); - angVel = this->targetVel.angular().z(); - } - + std::lock_guard lock(this->mutex); this->linearVelocity = msgs::Convert(this->targetVel.linear()); this->angularVelocity = msgs::Convert(this->targetVel.angular()); } @@ -319,7 +313,7 @@ void VelocityControlPrivate::UpdateLinkVelocity( const ignition::gazebo::UpdateInfo &/*_info*/, const ignition::gazebo::EntityComponentManager &/*_ecm*/) { - IGN_PROFILE("VeocityControl::UpdateLinkVelocity"); + IGN_PROFILE("VelocityControl::UpdateLinkVelocity"); std::lock_guard lock(this->mutex); for (const auto& [linkName, msg] : this->linkVels) From 7c17416757eb8f9aa7053c4517ff8e4b2f700e0c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 22 Feb 2021 12:40:52 -0800 Subject: [PATCH 23/26] fix world files Signed-off-by: Ian Chen --- examples/worlds/pendulum_links.sdf | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf index ca63f87d2c..9165bc0178 100644 --- a/examples/worlds/pendulum_links.sdf +++ b/examples/worlds/pendulum_links.sdf @@ -67,7 +67,7 @@ true /world/velocity_control/control /world/velocity_control/stats - + From c018610dba167fbf7cd1e0df6a9d8e6c3f16958c Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 1 Apr 2021 15:54:32 -0700 Subject: [PATCH 24/26] update example world, codecheck Signed-off-by: Ian Chen --- examples/worlds/pendulum_links.sdf | 73 +------------------ .../velocity_control/VelocityControl.cc | 6 +- 2 files changed, 6 insertions(+), 73 deletions(-) diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf index 9165bc0178..2f62bab25c 100644 --- a/examples/worlds/pendulum_links.sdf +++ b/examples/worlds/pendulum_links.sdf @@ -1,3 +1,4 @@ + - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/velocity_control/control - /world/velocity_control/stats - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/velocity_control/stats - - - - - - - - - true 0 0 10 0 0 0 diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index de38675bdd..7ed361c627 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -115,10 +117,12 @@ void VelocityControl::Configure(const Entity &_entity, } // Subscribe to model commands - std::string modelTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; + std::string modelTopic{ + "/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; if (_sdf->HasElement("topic")) modelTopic = _sdf->Get("topic"); modelTopic = transport::TopicUtils::AsValidTopic(modelTopic); + this->dataPtr->node.Subscribe( modelTopic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); ignmsg << "VelocityControl subscribing to twist messages on [" From b9e67792b3765c97fe3a8eea8d8ce2f7926a5e72 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 15 Apr 2021 14:35:45 -0700 Subject: [PATCH 25/26] add test, clear vel cmds Signed-off-by: Ian Chen --- src/systems/physics/Physics.cc | 14 +++ test/integration/velocity_control_system.cc | 113 ++++++++++++++++++++ test/worlds/velocity_control.sdf | 1 + 3 files changed, 128 insertions(+) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 4e108d20ff..72ccc67339 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2132,6 +2132,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm) return true; }); + _ecm.Each( + [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + + _ecm.Each( + [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + // Update joint positions _ecm.Each( [&](const Entity &_entity, components::Joint *, diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index df1354b924..b34c102bd0 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -21,6 +21,7 @@ #include #include +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Pose.hh" @@ -126,6 +127,110 @@ class VelocityControlTest : public ::testing::TestWithParam EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i; } } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestPublishLinkCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + // currently only tpe supports link velocity cmds + serverConfig.SetPhysicsEngine("ignition-physics-tpe-plugin"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle link poses + test::Relay testSystem; + + std::vector modelPoses; + std::vector linkPoses; + testSystem.OnPostUpdate([&linkPoses, &modelPoses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto modelId = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle_blue")); + EXPECT_NE(kNullEntity, modelId); + + auto modelPoseComp = _ecm.Component(modelId); + ASSERT_NE(nullptr, modelPoseComp); + + modelPoses.push_back(modelPoseComp->Data()); + + auto linkId = _ecm.EntityByComponents( + components::Link(), + components::Name("caster")); + EXPECT_NE(kNullEntity, linkId); + + auto linkPoseComp = _ecm.Component(linkId); + ASSERT_NE(nullptr, linkPoseComp); + + linkPoses.push_back(linkPoseComp->Data()); + + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, modelPoses.size()); + + for (const auto &pose : modelPoses) + { + EXPECT_EQ(modelPoses[0], pose); + } + for (const auto &pose : linkPoses) + { + EXPECT_EQ(linkPoses[0], pose); + } + + // Publish command and check that link moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + msgs::Twist msg; + + const double desiredLinVel = 10.5; + const double desiredAngVel = 0.2; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + + // Give some time for message to be received + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, modelPoses.size()); + ASSERT_EQ(4000u, linkPoses.size()); + + // verify that the model is stationary + for (unsigned int i = 1001; i < modelPoses.size(); ++i) + { + EXPECT_EQ(modelPoses[0], modelPoses[i]); + } + + // verify that the link is moving in +x and rotating about its origin + for (unsigned int i = 1001; i < linkPoses.size(); ++i) + { + EXPECT_GT(linkPoses[i].Pos().X(), linkPoses[i-1].Pos().X()) << i; + EXPECT_NEAR(linkPoses[i].Pos().Y(), linkPoses[i-1].Pos().Y(), 1e-5); + EXPECT_NEAR(linkPoses[i].Pos().Z(), linkPoses[i-1].Pos().Z(), 1e-5); + EXPECT_NEAR(linkPoses[i].Rot().Euler().X(), + linkPoses[i-1].Rot().Euler().X(), 1e-5) << i; + EXPECT_NEAR(linkPoses[i].Rot().Euler().Y(), + linkPoses[i-1].Rot().Euler().Y(), 1e-5) << i; + EXPECT_GT(linkPoses[i].Rot().Euler().Z(), linkPoses[i-1].Rot().Euler().Z()) << i; + } + } }; ///////////////////////////////////////////////// @@ -136,6 +241,14 @@ TEST_P(VelocityControlTest, PublishCmd) "/model/vehicle_blue/cmd_vel"); } +///////////////////////////////////////////////// +TEST_P(VelocityControlTest, PublishLinkCmd) +{ + TestPublishLinkCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/velocity_control.sdf", + "/model/vehicle_blue/link/caster/cmd_vel"); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, VelocityControlTest, ::testing::Range(1, 2)); diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf index 2e104e5f08..bae5e14df4 100644 --- a/test/worlds/velocity_control.sdf +++ b/test/worlds/velocity_control.sdf @@ -310,6 +310,7 @@ + caster From abdcfb26752aa3912d6913d6d832ab98ae449dbd Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 15 Apr 2021 14:46:38 -0700 Subject: [PATCH 26/26] codecheck Signed-off-by: Ian Chen --- test/integration/velocity_control_system.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index b34c102bd0..5eb7722440 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -149,7 +149,8 @@ class VelocityControlTest : public ::testing::TestWithParam std::vector modelPoses; std::vector linkPoses; - testSystem.OnPostUpdate([&linkPoses, &modelPoses](const gazebo::UpdateInfo &, + testSystem.OnPostUpdate([&linkPoses, &modelPoses]( + const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) { auto modelId = _ecm.EntityByComponents( @@ -228,7 +229,8 @@ class VelocityControlTest : public ::testing::TestWithParam linkPoses[i-1].Rot().Euler().X(), 1e-5) << i; EXPECT_NEAR(linkPoses[i].Rot().Euler().Y(), linkPoses[i-1].Rot().Euler().Y(), 1e-5) << i; - EXPECT_GT(linkPoses[i].Rot().Euler().Z(), linkPoses[i-1].Rot().Euler().Z()) << i; + EXPECT_GT(linkPoses[i].Rot().Euler().Z(), + linkPoses[i-1].Rot().Euler().Z()) << i; } } };