From 553e9a43ea4790ee861c89521580a3d595f27a40 Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Thu, 19 Mar 2020 18:28:44 +0100 Subject: [PATCH] Scenario: new APIs for gazebo's ECM --- cpp/scenario/gazebo/CMakeLists.txt | 97 +- .../gazebo/include/scenario/gazebo/Joint.h | 185 +++ .../gazebo/include/scenario/gazebo/Link.h | 124 ++ .../gazebo/include/scenario/gazebo/Log.h | 37 + .../gazebo/include/scenario/gazebo/Model.h | 222 ++++ .../gazebo/include/scenario/gazebo/World.h | 93 ++ .../gazebo/components/BasePoseTarget.h | 51 + .../components/BaseWorldAccelerationTarget.h | 64 + .../components/BaseWorldVelocityTarget.h | 64 + .../components/JointAccelerationTarget.h | 60 + .../gazebo/components/JointControlMode.h | 57 + .../gazebo/components/JointControllerPeriod.h | 54 + .../gazebo/components/JointEffortLimit.h | 51 + .../scenario/gazebo/components/JointPID.h | 50 + .../gazebo/components/JointPositionReset.h | 35 +- .../gazebo/components/JointPositionTarget.h | 58 + .../gazebo/components/JointVelocityReset.h | 32 +- .../gazebo/components/JointVelocityTarget.h | 60 + .../gazebo/components/SimulatedTime.h | 55 + .../gazebo/components/WorldVelocityCmd.h | 42 +- .../include/scenario/gazebo/exceptions.h | 293 +++++ .../gazebo/include/scenario/gazebo/helpers.h | 273 +++++ .../gazebo/include/scenario/gazebo/utils.h | 52 + cpp/scenario/gazebo/src/Joint.cpp | 776 ++++++++++++ cpp/scenario/gazebo/src/Link.cpp | 416 +++++++ cpp/scenario/gazebo/src/Model.cpp | 1051 +++++++++++++++++ cpp/scenario/gazebo/src/World.cpp | 327 +++++ cpp/scenario/gazebo/src/helpers.cpp | 284 +++++ cpp/scenario/gazebo/src/utils.cpp | 140 +++ 29 files changed, 5046 insertions(+), 57 deletions(-) create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/Joint.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/Link.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/Log.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/Model.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/World.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/BasePoseTarget.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/JointControlMode.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/JointEffortLimit.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/JointPID.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionTarget.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/components/SimulatedTime.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/exceptions.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/helpers.h create mode 100644 cpp/scenario/gazebo/include/scenario/gazebo/utils.h create mode 100644 cpp/scenario/gazebo/src/Joint.cpp create mode 100644 cpp/scenario/gazebo/src/Link.cpp create mode 100644 cpp/scenario/gazebo/src/Model.cpp create mode 100644 cpp/scenario/gazebo/src/World.cpp create mode 100644 cpp/scenario/gazebo/src/helpers.cpp create mode 100644 cpp/scenario/gazebo/src/utils.cpp diff --git a/cpp/scenario/gazebo/CMakeLists.txt b/cpp/scenario/gazebo/CMakeLists.txt index c1670656f..86baecf1e 100644 --- a/cpp/scenario/gazebo/CMakeLists.txt +++ b/cpp/scenario/gazebo/CMakeLists.txt @@ -1,20 +1,52 @@ -# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. +# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) +# All rights reserved. +# +# This project is dual licensed under LGPL v2.1+ or Apachi License. +# +# - - - - - - - - - - - - - - - - - - +# +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. +# +# - - - - - - - - - - - - - - - - - - +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. # ================ # Extra Components # ================ set(EXTRA_COMPONENTS_HDRS +# ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/ECMMutex.h +# ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/PreUpdateConditionVariable.h ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointPID.h - ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/Constraint.h - ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/ConstraintsData.h +# ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/Constraint.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/SimulatedTime.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/BasePoseTarget.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/BaseWorldVelocityTarget.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h +# ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/InitialWorldSdf.h +# ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/InitialModelSdf.h +# ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/ConstraintsData.h ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointEffortLimit.h ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointControlMode.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/WorldVelocityCmd.h ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointPositionReset.h ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointVelocityReset.h - ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/WorldVelocityCmd.h) + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointPositionTarget.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointVelocityTarget.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointAccelerationTarget.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/gazebo/components/JointControllerPeriod.h) add_library(ExtraComponents INTERFACE) target_sources(ExtraComponents INTERFACE ${EXTRA_COMPONENTS_HDRS}) @@ -25,10 +57,47 @@ target_include_directories(ExtraComponents INTERFACE target_link_libraries(ExtraComponents INTERFACE ignition-gazebo3::core) -if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") - set_target_properties(ExtraComponents PROPERTIES - PUBLIC_HEADER "${EXTRA_COMPONENTS_HDRS}") -endif() +set_target_properties(ExtraComponents PROPERTIES + PUBLIC_HEADER "${EXTRA_COMPONENTS_HDRS}") + +# ============== +# ScenarioGazebo +# ============== + +set(SCENARIO_PUBLIC_HDRS + include/scenario/gazebo/World.h + include/scenario/gazebo/Model.h + include/scenario/gazebo/Joint.h + include/scenario/gazebo/Link.h + include/scenario/gazebo/Log.h + include/scenario/gazebo/utils.h + include/scenario/gazebo/exceptions.h) + +add_library(ScenarioGazebo + ${SCENARIO_PUBLIC_HDRS} + include/scenario/gazebo/helpers.h + src/World.cpp + src/Model.cpp + src/Joint.cpp + src/Link.cpp + src/utils.cpp + src/helpers.cpp) + +target_include_directories(ScenarioGazebo PUBLIC + $ + $) + +target_link_libraries(ScenarioGazebo + PRIVATE + ExtraComponents + ignition-gazebo3::core) + +set_target_properties(ScenarioGazebo PROPERTIES + PUBLIC_HEADER "${SCENARIO_PUBLIC_HDRS}") + +# =================== +# Install the targets +# =================== if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") install( @@ -39,4 +108,12 @@ if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI") ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/scenario/gazebo/components) + install( + TARGETS + ScenarioGazebo + EXPORT scenario + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/scenario/gazebo) endif() diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h b/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h new file mode 100644 index 000000000..589215b7d --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Joint.h @@ -0,0 +1,185 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_JOINT_H +#define SCENARIO_GAZEBO_JOINT_H + +#include +#include +#include + +#include +#include +#include +#include + +namespace scenario { + namespace base { + struct PID; + struct Limit; + enum class JointType + { + Invalid, + Fixed, + Revolute, + Prismatic, + Ball, + }; + enum class JointControlMode + { + Idle, + Force, + Velocity, + Position, + PositionInterpolated, + }; + } // namespace base + namespace gazebo { + class Joint; + class Model; + } // namespace gazebo +} // namespace scenario + +class scenario::gazebo::Joint +{ +public: + Joint(); + virtual ~Joint(); + + // ============ + // Gazebo Joint + // ============ + + uint64_t id() const; + bool initialize(const ignition::gazebo::Entity jointEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager); + bool createECMResources(); + + bool historyOfAppliedJointForcesEnabled() const; + bool enableHistoryOfAppliedJointForces( // + const bool enable, + const size_t maxHistorySize = 100); + std::vector getHistoryOfAppliedJointForces() const; + + // ========== + // Joint Core + // ========== + + size_t dofs() const; + std::string name() const; + base::JointType type() const; + + base::Limit positionLimit() const; + + base::JointControlMode controlMode() const; + bool setControlMode(const base::JointControlMode mode); + + double controllerPeriod() const; + + double maxEffort() const; + bool setMaxEffort(const double maxEffort); + + base::PID pid() const; + bool setPID(const base::PID& pid); + + // ================== + // Single DOF methods + // ================== + + double force(const size_t dof = 0) const; + double position(const size_t dof = 0) const; + double velocity(const size_t dof = 0) const; + + bool setForce(const double force, const size_t dof = 0); + bool setPositionTarget(const double position, const size_t dof = 0); + bool setVelocityTarget(const double velocity, const size_t dof = 0); + bool setAccelerationTarget(const double acceleration, const size_t dof = 0); + + double positionTarget(const size_t dof = 0) const; + double velocityTarget(const size_t dof = 0) const; + double accelerationTarget(const size_t dof = 0) const; + + bool resetPosition(const double position, const size_t dof = 0); + bool resetVelocity(const double velocity, const size_t dof = 0); + bool reset(const double position, // + const double velocity, + const size_t dof = 0); + + // ================= + // Multi DOF methods + // ================= + + std::vector jointForce() const; + std::vector jointPosition() const; + std::vector jointVelocity() const; + + bool setJointForce(const std::vector& force); + bool setJointPositionTarget(const std::vector& position); + bool setJointVelocityTarget(const std::vector& velocity); + bool setJointAccelerationTarget(const std::vector& acceleration); + + std::vector jointPositionTarget() const; + std::vector jointVelocityTarget() const; + std::vector jointAccelerationTarget() const; + + bool resetJointPosition(const std::vector& position); + bool resetJointVelocity(const std::vector& velocity); + bool resetJoint(const std::vector& position, + const std::vector& velocity); + +private: + class Impl; + std::unique_ptr pImpl; +}; + +struct scenario::base::PID +{ + PID() = default; + PID(const double _p, const double _i, const double _d) + : p(_p) + , i(_i) + , d(_d) + {} + + double p = 0; + double i = 0; + double d = 0; +}; + +struct scenario::base::Limit +{ + Limit() = default; + Limit(const double _min, const double _max) + : min(_min) + , max(_max) + {} + + double min = std::numeric_limits::lowest(); + double max = std::numeric_limits::max(); +}; + +#endif // SCENARIO_GAZEBO_JOINT_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Link.h b/cpp/scenario/gazebo/include/scenario/gazebo/Link.h new file mode 100644 index 000000000..760bbb1a7 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Link.h @@ -0,0 +1,124 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_LINK_H +#define SCENARIO_GAZEBO_LINK_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace scenario { + namespace base { + struct Pose; + struct ContactData; + } // namespace base + namespace gazebo { + class Link; + class Model; + } // namespace gazebo +} // namespace scenario + +class scenario::gazebo::Link +{ +public: + Link(); + virtual ~Link(); + + // =========== + // Gazebo Link + // =========== + + uint64_t id() const; + bool initialize(const ignition::gazebo::Entity linkEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager); + bool createECMResources(); + + // ========= + // Link Core + // ========= + + std::string name() const; + std::array position() const; + std::array orientation() const; + + std::array worldLinearVelocity() const; + std::array worldAngularVelocity() const; + std::array bodyLinearVelocity() const; + std::array bodyAngularVelocity() const; + + std::array worldLinearAcceleration() const; + std::array worldAngularAcceleration() const; + std::array bodyLinearAcceleration() const; + std::array bodyAngularAcceleration() const; + + bool contactsEnabled() const; + bool enableContactDetection(const bool enable); + + bool inContact() const; + std::vector contactData() const; + + bool applyWorldForce(const std::array& force, + const double duration = 0.0); + bool applyWorldTorque(const std::array& torque, + const double duration = 0.0); + +private: + class Impl; + std::unique_ptr pImpl; +}; + +struct scenario::base::Pose +{ + Pose() = default; + Pose(std::array p, std::array o) + : position(p) + , orientation(o) + {} + + // TODO: constructor + std::array position = {0, 0, 0}; + std::array orientation = {1, 0, 0, 0}; +}; + +struct scenario::base::ContactData +{ + std::string bodyA; + std::string bodyB; + std::array depth; + std::array wrench; + std::array normal; + std::array position; +}; + +#endif // SCENARIO_GAZEBO_LINK_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Log.h b/cpp/scenario/gazebo/include/scenario/gazebo/Log.h new file mode 100644 index 000000000..c5f6f9589 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Log.h @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_LOG +#define SCENARIO_GAZEBO_LOG + +#include +#define gymppError ::ignition::common::Console::err(__FILE__, __LINE__) +#define gymppWarning ::ignition::common::Console::warn(__FILE__, __LINE__) +#define gymppMessage ::ignition::common::Console::msg(__FILE__, __LINE__) +#define gymppDebug ::ignition::common::Console::dbg(__FILE__, __LINE__) +#define gymppLog ::ignition::common::Console::log(__FILE__, __LINE__) + +#endif // SCENARIO_GAZEBO_LOG diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/Model.h b/cpp/scenario/gazebo/include/scenario/gazebo/Model.h new file mode 100644 index 000000000..efdb8d309 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/Model.h @@ -0,0 +1,222 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_MODEL_H +#define SCENARIO_GAZEBO_MODEL_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace scenario { + namespace base { + struct ContactData; + enum class JointControlMode; + } // namespace base + namespace gazebo { + class Link; + class Joint; + class Model; + class World; + using LinkPtr = std::shared_ptr; + using JointPtr = std::shared_ptr; + using ModelPtr = std::shared_ptr; + } // namespace gazebo +} // namespace scenario + +class scenario::gazebo::Model +{ +public: + Model(); + virtual ~Model(); + + // ============ + // Gazebo Model + // ============ + + uint64_t id() const; + bool initialize(const ignition::gazebo::Entity modelEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager); + bool createECMResources(); + + bool insertModelPlugin(const std::string& libName, + const std::string& className, + const std::string& context = {}); + + bool historyOfAppliedJointForcesEnabled() const; + bool enableHistoryOfAppliedJointForces( // + const bool enable, + const size_t maxHistorySizePerJoint = 100); + std::vector getHistoryOfAppliedJointForces() const; + + // ========== + // Model Core + // ========== + + bool valid() const; + + size_t dofs() const; + std::string name() const; + + size_t nrOfLinks() const; + size_t nrOfJoints() const; + + LinkPtr getLink(const std::string& linkName) const; + JointPtr getJoint(const std::string& jointName) const; + + std::vector linkNames(const bool scoped = false) const; + std::vector jointNames(const bool scoped = false) const; + + double controllerPeriod() const; + bool setControllerPeriod(const double period); + + // ======== + // Contacts + // ======== + + bool contactsEnabled() const; + bool enableContacts(const bool enable); + + bool selfCollisions() const; + bool enableSelfCollisions(const bool enable); + + std::vector linksInContact() const; + + std::vector + contacts(const std::vector& linkNames = {}) const; + + // ================== + // Vectorized Methods + // ================== + + std::vector jointForces( // + const std::vector& jointNames = {}) const; + std::vector jointPositions( // + const std::vector& jointNames = {}) const; + std::vector jointVelocities( // + const std::vector& jointNames = {}) const; + + bool setJointControlMode(const base::JointControlMode& mode, + const std::vector& jointNames = {}); + + std::vector links( // + const std::vector& linkNames = {}) const; + std::vector joints( // + const std::vector& jointNames = {}) const; + + // ========================= + // Vectorized Target Methods + // ========================= + + bool setJointForces( // + const std::vector forces, + const std::vector& jointNames = {}); + bool setJointPositionTargets( // + const std::vector positions, + const std::vector& jointNames = {}); + bool setJointVelocityTargets( // + const std::vector velocities, + const std::vector& jointNames = {}); + bool setJointAccelerationTargets( // + const std::vector velocities, + const std::vector& jointNames = {}); + + bool resetJointPositions( // + const std::vector positions, + const std::vector& jointNames = {}); + bool resetJointVelocities( // + const std::vector velocities, + const std::vector& jointNames = {}); + + std::vector jointPositionTargets( // + const std::vector& jointNames = {}) const; + std::vector jointVelocityTargets( // + const std::vector& jointNames = {}) const; + std::vector jointAccelerationTargets( // + const std::vector& jointNames = {}) const; + + // ========= + // Base Link + // ========= + + std::string baseFrame() const; + bool setBaseFrame(const std::string& frameName); + + bool fixedBase() const; + bool setAsFixedBase(const bool fixedBase); + + std::array baseWrench() const; + std::array basePosition() const; + std::array baseOrientation() const; + std::array baseLinearVelocity() const; + std::array baseAngularVelocity() const; + + bool resetBaseLinearVelocity(const std::array& linear); + bool resetBaseAngularVelocity(const std::array& angular); + bool resetBaseVelocity(const std::array& linear, + const std::array& angular); + + bool resetBasePose(const std::array& position, + const std::array& orientation); + bool resetBasePosition(const std::array& position); + bool resetBaseOrientation(const std::array& orientation); + + // ================= + // Base Link Targets + // ================= + + bool setBasePoseTarget(const std::array& position, + const std::array& orientation); + bool setBasePositionTarget(const std::array& position); + bool setBasOrientationTarget(const std::array& orientation); + + bool setBaseVelocityTarget(const std::array& linear, + const std::array& angular); + bool setBaseLinearVelocityTarget(const std::array& linear); + bool setBaseAngularVelocityTarget(const std::array& angular); + bool setBaseLinearAccelerationTarget(const std::array& linear); + bool setBaseAngularAccelerationTarget(const std::array& angular); + + std::array getBasePositionTarget() const; + std::array getBaseOrientationTarget() const; + std::array getBaseLinearVelocityTarget() const; + std::array getBaseAngularVelocityTarget() const; + std::array getBaseLinearAccelerationTarget() const; + std::array getBaseAngularAccelerationTarget() const; + +private: + class Impl; + std::unique_ptr pImpl; +}; + +#endif // SCENARIO_GAZEBO_MODEL_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/World.h b/cpp/scenario/gazebo/include/scenario/gazebo/World.h new file mode 100644 index 000000000..86091e25f --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/World.h @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_WORLD_H +#define SCENARIO_GAZEBO_WORLD_H + +#include "scenario/gazebo/Joint.h" + +#include +#include +#include + +#include +#include +#include + +namespace scenario { + namespace gazebo { + class World; + class Model; + using ModelPtr = std::shared_ptr; + using WorldPtr = std::shared_ptr; + } // namespace gazebo +} // namespace scenario + +class scenario::gazebo::World +{ +public: + World(); + virtual ~World(); + + uint64_t id() const; + bool initialize(const ignition::gazebo::Entity worldEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager); + bool createECMResources(); + + bool insertWorldPlugin(const std::string& libName, + const std::string& className, + const std::string& context = {}); + + double time() const; + std::string name() const; + std::vector modelNames() const; + scenario::gazebo::ModelPtr getModel(const std::string& modelName) const; + + bool insertModel(const std::string& modelFile, + const std::string& overrideModelName = {}); + bool removeModel(const std::string& modelName); + +private: + class Impl; + std::unique_ptr pImpl; +}; + +struct scenario::gazebo::ConstraintData +{ + static const size_t InvalidId = 0; + + std::string scopedFrameChild; + std::string scopedFrameParent = "world"; + + std::array child_p_parent = {0, 0, 0}; + std::array child_quat_parent = {1, 0, 0, 0}; + + base::JointType type = base::JointType::Fixed; + std::array axisInChildFrame = {0, 0, 0}; +}; + +#endif // SCENARIO_GAZEBO_WORLD_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/BasePoseTarget.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/BasePoseTarget.h new file mode 100644 index 000000000..20fa7a892 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/BasePoseTarget.h @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_BASEPOSETARGET_H +#define IGNITION_GAZEBO_COMPONENTS_BASEPOSETARGET_H + +#include +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Base pose target used by floating base controllers. + using BasePoseTarget = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.BasePoseTarget", + BasePoseTarget) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_BASEPOSETARGET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h new file mode 100644 index 000000000..20569ebc7 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldAccelerationTarget.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_BASEWORLDACCELERATIONTARGET_H +#define IGNITION_GAZEBO_COMPONENTS_BASEWORLDACCELERATIONTARGET_H + +#include +#include +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Base world linear acceleration target used by + /// floating base controllers. + using BaseWorldLinearAccelerationTarget = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.BaseWorldLinearAccelerationTarget", + BaseWorldLinearAccelerationTarget) + + /// \brief Base world angular acceleration target used by + /// floating base controllers. + using BaseWorldAngularAccelerationTarget = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components." + "BaseWorldAngularAccelerationTargetTarget", + BaseWorldAngularAccelerationTarget) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_BASEWORLDACCELERATIONTARGET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h new file mode 100644 index 000000000..2c160b2f2 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/BaseWorldVelocityTarget.h @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_BASEWORLDVELOCITYTARGET_H +#define IGNITION_GAZEBO_COMPONENTS_BASEWORLDVELOCITYTARGET_H + +#include +#include +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Base world linear velocity target used by + /// floating base controllers. + using BaseWorldLinearVelocityTarget = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.BaseWorldLinearVelocityTarget", + BaseWorldLinearVelocityTarget) + + /// \brief Base world angular velocity target used by + /// floating base controllers. + using BaseWorldAngularVelocityTarget = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components." + "BaseWorldAngularVelocityTargetTarget", + BaseWorldAngularVelocityTarget) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_BASEWORLDVELOCITYTARGET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h new file mode 100644 index 000000000..b0d2c2cf3 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointAccelerationTarget.h @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATIONTARGET_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATIONTARGET_H + +#include + +#include +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Joint acceleration target in SI units (rad/s/s for + /// revolute, m/s/s for prismatic) used by joint + /// controllers. + /// + /// The component wraps a std::vector of size equal to the + /// degrees of freedom of the joint. + using JointAccelerationTarget = + Component, + class JointAccelerationTargetTag, + serializers::VectorDoubleSerializer>; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointAccelerationTarget", + JointAccelerationTarget) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTACCELERATIONTARGET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointControlMode.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointControlMode.h new file mode 100644 index 000000000..bf6438c6b --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointControlMode.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLMODE_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLMODE_H + +#include "scenario/gazebo/Joint.h" + +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace serializers { + class JointControlModeSerializer; + } // namespace serializers + + namespace components { + /// \brief Joint control mode. + using JointControlMode = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointControlMode", + JointControlMode) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLMODE_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h new file mode 100644 index 000000000..6e7d9e44e --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointControllerPeriod.h @@ -0,0 +1,54 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLERPERIOD_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLERPERIOD_H + +#include +#include +#include +#include + +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Joint controller period in seconds. + using JointControllerPeriod = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointControllerPeriod", + JointControllerPeriod) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTCONTROLLERPERIOD_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointEffortLimit.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointEffortLimit.h new file mode 100644 index 000000000..061c7f7e2 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointEffortLimit.h @@ -0,0 +1,51 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTEFFORTLIMIT_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTEFFORTLIMIT_H + +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Joint effort limit in SI units (Nm for revolute, + /// N for prismatic). + using JointEffortLimit = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointEffortLimit", + JointEffortLimit) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTEFFORTLIMIT_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPID.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPID.h new file mode 100644 index 000000000..c0303be71 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPID.h @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTPID_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTPID_H + +#include +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Joint PID controller. + using JointPID = + Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointPID", + JointPID) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTPID_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionReset.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionReset.h index 3cda63cbe..565510c2b 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionReset.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionReset.h @@ -1,13 +1,15 @@ /* - * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * This software may be modified and distributed under the terms of the * GNU Lesser General Public License v2.1 or any later version. * - * ================================================== - * - * Copyright (C) 2018 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. @@ -22,8 +24,8 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONRESET_HH_ -#define IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONRESET_HH_ +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONRESET_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONRESET_H #include @@ -37,18 +39,21 @@ namespace ignition { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace components { - /// \brief Joint positions in SI units (rad for revolute, m for prismatic). + /// \brief Joint positions in SI units (rad for revolute, m for + /// prismatic). /// - /// The component wraps a std::vector of size equal to the degrees of freedom - /// of the joint. - using JointPositionReset = Component, - class JointPositionResetTag, - serializers::VectorDoubleSerializer>; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointPositionReset", - JointPositionReset) + /// The component wraps a std::vector of size equal to the + /// degrees of freedom of the joint. + using JointPositionReset = + Component, + class JointPositionResetTag, + serializers::VectorDoubleSerializer>; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointPositionReset", + JointPositionReset) } // namespace components } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo } // namespace ignition -#endif +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONRESET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionTarget.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionTarget.h new file mode 100644 index 000000000..4b7f68890 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointPositionTarget.h @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONTARGET_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONTARGET_H + +#include + +#include +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Joint position target in SI units (rad for revolute, + /// m for prismatic) used by joint controllers. + /// + /// The component wraps a std::vector of size equal to the + /// degrees of freedom of the joint. + using JointPositionTarget = + Component, + class JointPositionTargetTag>; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointPositionTarget", + JointPositionTarget) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONTARGET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityReset.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityReset.h index b13ebf53e..e36d90982 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityReset.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityReset.h @@ -1,13 +1,15 @@ /* - * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * This software may be modified and distributed under the terms of the * GNU Lesser General Public License v2.1 or any later version. * - * ================================================== - * - * Copyright (C) 2018 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. @@ -22,8 +24,8 @@ * limitations under the License. */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYRESET_HH_ -#define IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYRESET_HH_ +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYRESET_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYRESET_H #include @@ -40,16 +42,18 @@ namespace ignition { /// \brief Joint velocities in SI units /// (rad/s for revolute, m/s for prismatic). /// - /// The component wraps a std::vector of size equal to the degrees of freedom - /// of the joint. - using JointVelocityReset = Component, - class JointVelocityResetTag, - serializers::VectorDoubleSerializer>; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointVelocityReset", - JointVelocityReset) + /// The component wraps a std::vector of size equal to the + /// degrees of freedom of the joint. + using JointVelocityReset = + Component, + class JointVelocityResetTag, + serializers::VectorDoubleSerializer>; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointVelocityReset", + JointVelocityReset) } // namespace components } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo } // namespace ignition -#endif +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYRESET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h new file mode 100644 index 000000000..adac8d00f --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/JointVelocityTarget.h @@ -0,0 +1,60 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYTARGET_H +#define IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYTARGET_H + +#include + +#include +#include +#include +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief Joint velocity target in SI units (rad/s for + /// revolute, m/s for prismatic) used by joint + /// controllers. + /// + /// The component wraps a std::vector of size equal to the + /// degrees of freedom of the joint. + using JointVelocityTarget = + Component, + class JointVelocityTargetTag, + serializers::VectorDoubleSerializer>; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointVelocityTarget", + JointVelocityTarget) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYTARGET_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/SimulatedTime.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/SimulatedTime.h new file mode 100644 index 000000000..785ee50f7 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/SimulatedTime.h @@ -0,0 +1,55 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef IGNITION_GAZEBO_COMPONENTS_SIMULATEDTIME_H +#define IGNITION_GAZEBO_COMPONENTS_SIMULATEDTIME_H + +#include +#include +#include +#include + +#include + +namespace ignition { + namespace gazebo { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + namespace components { + /// \brief A component that holds the world's simulated time in + /// seconds. + using SimulatedTime = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.SimulatedTime", + SimulatedTime) + } // namespace components + } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_COMPONENTS_SIMULATEDTIME_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h b/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h index 6f8ac2176..cc6fa6ba9 100644 --- a/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h +++ b/cpp/scenario/gazebo/include/scenario/gazebo/components/WorldVelocityCmd.h @@ -1,13 +1,15 @@ /* - * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT) + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) * All rights reserved. * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * * This software may be modified and distributed under the terms of the * GNU Lesser General Public License v2.1 or any later version. * - * ================================================== - * - * Copyright (C) 2019 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. @@ -20,10 +22,10 @@ * 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_COMPONENTS_WORLDVELOCITYCMD_HH_ -#define IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_HH_ + +#ifndef IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H +#define IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H #include @@ -41,21 +43,25 @@ namespace ignition { { math::Vector3d linear; math::Vector3d angular; - }; - bool operator==(const WorldVelocity& a, const WorldVelocity& b) - { - return a.linear == b.linear && a.angular == b.angular; - } + bool operator==(const WorldVelocity& other) const + { + return this->linear == other.linear + && this->angular == other.angular; + } + }; namespace components { - /// \brief A component type that contains commanded velocity of an - /// entity in the world frame represented by ignition::math::Vector3d. - using WorldVelocityCmd = Component; - IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.WorldVelocityCmdTag", - WorldVelocityCmd) + /// \brief A component type that contains commanded velocity of + /// an entity in the world frame represented by + /// ignition::math::Vector3d. + using WorldVelocityCmd = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.WorldVelocityCmdTag", + WorldVelocityCmd) } // namespace components } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo } // namespace ignition -#endif // IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_HH_ +#endif // IGNITION_GAZEBO_COMPONENTS_WORLDVELOCITYCMD_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/exceptions.h b/cpp/scenario/gazebo/include/scenario/gazebo/exceptions.h new file mode 100644 index 000000000..51c8bf7b9 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/exceptions.h @@ -0,0 +1,293 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_EXCEPTIONS_H +#define SCENARIO_GAZEBO_EXCEPTIONS_H + +#include +#include +#include + +#include +#include +#include + +namespace scenario { + namespace gazebo { + namespace exceptions { + class LinkError; + class JointError; + class ModelError; + class DOFMismatch; + class LinkNotFound; + class JointNotFound; + class ModelNotFound; + class ComponentNotFound; + class NotImplementedError; + } // namespace exceptions + } // namespace gazebo +} // namespace scenario + +class scenario::gazebo::exceptions::LinkError : public std::runtime_error +{ +private: + std::string linkName; + +public: + explicit LinkError(const std::string& msg, const std::string& linkName = {}) + : std::runtime_error(msg) + , linkName(linkName) + {} + + const char* what() const noexcept override + { + std::string prefix; + + if (!linkName.empty()) { + prefix = "[" + linkName + "] "; + } + + std::string what = prefix + std::runtime_error::what(); + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::JointError : public std::runtime_error +{ +private: + std::string jointName; + +public: + explicit JointError(const std::string& msg, + const std::string& jointName = {}) + : std::runtime_error(msg) + , jointName(jointName) + {} + + const char* what() const noexcept override + { + std::string prefix; + + if (!jointName.empty()) { + prefix = "[" + jointName + "] "; + } + + std::string what = prefix + std::runtime_error::what(); + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::ModelError : public std::runtime_error +{ +private: + std::string modelName; + +public: + explicit ModelError(const std::string& msg, + const std::string& modelName = {}) + : std::runtime_error(msg) + , modelName(modelName) + {} + + const char* what() const noexcept override + { + std::string prefix; + + if (!modelName.empty()) { + prefix = "[" + modelName + "] "; + } + + std::string what = prefix + std::runtime_error::what(); + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::DOFMismatch : public std::runtime_error +{ +private: + size_t dataDoFs; + size_t jointDoFs; + std::string jointName; + +public: + explicit DOFMismatch(const size_t jointDoFs, + const size_t dataDoFs, + const std::string& jointName = {}) + : std::runtime_error("") + , jointDoFs(jointDoFs) + , dataDoFs(dataDoFs) + , jointName(jointName) + {} + + const char* what() const noexcept override + { + std::string prefix; + + if (!jointName.empty()) { + prefix = "[" + jointName + "] "; + } + + std::string what = prefix + + "Nr of DoFs joint=" + std::to_string(jointDoFs) + + " data=" + std::to_string(dataDoFs); + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::LinkNotFound : public std::runtime_error +{ +public: + explicit LinkNotFound(const std::string& error) + : std::runtime_error(error) + {} + + const char* what() const noexcept override + { + std::string linkName = std::runtime_error::what(); + std::string prefix = "[" + linkName + "] "; + std::string what = prefix + "Link does not exist"; + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::JointNotFound : public std::runtime_error +{ +public: + explicit JointNotFound(const std::string& error) + : std::runtime_error(error) + {} + + const char* what() const noexcept override + { + std::string jointName = std::runtime_error::what(); + std::string prefix = "[" + jointName + "] "; + std::string what = prefix + "Joint does not exist"; + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::ModelNotFound : public std::runtime_error +{ +public: + explicit ModelNotFound(const std::string& error) + : std::runtime_error(error) + {} + + const char* what() const noexcept override + { + std::string modelName = std::runtime_error::what(); + std::string prefix = "[" + modelName + "] "; + std::string what = prefix + "Model does not exist"; + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::NotImplementedError + : public std::logic_error +{ +public: + explicit NotImplementedError(const std::string& symbol) + : std::logic_error(symbol) + {} + + const char* what() const noexcept override + { + std::string symbol = std::logic_error::what(); + std::string what = "Symbol not implemented: " + symbol; + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +class scenario::gazebo::exceptions::ComponentNotFound + : public std::runtime_error +{ + ignition::gazebo::Entity entity; + ignition::gazebo::ComponentTypeId id; + +public: + explicit ComponentNotFound( + const ignition::gazebo::ComponentTypeId id, + const ignition::gazebo::Entity entity = ignition::gazebo::kNullEntity) + : std::runtime_error("") + , entity(entity) + , id(id) + {} + + const char* what() const noexcept override + { + std::string prefix; + + if (entity != ignition::gazebo::kNullEntity) { + prefix = "[Entity=" + std::to_string(entity) + "] "; + } + + // Get the name of the component from the factory + std::string componentName = + ignition::gazebo::components::Factory::Instance()->Name(id); + + // Build the message + std::string what = prefix + "Component not found: " + componentName; + + char* ptr = new char[what.size() + 1]; + strcpy(ptr, what.c_str()); + + return ptr; + } +}; + +#endif // SCENARIO_GAZEBO_EXCEPTIONS_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h new file mode 100644 index 000000000..d49379386 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/helpers.h @@ -0,0 +1,273 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_HELPERS_H +#define SCENARIO_GAZEBO_HELPERS_H + +#include "scenario/gazebo/Joint.h" +#include "scenario/gazebo/Link.h" +#include "scenario/gazebo/exceptions.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace scenario { + namespace gazebo { + namespace utils { + + std::shared_ptr + getSdfRootFromFile(const std::string& sdfFileName); + std::shared_ptr + getSdfRootFromString(const std::string& sdfFileName); + + const std::string ScenarioVerboseEnvVar = "SCENARIO_VERBOSE"; + bool verboseFromEnvironment(); + + template + auto getComponent(ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity, + ComponentDataTypeT defaultValue = {}); + + template + auto + getExistingComponent(ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity); + + template + auto getComponentData(ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity) + -> decltype(ComponentTypeT().Data()); + + template + auto getExistingComponentData( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity) + -> decltype(ComponentTypeT().Data()); + + scenario::base::Pose + fromIgnitionPose(const ignition::math::Pose3d& ignitionPose); + + ignition::math::Pose3d + toIgnitionPose(const scenario::base::Pose& pose); + + std::vector fromIgnitionContactsMsgs( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::msgs::Contacts& contactsMsg); + + bool renameSDFModel(sdf::Root& sdfRoot, + const std::string& newModelName, + size_t modelIndex = 0); + sdf::ElementPtr getPluginSDFElement(const std::string& libName, + const std::string& className); + + sdf::JointType toSdf(const scenario::base::JointType type); + scenario::base::JointType fromSdf(const sdf::JointType sdfType); + + template + auto defaultEqualityOperator(const T& a, const T& b) -> bool + { + return a == b; + } + + template + auto setComponentData( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity, + const ComponentDataTypeT& data, + const std::function& eql = + defaultEqualityOperator); + + template + auto setExistingComponentData( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity, + const ComponentDataTypeT& data, + const std::function& eql = + defaultEqualityOperator); + + static inline std::array + fromIgnitionVector(const ignition::math::Vector3d& ignitionVector) + { + return { + ignitionVector.X(), ignitionVector.Y(), ignitionVector.Z()}; + } + + static inline std::array fromIgnitionQuaternion( + const ignition::math::Quaterniond& ignitionQuaternion) + { + return {ignitionQuaternion.W(), + ignitionQuaternion.X(), + ignitionQuaternion.Y(), + ignitionQuaternion.Z()}; + } + + static inline ignition::math::Vector3d + toIgnitionVector3(const std::array& vector) + { + return {vector[0], vector[1], vector[2]}; + } + + static inline ignition::math::Vector4d + toIgnitionVector4(const std::array& vector) + { + return {vector[0], vector[1], vector[2], vector[3]}; + } + + static inline ignition::math::Quaterniond + toIgnitionQuaternion(const std::array& vector) + { + return {vector[0], vector[1], vector[2], vector[3]}; + } + + static inline ignition::math::PID + toIgnitionPID(const scenario::base::PID& pid) + { + return {pid.p, pid.i, pid.d}; + } + + template + class DummyComponentSerializer + { + public: + static std::ostream& + Serialize(std::ostream& out, const ComponentDataTypeT& /*data*/) + { + return out; + } + static std::istream& Deserialize(std::istream& in, + ComponentDataTypeT& /*data*/) + { + return in; + } + }; + } // namespace utils + } // namespace gazebo +} // namespace scenario + +template +auto scenario::gazebo::utils::getComponent( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity, + ComponentDataTypeT defaultValue) +{ + if (!ecm) { + throw std::runtime_error("ECM pointer not valid"); + } + + auto* component = ecm->Component(entity); + + if (!component) { + ecm->CreateComponent(entity, ComponentTypeT(defaultValue)); + component = ecm->Component(entity); + } + + return component; +} + +template +auto scenario::gazebo::utils::getExistingComponent( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity) +{ + if (!ecm) { + throw std::runtime_error("ECM pointer not valid"); + } + + auto* component = ecm->Component(entity); + + if (!component) { + throw exceptions::ComponentNotFound(ComponentTypeT::typeId, entity); + } + + return component; +} + +template +auto scenario::gazebo::utils::getComponentData( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity) -> decltype(ComponentTypeT().Data()) +{ + using ComponentDataType = + typename std::remove_reference::type; + + auto component = + getComponent(ecm, entity); + + return component->Data(); +} + +template +auto scenario::gazebo::utils::getExistingComponentData( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity) -> decltype(ComponentTypeT().Data()) +{ + auto component = getExistingComponent(ecm, entity); + + return component->Data(); +} + +template +auto scenario::gazebo::utils::setComponentData( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity, + const ComponentDataTypeT& data, + const std::function& eql) +{ + auto component = + utils::getComponent(ecm, entity); + + component->SetData(data, eql); +} + +template +auto scenario::gazebo::utils::setExistingComponentData( + ignition::gazebo::EntityComponentManager* ecm, + const ignition::gazebo::Entity entity, + const ComponentDataTypeT& data, + const std::function& eql) +{ + auto component = utils::getExistingComponent(ecm, entity); + component->SetData(data, eql); +} + +#endif // SCENARIO_GAZEBO_HELPERS_H diff --git a/cpp/scenario/gazebo/include/scenario/gazebo/utils.h b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h new file mode 100644 index 000000000..e9f5d99e2 --- /dev/null +++ b/cpp/scenario/gazebo/include/scenario/gazebo/utils.h @@ -0,0 +1,52 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef SCENARIO_GAZEBO_UTILS_H +#define SCENARIO_GAZEBO_UTILS_H + +#include + +#ifdef NDEBUG +#define DEFAULT_VERBOSITY 2 +#else +#define DEFAULT_VERBOSITY 4 +#endif + +namespace scenario { + namespace gazebo { + namespace utils { + void setVerbosity(const int level = DEFAULT_VERBOSITY); + std::string findSdfFile(const std::string& fileName); + bool sdfStringValid(const std::string& sdfString); + std::string getSdfString(const std::string& fileName); + std::string getModelNameFromSdf(const std::string& fileName, + const size_t modelIndex = 0); + std::string getEmptyWorld(); + } // namespace utils + } // namespace gazebo +} // namespace scenario + +#endif // SCENARIO_GAZEBO_UTILS_H diff --git a/cpp/scenario/gazebo/src/Joint.cpp b/cpp/scenario/gazebo/src/Joint.cpp new file mode 100644 index 000000000..eb2834811 --- /dev/null +++ b/cpp/scenario/gazebo/src/Joint.cpp @@ -0,0 +1,776 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "scenario/gazebo/Joint.h" + +#include "scenario/gazebo/Log.h" +#include "scenario/gazebo/components/JointAccelerationTarget.h" +#include "scenario/gazebo/components/JointControlMode.h" +#include "scenario/gazebo/components/JointControllerPeriod.h" +#include "scenario/gazebo/components/JointEffortLimit.h" +#include "scenario/gazebo/components/JointPID.h" +#include "scenario/gazebo/components/JointPositionReset.h" +#include "scenario/gazebo/components/JointPositionTarget.h" +#include "scenario/gazebo/components/JointVelocityReset.h" +#include "scenario/gazebo/components/JointVelocityTarget.h" +#include "scenario/gazebo/helpers.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace scenario::gazebo; +const ignition::math::PID DefaultPID(1, 0.1, 0.01, 1, -1, 10000, -10000); + +class Joint::Impl +{ +public: + ignition::gazebo::EventManager* eventManager = nullptr; + ignition::gazebo::EntityComponentManager* ecm = nullptr; + + ignition::gazebo::Entity jointEntity = ignition::gazebo::kNullEntity; +}; + +Joint::Joint() + : pImpl{std::make_unique()} +{} + +uint64_t Joint::id() const +{ + return pImpl->jointEntity; +} + +Joint::~Joint() = default; + +bool Joint::initialize(const ignition::gazebo::Entity jointEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager) +{ + if (jointEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + gymppError << "Failed to initialize Joint" << std::endl; + return false; + } + + pImpl->ecm = ecm; + pImpl->jointEntity = jointEntity; + pImpl->eventManager = eventManager; + + if (this->dofs() > 1) { + gymppError << "Joints with DoFs > 1 are not currently supported" + << std::endl; + return false; + } + + return true; +} + +bool Joint::createECMResources() +{ + gymppMessage << " [" << pImpl->jointEntity << "] " << this->name() + << std::endl; + + using namespace ignition::gazebo; + + const std::vector zero(this->dofs(), 0.0); + + // Create required components + pImpl->ecm->CreateComponent(pImpl->jointEntity, + components::JointForce(zero)); + pImpl->ecm->CreateComponent(pImpl->jointEntity, + components::JointPosition(zero)); + pImpl->ecm->CreateComponent(pImpl->jointEntity, + components::JointVelocity(zero)); + pImpl->ecm->CreateComponent(pImpl->jointEntity, + components::JointPID(DefaultPID)); + pImpl->ecm->CreateComponent( + pImpl->jointEntity, + components::JointControlMode(base::JointControlMode::Idle)); + pImpl->ecm->CreateComponent( + pImpl->jointEntity, + components::JointEffortLimit(std::numeric_limits::infinity())); + + return true; +} + +size_t Joint::dofs() const +{ + switch (this->type()) { + case base::JointType::Invalid: + return 0; + case base::JointType::Fixed: + return 0; + case base::JointType::Revolute: + return 1; + case base::JointType::Prismatic: + return 1; + case base::JointType::Ball: + return 3; + } + + assert(false); + return 0; +} + +std::string Joint::name() const +{ + std::string name = utils::getExistingComponentData< // + ignition::gazebo::components::Name>(pImpl->ecm, pImpl->jointEntity); + + return name; +} + +scenario::base::JointType Joint::type() const +{ + // Get the joint type + sdf::JointType sdfType = utils::getExistingComponentData< // + ignition::gazebo::components::JointType>(pImpl->ecm, + pImpl->jointEntity); + + // Convert the joint type + base::JointType type = utils::fromSdf(sdfType); + + return type; +} + +scenario::base::Limit Joint::positionLimit() const +{ + base::Limit limit; + + switch (this->type()) { + case base::JointType::Revolute: + case base::JointType::Prismatic: { + sdf::JointAxis& axis = utils::getExistingComponentData< // + ignition::gazebo::components::JointAxis>(pImpl->ecm, + pImpl->jointEntity); + limit.min = axis.Lower(); + limit.max = axis.Upper(); + break; + } + case base::JointType::Fixed: + limit.min = 0; + limit.max = 0; + break; + case base::JointType::Invalid: + case base::JointType::Ball: + gymppWarning << "Type of Joint '" << this->name() + << "' has no limits" << std::endl; + break; + } + + return limit; +} + +scenario::base::JointControlMode Joint::controlMode() const +{ + base::JointControlMode jointControlMode = utils::getExistingComponentData< + ignition::gazebo::components::JointControlMode>(pImpl->ecm, + pImpl->jointEntity); + + return jointControlMode; +} + +bool Joint::setControlMode(const scenario::base::JointControlMode mode) +{ + if (mode == base::JointControlMode::PositionInterpolated) { + gymppError << "PositionInterpolated not yet supported" << std::endl; + return false; + } + + utils::setExistingComponentData< + ignition::gazebo::components::JointControlMode>( + pImpl->ecm, pImpl->jointEntity, mode); + + // Delete the existing targets if they exist + gymppDebug << "Deleting existing position and velocity targets after " + << "changing control mode" << std::endl; + pImpl->ecm->RemoveComponent( + pImpl->jointEntity, + ignition::gazebo::components::JointPositionTarget().TypeId()); + pImpl->ecm->RemoveComponent( + pImpl->jointEntity, + ignition::gazebo::components::JointVelocityTarget().TypeId()); + + // Initialize the target as the current position / velocity + switch (mode) { + case base::JointControlMode::Position: + case base::JointControlMode::PositionInterpolated: + pImpl->ecm->CreateComponent( + pImpl->jointEntity, + ignition::gazebo::components::JointPositionTarget( + this->jointPosition())); + break; + case base::JointControlMode::Velocity: + pImpl->ecm->CreateComponent( + pImpl->jointEntity, + ignition::gazebo::components::JointVelocityTarget( + this->jointVelocity())); + break; + case base::JointControlMode::Idle: + case base::JointControlMode::Force: + break; + } + + // Get the PID + ignition::math::PID& pid = utils::getExistingComponentData< // + ignition::gazebo::components::JointPID>(pImpl->ecm, pImpl->jointEntity); + + // Reset the PID + pid.Reset(); + + return true; +} + +double Joint::controllerPeriod() const +{ + auto duration = utils::getExistingComponentData< // + ignition::gazebo::components::JointControllerPeriod>( + pImpl->ecm, pImpl->ecm->ParentEntity(pImpl->jointEntity)); + + return std::chrono::duration(duration).count(); +} + +double Joint::maxEffort() const +{ + double jointEffortLimit = utils::getExistingComponentData< // + ignition::gazebo::components::JointEffortLimit>(pImpl->ecm, + pImpl->jointEntity); + + return jointEffortLimit; +} + +bool Joint::setMaxEffort(const double maxEffort) +{ + utils::setExistingComponentData< + ignition::gazebo::components::JointEffortLimit>( + pImpl->ecm, pImpl->jointEntity, maxEffort); + + // Get the PID + ignition::math::PID& pid = utils::getExistingComponentData< // + ignition::gazebo::components::JointPID>(pImpl->ecm, pImpl->jointEntity); + + // Update the PID + pid.SetCmdMax(maxEffort); + pid.SetCmdMin(-maxEffort); + + return true; +} + +scenario::base::PID Joint::pid() const +{ + const ignition::math::PID& pidIgnitionMath = + utils::getExistingComponentData( + pImpl->ecm, pImpl->jointEntity); + + base::PID pid; + pid.p = pidIgnitionMath.PGain(); + pid.i = pidIgnitionMath.IGain(); + pid.d = pidIgnitionMath.DGain(); + + return pid; +} + +bool Joint::setPID(const scenario::base::PID& pid) +{ + auto eqOp = [](const ignition::math::PID& a, + const ignition::math::PID& b) -> bool { + bool equal = true; + double epsilon = std::numeric_limits::epsilon(); + + equal = equal && std::abs(a.PGain() - b.PGain()) > epsilon; + equal = equal && std::abs(a.IGain() - b.IGain()) > epsilon; + equal = equal && std::abs(a.DGain() - b.DGain()) > epsilon; + equal = equal && std::abs(a.IMin() - b.IMin()) > epsilon; + equal = equal && std::abs(a.IMax() - b.IMax()) > epsilon; + equal = equal && std::abs(a.CmdMin() - b.CmdMin()) > epsilon; + equal = equal && std::abs(a.CmdMax() - b.CmdMax()) > epsilon; + equal = equal && std::abs(a.CmdOffset() - b.CmdOffset()) > epsilon; + return equal; + }; + + // Create the new PID + ignition::math::PID pidIgnitionMath = utils::toIgnitionPID(pid); + pidIgnitionMath.SetCmdMax(this->maxEffort()); + pidIgnitionMath.SetCmdMin(-this->maxEffort()); + + // Store the new PID in the ECM + utils::setExistingComponentData( + pImpl->ecm, pImpl->jointEntity, pidIgnitionMath, eqOp); + + return true; +} + +double Joint::force(const size_t dof) const +{ + std::vector force = this->jointForce(); + return force[dof]; +} + +double Joint::position(const size_t dof) const +{ + std::vector position = this->jointPosition(); + return position[dof]; +} + +double Joint::velocity(const size_t dof) const +{ + std::vector velocity = this->jointVelocity(); + return velocity[dof]; +} + +bool Joint::setForce(const double force, const size_t dof) +{ + if (this->controlMode() != base::JointControlMode::Force) { + gymppError << "The active joint control mode does not accept a force" + << std::endl; + return false; + } + + if (dof >= this->dofs()) { + gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; + return false; + } + + auto& jointForce = utils::getComponentData< // + ignition::gazebo::components::JointForceCmd>(pImpl->ecm, + pImpl->jointEntity); + + if (jointForce.size() != this->dofs()) { + assert(jointForce.size() == 0); + jointForce = std::vector(this->dofs(), 0.0); + } + + double forceClipped; + double maxEffort = this->maxEffort(); + + forceClipped = std::min(force, maxEffort); + forceClipped = std::max(force, -maxEffort); + + jointForce[dof] = forceClipped; + return true; +} + +bool Joint::setPositionTarget(const double position, const size_t dof) +{ + const std::vector allowedControlModes = { + base::JointControlMode::Position, + base::JointControlMode::PositionInterpolated, + base::JointControlMode::Idle, + base::JointControlMode::Force}; + + auto it = std::find(allowedControlModes.begin(), + allowedControlModes.end(), + this->controlMode()); + + if (it == allowedControlModes.end()) { + gymppError + << "The active joint control mode does not accept a position target" + << std::endl; + return false; + } + + if (dof >= this->dofs()) { + gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; + return false; + } + + auto& jointPositionTarget = utils::getComponentData< // + ignition::gazebo::components::JointPositionTarget>(pImpl->ecm, + pImpl->jointEntity); + + if (jointPositionTarget.size() != this->dofs()) { + assert(jointPositionTarget.size() == 0); + jointPositionTarget = std::vector(this->dofs(), 0.0); + } + + jointPositionTarget[dof] = position; + return true; +} + +bool Joint::setVelocityTarget(const double velocity, const size_t dof) +{ + if (!(this->controlMode() == base::JointControlMode::Velocity + || this->controlMode() == base::JointControlMode::Idle + || this->controlMode() == base::JointControlMode::Force)) { + gymppError + << "The active joint control mode does not accept a velocity target" + << std::endl; + return false; + } + + if (dof >= this->dofs()) { + gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; + return false; + } + + auto& jointVelocityTarget = utils::getComponentData< // + ignition::gazebo::components::JointVelocityTarget>(pImpl->ecm, + pImpl->jointEntity); + + if (jointVelocityTarget.size() != this->dofs()) { + assert(jointVelocityTarget.size() == 0); + jointVelocityTarget = std::vector(this->dofs(), 0.0); + } + + jointVelocityTarget[dof] = velocity; + return true; +} + +bool Joint::setAccelerationTarget(const double acceleration, const size_t dof) +{ + if (!(this->controlMode() == base::JointControlMode::Idle + || this->controlMode() == base::JointControlMode::Force)) { + gymppError + << "The active joint control mode does not accept a velocity target" + << std::endl; + return false; + } + + if (dof >= this->dofs()) { + gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; + return false; + } + + auto& jointAccelerationTarget = utils::getComponentData< // + ignition::gazebo::components::JointAccelerationTarget>( + pImpl->ecm, pImpl->jointEntity); + + if (jointAccelerationTarget.size() != this->dofs()) { + assert(jointAccelerationTarget.size() == 0); + jointAccelerationTarget = std::vector(this->dofs(), 0.0); + } + + jointAccelerationTarget[dof] = acceleration; + return true; +} + +double Joint::positionTarget(const size_t dof) const +{ + std::vector positionTarget = this->jointPositionTarget(); + return positionTarget[dof]; +} + +double Joint::velocityTarget(const size_t dof) const +{ + std::vector velocityTarget = this->jointVelocityTarget(); + return velocityTarget[dof]; +} + +double Joint::accelerationTarget(const size_t dof) const +{ + std::vector accelerationTarget = this->jointAccelerationTarget(); + return accelerationTarget[dof]; +} + +bool Joint::resetPosition(const double position, size_t dof) +{ + if (dof >= this->dofs()) { + gymppError << "Joint '" << this->name() << "' does not have DoF#" << dof + << std::endl; + return false; + } + + auto& jointPositionReset = utils::getComponentData< // + ignition::gazebo::components::JointPositionReset>(pImpl->ecm, + pImpl->jointEntity); + + if (jointPositionReset.size() != this->dofs()) { + assert(jointPositionReset.size() == 0); + jointPositionReset = std::vector(this->dofs(), 0.0); + } + + jointPositionReset[dof] = position; + return true; +} + +bool Joint::resetVelocity(const double velocity, const size_t dof) +{ + if (dof >= this->dofs()) { + gymppError << "Joint '" << this->name() << " does not have DoF#" << dof + << std::endl; + return false; + } + + auto& jointVelocityReset = utils::getComponentData< // + ignition::gazebo::components::JointVelocityReset>(pImpl->ecm, + pImpl->jointEntity); + + if (jointVelocityReset.size() != this->dofs()) { + assert(jointVelocityReset.size() == 0); + jointVelocityReset = std::vector(this->dofs(), 0.0); + } + + jointVelocityReset[dof] = velocity; + return true; +} + +bool Joint::reset(const double position, const double velocity, size_t dof) +{ + bool ok = true; + + ok = ok && this->resetPosition(position, dof); + ok = ok && this->resetVelocity(velocity, dof); + + if (!ok) { + gymppError << "Failed to reset state of joint '" << this->name() << "'" + << std::endl; + return false; + } + + // Reset the PID + auto JointPIDComponent = utils::getExistingComponent< // + ignition::gazebo::components::JointPID>(pImpl->ecm, pImpl->jointEntity); + JointPIDComponent->Data().Reset(); + + return true; +} + +std::vector Joint::jointForce() const +{ + std::vector& jointForce = utils::getExistingComponentData< // + ignition::gazebo::components::JointForce>(pImpl->ecm, + pImpl->jointEntity); + + if (jointForce.size() != this->dofs()) { + throw exceptions::DOFMismatch( + this->dofs(), jointForce.size(), this->name()); + } + + return jointForce; +} + +std::vector Joint::jointPosition() const +{ + std::vector& jointPosition = utils::getExistingComponentData< // + ignition::gazebo::components::JointPosition>(pImpl->ecm, + pImpl->jointEntity); + + if (jointPosition.size() != this->dofs()) { + throw exceptions::DOFMismatch( + this->dofs(), jointPosition.size(), this->name()); + } + + return jointPosition; +} + +std::vector Joint::jointVelocity() const +{ + std::vector& jointVelocity = utils::getExistingComponentData< // + ignition::gazebo::components::JointVelocity>(pImpl->ecm, + pImpl->jointEntity); + + if (jointVelocity.size() != this->dofs()) { + throw exceptions::DOFMismatch( + this->dofs(), jointVelocity.size(), this->name()); + } + + return jointVelocity; +} + +bool Joint::setJointForce(const std::vector& force) +{ + if (force.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; + } + + auto& jointForce = utils::getComponentData< // + ignition::gazebo::components::JointForceCmd>(pImpl->ecm, + pImpl->jointEntity); + + jointForce = force; + return true; +} + +bool Joint::setJointPositionTarget(const std::vector& position) +{ + if (position.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; + } + + auto& jointPositionTarget = utils::getComponentData< // + ignition::gazebo::components::JointPositionTarget>(pImpl->ecm, + pImpl->jointEntity); + + jointPositionTarget = position; + return true; +} + +bool Joint::setJointVelocityTarget(const std::vector& velocity) +{ + if (velocity.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; + } + + auto& jointVelocityTarget = utils::getComponentData< // + ignition::gazebo::components::JointVelocityTarget>(pImpl->ecm, + pImpl->jointEntity); + + jointVelocityTarget = velocity; + return true; +} + +bool Joint::setJointAccelerationTarget(const std::vector& acceleration) +{ + if (acceleration.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; + } + + auto& jointAccelerationTarget = utils::getComponentData< // + ignition::gazebo::components::JointAccelerationTarget>( + pImpl->ecm, pImpl->jointEntity); + + jointAccelerationTarget = acceleration; + return true; +} + +std::vector Joint::jointPositionTarget() const +{ + std::vector& jointPositionTarget = utils::getExistingComponentData< + ignition::gazebo::components::JointPositionTarget>(pImpl->ecm, + pImpl->jointEntity); + + if (jointPositionTarget.size() != this->dofs()) { + throw exceptions::DOFMismatch( + this->dofs(), jointPositionTarget.size(), this->name()); + } + + return jointPositionTarget; +} + +std::vector Joint::jointVelocityTarget() const +{ + std::vector& jointVelocityTarget = utils::getExistingComponentData< + ignition::gazebo::components::JointVelocityTarget>(pImpl->ecm, + pImpl->jointEntity); + + if (jointVelocityTarget.size() != this->dofs()) { + throw exceptions::DOFMismatch( + this->dofs(), jointVelocityTarget.size(), this->name()); + } + + return jointVelocityTarget; +} + +std::vector Joint::jointAccelerationTarget() const +{ + std::vector& jointAccelTarget = utils::getExistingComponentData< + ignition::gazebo::components::JointAccelerationTarget>( + pImpl->ecm, pImpl->jointEntity); + + if (jointAccelTarget.size() != this->dofs()) { + throw exceptions::DOFMismatch( + this->dofs(), jointAccelTarget.size(), this->name()); + } + + return jointAccelTarget; +} + +bool Joint::resetJointPosition(const std::vector& position) +{ + if (position.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; + } + + auto& jointPositionReset = utils::getComponentData< // + ignition::gazebo::components::JointPositionReset>(pImpl->ecm, + pImpl->jointEntity); + + // Update the position + jointPositionReset = position; + + // Get the PID + ignition::math::PID& pid = utils::getExistingComponentData< // + ignition::gazebo::components::JointPID>(pImpl->ecm, pImpl->jointEntity); + + // Reset the PID + pid.Reset(); + + return true; +} + +bool Joint::resetJointVelocity(const std::vector& velocity) +{ + if (velocity.size() != this->dofs()) { + gymppError << "Wrong number of elements (joint_dofs=" << this->dofs() + << ")" << std::endl; + return false; + } + + auto& jointVelocityReset = utils::getComponentData< // + ignition::gazebo::components::JointVelocityReset>(pImpl->ecm, + pImpl->jointEntity); + + // Update the velocity + jointVelocityReset = velocity; + + // Get the PID + ignition::math::PID& pid = utils::getExistingComponentData< // + ignition::gazebo::components::JointPID>(pImpl->ecm, pImpl->jointEntity); + + // Reset the PID + pid.Reset(); + + return true; +} + +bool Joint::resetJoint(const std::vector& position, + const std::vector& velocity) +{ + bool ok = true; + + ok = ok && this->resetJointPosition(position); + ok = ok && this->resetJointVelocity(velocity); + + if (!ok) { + gymppError << "Failed to reset joint '" << this->name() << "'" + << std::endl; + return false; + } + + return true; +} diff --git a/cpp/scenario/gazebo/src/Link.cpp b/cpp/scenario/gazebo/src/Link.cpp new file mode 100644 index 000000000..8c8979892 --- /dev/null +++ b/cpp/scenario/gazebo/src/Link.cpp @@ -0,0 +1,416 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "scenario/gazebo/Link.h" + +#include "scenario/gazebo/Log.h" +#include "scenario/gazebo/exceptions.h" +#include "scenario/gazebo/helpers.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace scenario::gazebo; + +class Link::Impl +{ +public: + ignition::gazebo::EventManager* eventManager = nullptr; + ignition::gazebo::EntityComponentManager* ecm = nullptr; + + ignition::gazebo::Link link; + ignition::gazebo::Entity linkEntity = ignition::gazebo::kNullEntity; + + inline bool isCanonical() const + { + return ecm->EntityHasComponentType( + linkEntity, ignition::gazebo::components::CanonicalLink().TypeId()); + } +}; + +Link::Link() + : pImpl{std::make_unique()} +{} + +uint64_t Link::id() const +{ + return pImpl->linkEntity; +} + +Link::~Link() = default; + +bool Link::initialize(const ignition::gazebo::Entity linkEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager) +{ + if (linkEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + gymppError << "Failed to initialize Link" << std::endl; + return false; + } + + pImpl->ecm = ecm; + pImpl->linkEntity = linkEntity; + pImpl->eventManager = eventManager; + + pImpl->link = ignition::gazebo::Link(linkEntity); + + // Check that the link is valid + if (!pImpl->link.Valid(*ecm)) { + gymppError << "The link entity is not valid" << std::endl; + return false; + } + + return true; +} + +bool Link::createECMResources() +{ + gymppMessage << " [" << pImpl->linkEntity << "] " << this->name() + << std::endl; + + using namespace ignition::gazebo; + + // Create link components + pImpl->ecm->CreateComponent(pImpl->linkEntity, // + components::WorldPose()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::WorldLinearVelocity()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::WorldAngularVelocity()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::WorldLinearAcceleration()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::WorldAngularAcceleration()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::LinearVelocity()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::AngularVelocity()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::LinearAcceleration()); + pImpl->ecm->CreateComponent(pImpl->linkEntity, + components::AngularAcceleration()); + + return true; +} + +std::string Link::name() const +{ + auto linkName = pImpl->link.Name(*pImpl->ecm); + + if (!linkName) { + throw exceptions::LinkError("Failed to get link name"); + } + + return pImpl->link.Name(*pImpl->ecm).value(); +} + +std::array Link::position() const +{ + ignition::math::Pose3d linkPose; + + if (!pImpl->isCanonical()) { + auto linkPoseOptional = pImpl->link.WorldPose(*pImpl->ecm); + + if (!linkPoseOptional.has_value()) { + throw exceptions::LinkError("Failed to get world position", name()); + } + + linkPose = linkPoseOptional.value(); + } + else { + auto parentModelOptional = pImpl->link.ParentModel(*pImpl->ecm); + assert(parentModelOptional.has_value()); + + ignition::gazebo::Model parentModel = parentModelOptional.value(); + ignition::gazebo::Entity parentModelEntity = parentModel.Entity(); + + auto W_H_M = utils::getExistingComponentData< // + ignition::gazebo::components::Pose>(pImpl->ecm, parentModelEntity); + + auto M_H_B = utils::getExistingComponentData< // + ignition::gazebo::components::Pose>(pImpl->ecm, pImpl->linkEntity); + + linkPose = W_H_M * M_H_B; + } + + return utils::fromIgnitionPose(linkPose).position; +} + +std::array Link::orientation() const +{ + ignition::math::Pose3d linkPose; + + if (!pImpl->isCanonical()) { + auto linkPoseOptional = pImpl->link.WorldPose(*pImpl->ecm); + + if (!linkPoseOptional.has_value()) { + throw exceptions::LinkError("Failed to get world position", name()); + } + + linkPose = linkPoseOptional.value(); + } + else { + auto parentModelOptional = pImpl->link.ParentModel(*pImpl->ecm); + assert(parentModelOptional.has_value()); + + ignition::gazebo::Model parentModel = parentModelOptional.value(); + ignition::gazebo::Entity parentModelEntity = parentModel.Entity(); + + auto W_H_M = utils::getExistingComponentData< // + ignition::gazebo::components::Pose>(pImpl->ecm, parentModelEntity); + + auto M_H_B = utils::getExistingComponentData< // + ignition::gazebo::components::Pose>(pImpl->ecm, pImpl->linkEntity); + + linkPose = W_H_M * M_H_B; + } + + return utils::fromIgnitionPose(linkPose).orientation; +} + +std::array Link::worldLinearVelocity() const +{ + auto linkLinearVelocity = pImpl->link.WorldLinearVelocity(*pImpl->ecm); + + if (!linkLinearVelocity) { + throw exceptions::LinkError("Failed to get linear velocity", + this->name()); + } + + return utils::fromIgnitionVector(linkLinearVelocity.value()); +} + +std::array Link::worldAngularVelocity() const +{ + auto linkAngularVelocity = pImpl->link.WorldAngularVelocity(*pImpl->ecm); + + if (!linkAngularVelocity) { + throw exceptions::LinkError("Failed to get angular velocity", + this->name()); + } + + return utils::fromIgnitionVector(linkAngularVelocity.value()); +} + +std::array Link::bodyLinearVelocity() const +{ + auto linkBodyLinVel = utils::getComponentData< // + ignition::gazebo::components::AngularAcceleration>(pImpl->ecm, + pImpl->linkEntity); + + return utils::fromIgnitionVector(linkBodyLinVel); +} + +std::array Link::bodyAngularVelocity() const +{ + auto linkBodyAngVel = utils::getComponentData< // + ignition::gazebo::components::AngularAcceleration>(pImpl->ecm, + pImpl->linkEntity); + + return utils::fromIgnitionVector(linkBodyAngVel); +} + +std::array Link::worldLinearAcceleration() const +{ + auto linkLinearAcceleration = + pImpl->link.WorldLinearAcceleration(*pImpl->ecm); + + if (!linkLinearAcceleration) { + throw exceptions::LinkError("Failed to get linear acceleration", + this->name()); + } + + return utils::fromIgnitionVector(linkLinearAcceleration.value()); +} + +std::array Link::worldAngularAcceleration() const +{ + auto linkWorldAngAcc = utils::getComponentData< // + ignition::gazebo::components::WorldAngularAcceleration>( + pImpl->ecm, pImpl->linkEntity); + + return utils::fromIgnitionVector(linkWorldAngAcc); +} + +std::array Link::bodyLinearAcceleration() const +{ + auto linkBodyLinAcc = utils::getComponentData< // + ignition::gazebo::components::LinearAcceleration>(pImpl->ecm, + pImpl->linkEntity); + + return utils::fromIgnitionVector(linkBodyLinAcc); +} + +std::array Link::bodyAngularAcceleration() const +{ + auto linkBodyAngAcc = utils::getComponentData< // + ignition::gazebo::components::AngularAcceleration>(pImpl->ecm, + pImpl->linkEntity); + + return utils::fromIgnitionVector(linkBodyAngAcc); +} + +bool Link::contactsEnabled() const +{ + // Here we return true only if contacts are enables on all + // link's collision elements; + bool enabled = true; + + auto collisionEntities = pImpl->ecm->ChildrenByComponents( + pImpl->linkEntity, + ignition::gazebo::components::Collision(), + ignition::gazebo::components::ParentEntity(pImpl->linkEntity)); + + // Create the contact sensor data component that enables the Physics + // system to extract contact information from the physics engine + for (const auto collisionEntity : collisionEntities) { + bool hasContactSensorData = pImpl->ecm->EntityHasComponentType( + collisionEntity, + ignition::gazebo::components::ContactSensorData().TypeId()); + enabled = enabled && hasContactSensorData; + } + + return enabled; +} + +bool Link::enableContactDetection(const bool enable) +{ + if (enable && !this->contactsEnabled()) { + // Get all the collision entities of this link + auto collisionEntities = pImpl->ecm->ChildrenByComponents( + pImpl->linkEntity, + ignition::gazebo::components::Collision(), + ignition::gazebo::components::ParentEntity(pImpl->linkEntity)); + + // Create the contact sensor data component that enables the Physics + // system to extract contact information from the physics engine + for (const auto collisionEntity : collisionEntities) { + pImpl->ecm->CreateComponent( + collisionEntity, + ignition::gazebo::components::ContactSensorData()); + } + + return true; + } + + if (!enable && this->contactsEnabled()) { + // Get all the collision entities of this link + auto collisionEntities = pImpl->ecm->ChildrenByComponents( + pImpl->linkEntity, + ignition::gazebo::components::Collision(), + ignition::gazebo::components::ParentEntity(pImpl->linkEntity)); + + // Delete the contact sensor data component + for (const auto collisionEntity : collisionEntities) { + pImpl->ecm->RemoveComponent< + ignition::gazebo::components::ContactSensorData>( + collisionEntity); + } + + return true; + } + + return true; +} + +bool Link::inContact() const +{ + return this->contactData().empty() ? false : true; +} + +std::vector Link::contactData() const +{ + std::vector collisionEntities; + + // Get all the collision entities associated with this link + pImpl->ecm->Each( + [&](const ignition::gazebo::Entity& collisionEntity, + ignition::gazebo::components::Collision*, + ignition::gazebo::components::ContactSensorData*, + ignition::gazebo::components::ParentEntity* parentEntityComponent) + -> bool { + // Keep only the collision of this link + if (parentEntityComponent->Data() != pImpl->linkEntity) { + return true; + } + + collisionEntities.push_back(collisionEntity); + return true; + }); + + if (collisionEntities.empty()) { + return {}; + } + + std::vector linkAllContacts; + + for (const auto collisionEntity : collisionEntities) { + + // Get the contact data for the selected collision entity + ignition::msgs::Contacts contactSensorData = + utils::getExistingComponentData< // + ignition::gazebo::components::ContactSensorData>( + pImpl->ecm, collisionEntity); + + // Convert the ignition msg + std::vector collisionContacts = + utils::fromIgnitionContactsMsgs(pImpl->ecm, contactSensorData); + + // Insert the collision contact data to the link's buffer + linkAllContacts.insert(linkAllContacts.end(), + collisionContacts.begin(), + collisionContacts.end()); + } + + return linkAllContacts; +} + +bool Link::applyWorldForce(const std::array& force, + const double /*duration*/) +{ + pImpl->link.AddWorldForce(*pImpl->ecm, utils::toIgnitionVector3(force)); + return true; +} + +bool Link::applyWorldTorque(const std::array& /*torque*/, + const double /*duration*/) +{ + throw exceptions::NotImplementedError(__FUNCTION__); +} diff --git a/cpp/scenario/gazebo/src/Model.cpp b/cpp/scenario/gazebo/src/Model.cpp new file mode 100644 index 000000000..a8b015a62 --- /dev/null +++ b/cpp/scenario/gazebo/src/Model.cpp @@ -0,0 +1,1051 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "scenario/gazebo/Model.h" + +#include "scenario/gazebo/Joint.h" +#include "scenario/gazebo/Link.h" +#include "scenario/gazebo/Log.h" +#include "scenario/gazebo/components/BasePoseTarget.h" +#include "scenario/gazebo/components/BaseWorldAccelerationTarget.h" +#include "scenario/gazebo/components/BaseWorldVelocityTarget.h" +#include "scenario/gazebo/components/JointControllerPeriod.h" +#include "scenario/gazebo/components/WorldVelocityCmd.h" +#include "scenario/gazebo/exceptions.h" +#include "scenario/gazebo/helpers.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace scenario::gazebo; + +class Model::Impl +{ +public: + ignition::gazebo::EventManager* eventManager = nullptr; + ignition::gazebo::EntityComponentManager* ecm = nullptr; + + ignition::gazebo::Model model; + ignition::gazebo::Entity modelEntity = ignition::gazebo::kNullEntity; + + using LinkName = std::string; + using JointName = std::string; + + std::unordered_map links; + std::unordered_map joints; + + static std::vector getJointDataSerialized( + const Model* model, + const std::vector& jointNames, + std::function getJointData); + + static bool setJointDataSerialized( + Model* model, + const std::vector& data, + const std::vector& jointNames, + std::function setDataToDOF); +}; + +Model::Model() + : pImpl{std::make_unique()} +{} + +uint64_t Model::id() const +{ + return pImpl->modelEntity; +} + +Model::~Model() = default; + +bool Model::initialize(const ignition::gazebo::Entity modelEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager) +{ + if (modelEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + return false; + } + + pImpl->ecm = ecm; + pImpl->modelEntity = modelEntity; + pImpl->eventManager = eventManager; + + // Create the model + pImpl->model = ignition::gazebo::Model(modelEntity); + + // Check that the model is valid + if (!pImpl->model.Valid(*ecm)) { + gymppError << "The model entity is not valid" << std::endl; + return false; + } + + return true; +} + +bool Model::createECMResources() +{ + gymppMessage << "Model: [" << pImpl->modelEntity << "] " << this->name() + << std::endl; + + // Create required link resources + gymppMessage << "Links:" << std::endl; + for (const auto& linkName : this->linkNames()) { + if (!this->getLink(linkName)->createECMResources()) { + gymppError << "Failed to initialize ECM link resources" + << std::endl; + return false; + } + } + + // Create required model resources + gymppMessage << "Joints:" << std::endl; + for (const auto& jointName : this->jointNames()) { + if (!this->getJoint(jointName)->createECMResources()) { + gymppError << "Failed to initialize ECM joint resources" + << std::endl; + return false; + } + } + + // Initialize the Joint Controller period as maximum duration. + // In this way controllers are never updated unless a new period is + // configured. + pImpl->ecm->CreateComponent( + pImpl->modelEntity, + ignition::gazebo::components::JointControllerPeriod( + std::chrono::steady_clock::duration().max())); + + return true; +} + +bool Model::insertModelPlugin(const std::string& libName, + const std::string& className, + const std::string& context) +{ + // Create a new element without context + sdf::ElementPtr pluginElement = + utils::getPluginSDFElement(libName, className); + + // Insert the context into the plugin element + if (!context.empty()) { + + sdf::Root contextRoot; + auto errors = contextRoot.LoadSdfString(context); + + if (!errors.empty()) { + gymppError << "Failed to load plugin context" << std::endl; + + for (const auto& error : errors) { + gymppError << error << std::endl; + } + return false; + } + + // Get the first element of the context + // (stripping out the container) + auto contextNextElement = contextRoot.Element()->GetFirstElement(); + + // Insert the plugin context elements + while (contextNextElement) { + pluginElement->InsertElement(contextNextElement); + contextNextElement = contextNextElement->GetNextElement(); + } + } + + // The plugin element must be wrapped in another element, otherwise + // who receives it does not get the additional context + auto wrapped = sdf::SDF::WrapInRoot(pluginElement); + + // Trigger the plugin loading + pImpl->eventManager->Emit( + pImpl->modelEntity, wrapped); + + return true; +} + +bool Model::valid() const +{ + // TODO: extend the checks + return pImpl->model.Valid(*pImpl->ecm); +} + +size_t Model::dofs() const +{ + size_t dofs = 0; + + for (const auto& jointName : jointNames()) { + dofs += this->getJoint(jointName)->dofs(); + } + + return dofs; +} + +std::string Model::name() const +{ + return pImpl->model.Name(*pImpl->ecm); +} + +size_t Model::nrOfLinks() const +{ + return this->linkNames().size(); +} + +size_t Model::nrOfJoints() const +{ + return this->jointNames().size(); +} + +scenario::gazebo::LinkPtr Model::getLink(const std::string& linkName) const +{ + auto linkEntity = pImpl->model.LinkByName(*pImpl->ecm, linkName); + + if (linkEntity == ignition::gazebo::kNullEntity) { + throw exceptions::LinkNotFound(linkName); + } + + if (pImpl->links.find(linkName) != pImpl->links.end()) { + assert(pImpl->links.at(linkName)); + return pImpl->links.at(linkName); + } + + // Create the link + auto link = std::make_shared(); + + if (!link->initialize(linkEntity, pImpl->ecm, pImpl->eventManager)) { + throw exceptions::LinkError("Failed to initialize link", linkName); + } + + // Cache the link instance + pImpl->links[linkName] = link; + + return link; +} + +scenario::gazebo::JointPtr Model::getJoint(const std::string& jointName) const +{ + auto jointEntity = pImpl->model.JointByName(*pImpl->ecm, jointName); + + if (jointEntity == ignition::gazebo::kNullEntity) { + throw exceptions::JointNotFound(jointName); + } + + if (pImpl->joints.find(jointName) != pImpl->joints.end()) { + assert(pImpl->joints.at(jointName)); + return pImpl->joints.at(jointName); + } + + // Create the joint + auto joint = std::make_shared(); + + if (!joint->initialize(jointEntity, pImpl->ecm, pImpl->eventManager)) { + throw exceptions::JointError("Failed to initialize joint", jointName); + } + + // Cache the joint instance + pImpl->joints[jointName] = joint; + + return joint; +} + +std::vector Model::linkNames(const bool scoped) const +{ + std::vector linkNames; + + pImpl->ecm->Each( + [&](const ignition::gazebo::Entity& /*entity*/, + ignition::gazebo::components::Name* nameComponent, + ignition::gazebo::components::Link* /*linkComponent*/, + ignition::gazebo::components::ParentEntity* parentEntityComponent) + -> bool { + assert(nameComponent); + assert(parentEntityComponent); + + // Discard links not belonging to this model + if (parentEntityComponent->Data() != pImpl->modelEntity) { + return true; + } + + std::string prefix = ""; + if (scoped) { + prefix = this->name() + "::"; + } + + // Append the link name + linkNames.push_back(prefix + nameComponent->Data()); + return true; + }); + + return linkNames; +} + +std::vector Model::jointNames(const bool scoped) const +{ + std::vector jointNames; + + pImpl->ecm->Each( + [&](const ignition::gazebo::Entity& /*jointEntity*/, + ignition::gazebo::components::Name* nameComponent, + ignition::gazebo::components::Joint* /*jointComponent*/, + ignition::gazebo::components::ParentEntity* parentEntityComponent) + -> bool { + assert(nameComponent); + assert(parentEntityComponent); + + // Discard joints not belonging to this model + if (parentEntityComponent->Data() != pImpl->modelEntity) { + return true; + } + + // Discard joints with no DoFs + auto joint = this->getJoint(nameComponent->Data()); + if (joint->dofs() == 0) { + return true; + } + + std::string prefix = ""; + if (scoped) { + prefix = this->name() + "::"; + } + + // Append the joint name + jointNames.push_back(prefix + nameComponent->Data()); + return true; + }); + + return jointNames; +} + +double Model::controllerPeriod() const +{ + auto duration = utils::getExistingComponentData< // + ignition::gazebo::components::JointControllerPeriod>( + pImpl->ecm, pImpl->modelEntity); + + return std::chrono::duration(duration).count(); +} + +bool Model::setControllerPeriod(const double period) +{ + if (period <= 0) { + gymppError << "The controller period must be greater than zero" + << std::endl; + return false; + } + + using namespace std::chrono; + + // Create a floating-point duration + auto durationSec = duration(period); + + // Cast to integral duration + const auto durationNanoSec = round(durationSec); + + // Store the new period in the ECM + utils::setExistingComponentData< + ignition::gazebo::components::JointControllerPeriod>( + pImpl->ecm, pImpl->modelEntity, durationNanoSec); + return true; +} + +bool Model::contactsEnabled() const +{ + bool enabled = true; + + for (auto link : this->links()) { + enabled = enabled && link->contactsEnabled(); + } + + return enabled; +} + +bool Model::enableContacts(const bool enable) +{ + bool ok = true; + + for (auto link : this->links()) { + ok = ok && link->enableContactDetection(enable); + } + + return ok; +} + +bool Model::selfCollisions() const +{ + throw exceptions::NotImplementedError(__FUNCTION__); +} + +bool Model::enableSelfCollisions(const bool /*enable*/) +{ + throw exceptions::NotImplementedError(__FUNCTION__); +} + +std::vector Model::linksInContact() const +{ + std::vector linksInContact; + + for (const auto& linkName : this->linkNames()) { + linksInContact.push_back(linkName); + } + + return linksInContact; +} + +std::vector +Model::contacts(const std::vector& linkNames) const +{ + std::vector linkSerialization; + + if (!linkNames.empty()) { + linkSerialization = linkNames; + } + else { + linkSerialization = this->linkNames(); + } + + std::vector allContacts; + + for (const auto& linkName : linkSerialization) { + auto contacts = this->getLink(linkName)->contactData(); + std::move(contacts.begin(), // + contacts.end(), + std::back_inserter(allContacts)); + } + + return allContacts; +} + +std::vector +Model::jointForces(const std::vector& jointNames) const +{ + auto lambda = [](JointPtr joint, const size_t dof) -> double { + return joint->force(dof); + }; + + return Impl::getJointDataSerialized(this, jointNames, lambda); +} + +std::vector +Model::jointPositions(const std::vector& jointNames) const +{ + auto lambda = [](JointPtr joint, const size_t dof) -> double { + return joint->position(dof); + }; + + return Impl::getJointDataSerialized(this, jointNames, lambda); +} + +std::vector +Model::jointVelocities(const std::vector& jointNames) const +{ + auto lambda = [](JointPtr joint, const size_t dof) -> double { + return joint->velocity(dof); + }; + + return Impl::getJointDataSerialized(this, jointNames, lambda); +} + +bool Model::setJointControlMode(const scenario::base::JointControlMode& mode, + const std::vector& jointNames) +{ + std::vector jointSerialization; + + if (!jointNames.empty()) { + jointSerialization = jointNames; + } + else { + jointSerialization = this->jointNames(); + } + + bool ok = true; + + for (auto joint : this->joints(jointSerialization)) { + ok = ok && joint->setControlMode(mode); + } + + return ok; +} + +std::vector +Model::links(const std::vector& linkNames) const +{ + std::vector linkSerialization; + + if (!linkNames.empty()) { + linkSerialization = linkNames; + } + else { + linkSerialization = this->linkNames(); + } + + std::vector links; + + for (const auto& linkName : linkSerialization) { + links.push_back(this->getLink(linkName)); + } + + return links; +} + +std::vector +Model::joints(const std::vector& jointNames) const +{ + std::vector jointSerialization; + + if (!jointNames.empty()) { + jointSerialization = jointNames; + } + else { + jointSerialization = this->jointNames(); + } + + std::vector joints; + + for (const auto& jointName : jointSerialization) { + joints.push_back(this->getJoint(jointName)); + } + + return joints; +} + +bool Model::setJointForces(const std::vector forces, + const std::vector& jointNames) +{ + auto lambda = + [](JointPtr joint, const double force, const size_t dof) -> bool { + return joint->setForce(force, dof); + }; + + return Impl::setJointDataSerialized(this, forces, jointNames, lambda); +} + +bool Model::setJointPositionTargets(const std::vector positions, + const std::vector& jointNames) +{ + auto lambda = + [](JointPtr joint, const double position, const size_t dof) -> bool { + return joint->setPositionTarget(position, dof); + }; + + return Impl::setJointDataSerialized(this, positions, jointNames, lambda); +} + +bool Model::setJointVelocityTargets(const std::vector velocities, + const std::vector& jointNames) +{ + auto lambda = + [](JointPtr joint, const double velocity, const size_t dof) -> bool { + return joint->setVelocityTarget(velocity, dof); + }; + + return Impl::setJointDataSerialized(this, velocities, jointNames, lambda); +} + +bool Model::setJointAccelerationTargets( + const std::vector velocities, + const std::vector& jointNames) +{ + auto lambda = + [](JointPtr joint, const double velocity, const size_t dof) -> bool { + return joint->setAccelerationTarget(velocity, dof); + }; + + return Impl::setJointDataSerialized(this, velocities, jointNames, lambda); +} + +bool Model::resetJointPositions(const std::vector positions, + const std::vector& jointNames) +{ + auto lambda = + [](JointPtr joint, const double position, const size_t dof) -> bool { + return joint->resetPosition(position, dof); + }; + + return Impl::setJointDataSerialized(this, positions, jointNames, lambda); +} + +bool Model::resetJointVelocities(const std::vector velocities, + const std::vector& jointNames) +{ + auto lambda = + [](JointPtr joint, const double velocity, const size_t dof) -> bool { + return joint->resetVelocity(velocity, dof); + }; + + return Impl::setJointDataSerialized(this, velocities, jointNames, lambda); +} + +std::vector +Model::jointPositionTargets(const std::vector& jointNames) const +{ + auto lambda = [](JointPtr joint, const size_t dof) -> double { + return joint->positionTarget(dof); + }; + + return Impl::getJointDataSerialized(this, jointNames, lambda); +} + +std::vector +Model::jointVelocityTargets(const std::vector& jointNames) const +{ + auto lambda = [](JointPtr joint, const size_t dof) -> double { + return joint->velocityTarget(dof); + }; + + return Impl::getJointDataSerialized(this, jointNames, lambda); +} + +std::vector Model::jointAccelerationTargets( + const std::vector& jointNames) const +{ + auto lambda = [](JointPtr joint, const size_t dof) -> double { + return joint->accelerationTarget(dof); + }; + + return Impl::getJointDataSerialized(this, jointNames, lambda); +} + +std::string Model::baseFrame() const +{ + // Get all the canonical links of the model + auto candidateBaseLinks = pImpl->ecm->EntitiesByComponents( + ignition::gazebo::components::CanonicalLink(), + ignition::gazebo::components::ParentEntity(pImpl->model.Entity())); + + if (candidateBaseLinks.size() == 0) { + throw exceptions::ModelError("Failed to find the canonical link", + this->name()); + } + + if (candidateBaseLinks.size() > 1) { + throw exceptions::ModelError("Found multiple canonical link", + this->name()); + } + + // Get the name of the base link + std::string baseLinkName = utils::getExistingComponentData< // + ignition::gazebo::components::Name>(pImpl->ecm, + candidateBaseLinks.front()); + + return baseLinkName; +} + +bool Model::setBaseFrame(const std::string& frameName) +{ + const auto linkNames = this->linkNames(); + + if (std::find(linkNames.begin(), linkNames.end(), frameName) + == linkNames.end()) { + gymppError + << "Failed to set the model base on the frame of nonexistent link '" + << frameName << "'" << std::endl; + return false; + } + + if (frameName == this->baseFrame()) { + gymppDebug << "Frame '" << baseFrame() + << "' is already the current model base" << std::endl; + return true; + } + + gymppError << "Changing the base link is not yet supported" << std::endl; + throw exceptions::NotImplementedError(__FUNCTION__); +} + +bool Model::fixedBase() const +{ + throw exceptions::NotImplementedError(__FUNCTION__); +} + +bool Model::setAsFixedBase(const bool fixedBase) +{ + throw exceptions::NotImplementedError(__FUNCTION__); +} + +std::array Model::baseWrench() const +{ + throw exceptions::NotImplementedError(__FUNCTION__); +} + +std::array Model::basePosition() const +{ + // Get the model pose + ignition::math::Pose3d world_H_model = utils::getExistingComponentData< // + ignition::gazebo::components::Pose>(pImpl->ecm, pImpl->modelEntity); + + return utils::fromIgnitionPose(world_H_model).position; +} + +std::array Model::baseOrientation() const +{ + // Get the model pose + ignition::math::Pose3d world_H_model = utils::getExistingComponentData< // + ignition::gazebo::components::Pose>(pImpl->ecm, pImpl->modelEntity); + + return utils::fromIgnitionPose(world_H_model).orientation; +} + +std::array Model::baseLinearVelocity() const +{ + // We can get the velocity of the base link. Since there's only a rigid + // transformation between base and model frame, and the velocity is + // expressed in the world frame, we do not need to perform any conversion. + + // Get the name of the base link + const std::string baseLink = this->baseFrame(); + + // Return the linear velocity of the base link + return this->getLink(baseLink)->worldLinearVelocity(); +} + +std::array Model::baseAngularVelocity() const +{ + // We can get the velocity of the base link. Since there's only a rigid + // transformation between base and model frame, and the velocity is + // expressed in the world frame, we do not need to perform any conversion. + + // Get the name of the base link + const std::string baseLink = this->baseFrame(); + + // Return the angular velocity of the base link + return this->getLink(baseLink)->worldAngularVelocity(); +} + +bool Model::resetBasePose(const std::array& position, + const std::array& orientation) +{ + // Construct the desired transform between world and base + base::Pose pose; + pose.position = position; + pose.orientation = orientation; + ignition::math::Pose3d world_H_base = utils::toIgnitionPose(pose); + + // Get the entity of the canonical link + auto canonicalLinkEntity = pImpl->ecm->EntityByComponents( + ignition::gazebo::components::Link(), + ignition::gazebo::components::CanonicalLink(), + ignition::gazebo::components::Name(this->baseFrame()), + ignition::gazebo::components::ParentEntity(pImpl->modelEntity)); + + if (canonicalLinkEntity == ignition::gazebo::kNullEntity) { + gymppError << "Failed to get entity of canonical link" << std::endl; + return false; + } + + // Get the Pose component of the canonical link. + // This is the fixed transformation between the model and the base. + auto& model_H_base = utils::getExistingComponentData< // + ignition::gazebo::components::Pose>(pImpl->ecm, canonicalLinkEntity); + + // Compute the robot pose that corresponds to the desired base pose + const ignition::math::Pose3d& world_H_model = + world_H_base * model_H_base.Inverse(); + + // Store the new pose + utils::setComponentData( + pImpl->ecm, pImpl->modelEntity, world_H_model); + + return true; +} + +bool Model::resetBasePosition(const std::array& position) +{ + return this->resetBasePose(position, this->baseOrientation()); +} + +bool Model::resetBaseOrientation(const std::array& orientation) +{ + return this->resetBasePose(this->basePosition(), orientation); +} + +bool Model::resetBaseLinearVelocity(const std::array& linear) +{ + return this->resetBaseVelocity(linear, this->baseAngularVelocity()); +} + +bool Model::resetBaseAngularVelocity(const std::array& angular) +{ + return this->resetBaseVelocity(this->baseLinearVelocity(), angular); +} + +bool Model::resetBaseVelocity(const std::array& linear, + const std::array& angular) +{ + // Create the new model velocity + ignition::gazebo::WorldVelocity worldVelocity; + worldVelocity.linear = utils::toIgnitionVector3(linear); + worldVelocity.angular = utils::toIgnitionVector3(angular); + + // Store the new velocity + utils::setComponentData( + pImpl->ecm, pImpl->modelEntity, worldVelocity); + + return true; +} + +bool Model::setBasePoseTarget(const std::array& position, + const std::array& orientation) +{ + ignition::math::Pose3d basePoseTarget = + utils::toIgnitionPose(base::Pose{position, orientation}); + + utils::setComponentData( + pImpl->ecm, pImpl->modelEntity, basePoseTarget); + + return true; +} + +bool Model::setBasePositionTarget(const std::array& position) +{ + auto basePoseTargetComponent = utils::getComponent< // + ignition::gazebo::components::BasePoseTarget>( + pImpl->ecm, pImpl->modelEntity, ignition::math::Pose3d::Zero); + + auto basePoseTarget = + ignition::math::Pose3d(utils::toIgnitionVector3(position), + basePoseTargetComponent->Data().Rot()); + + utils::setExistingComponentData< + ignition::gazebo::components::BasePoseTarget>( + pImpl->ecm, pImpl->modelEntity, basePoseTarget); + + return true; +} + +bool Model::setBasOrientationTarget(const std::array& orientation) +{ + auto basePoseTargetComponent = utils::getComponent< // + ignition::gazebo::components::BasePoseTarget, + ignition::math::Pose3d>(pImpl->ecm, pImpl->modelEntity); + + auto basePoseTarget = + ignition::math::Pose3d(basePoseTargetComponent->Data().Pos(), + utils::toIgnitionQuaternion(orientation)); + + utils::setComponentData( + pImpl->ecm, pImpl->modelEntity, basePoseTarget); + + return true; +} + +bool Model::setBaseVelocityTarget(const std::array& linear, + const std::array& angular) +{ + return this->setBaseLinearVelocityTarget(linear) + && this->setBaseAngularVelocityTarget(angular); +} + +bool Model::setBaseLinearVelocityTarget(const std::array& linear) +{ + ignition::math::Vector3d baseWorldLinearVelocity = + utils::toIgnitionVector3(linear); + + utils::setComponentData< + ignition::gazebo::components::BaseWorldLinearVelocityTarget>( + pImpl->ecm, pImpl->modelEntity, baseWorldLinearVelocity); + + return true; +} + +bool Model::setBaseAngularVelocityTarget(const std::array& angular) +{ + ignition::math::Vector3d baseWorldAngularVelocity = + utils::toIgnitionVector3(angular); + + utils::setComponentData< + ignition::gazebo::components::BaseWorldAngularVelocityTarget>( + pImpl->ecm, pImpl->modelEntity, baseWorldAngularVelocity); + + return true; +} + +bool Model::setBaseLinearAccelerationTarget(const std::array& linear) +{ + ignition::math::Vector3d baseWorldLinearAcceleration = + utils::toIgnitionVector3(linear); + + utils::setComponentData< + ignition::gazebo::components::BaseWorldLinearAccelerationTarget>( + pImpl->ecm, pImpl->modelEntity, baseWorldLinearAcceleration); + + return true; +} + +bool Model::setBaseAngularAccelerationTarget( + const std::array& angular) +{ + ignition::math::Vector3d baseWorldAngularAcceleration = + utils::toIgnitionVector3(angular); + + utils::setComponentData< + ignition::gazebo::components::BaseWorldAngularAccelerationTarget>( + pImpl->ecm, pImpl->modelEntity, baseWorldAngularAcceleration); + + return true; +} + +std::array Model::getBasePositionTarget() const +{ + ignition::math::Pose3d& basePoseTarget = utils::getExistingComponentData< + ignition::gazebo::components::BasePoseTarget>(pImpl->ecm, + pImpl->modelEntity); + + return utils::fromIgnitionPose(basePoseTarget).position; +} + +std::array Model::getBaseOrientationTarget() const +{ + ignition::math::Pose3d& basePoseTarget = utils::getExistingComponentData< + ignition::gazebo::components::BasePoseTarget>(pImpl->ecm, + pImpl->modelEntity); + + return utils::fromIgnitionPose(basePoseTarget).orientation; +} + +std::array Model::getBaseLinearVelocityTarget() const +{ + ignition::math::Vector3d& baseLinTarget = utils::getExistingComponentData< + ignition::gazebo::components::BaseWorldLinearVelocityTarget>( + pImpl->ecm, pImpl->modelEntity); + + return utils::fromIgnitionVector(baseLinTarget); +} + +std::array Model::getBaseAngularVelocityTarget() const +{ + ignition::math::Vector3d& baseAngTarget = utils::getExistingComponentData< + ignition::gazebo::components::BaseWorldAngularVelocityTarget>( + pImpl->ecm, pImpl->modelEntity); + + return utils::fromIgnitionVector(baseAngTarget); +} + +std::array Model::getBaseLinearAccelerationTarget() const +{ + ignition::math::Vector3d& baseLinTarget = utils::getExistingComponentData< + ignition::gazebo::components::BaseWorldLinearAccelerationTarget>( + pImpl->ecm, pImpl->modelEntity); + + return utils::fromIgnitionVector(baseLinTarget); +} + +std::array Model::getBaseAngularAccelerationTarget() const +{ + ignition::math::Vector3d& baseAngTarget = utils::getExistingComponentData< + ignition::gazebo::components::BaseWorldAngularAccelerationTarget>( + pImpl->ecm, pImpl->modelEntity); + + return utils::fromIgnitionVector(baseAngTarget); +} + +// ====================== +// Implementation Methods +// ====================== + +std::vector Model::Impl::getJointDataSerialized( + const Model* model, + const std::vector& jointNames, + std::function getJointData) +{ + std::vector jointSerialization; + + if (!jointNames.empty()) { + jointSerialization = jointNames; + } + else { + jointSerialization = model->jointNames(); + } + + std::vector data; + data.reserve(model->dofs()); + + for (auto joint : model->joints(jointNames)) { + for (size_t dof = 0; dof < joint->dofs(); ++dof) { + data.push_back(getJointData(joint, dof)); + } + } + + return data; +} + +bool Model::Impl::setJointDataSerialized( + Model* model, + const std::vector& data, + const std::vector& jointNames, + std::function setJointData) +{ + std::vector jointSerialization; + + size_t expectedDOFs = 0; + + if (!jointNames.empty()) { + jointSerialization = jointNames; + + for (auto joint : model->joints(jointNames)) { + expectedDOFs += joint->dofs(); + } + } + else { + expectedDOFs = model->dofs(); + jointSerialization = model->jointNames(); + } + + if (data.size() != expectedDOFs) { + gymppError << "The size of the forces does not match the considered " + "joint's DOFs" + << std::endl; + return false; + } + + auto it = data.begin(); + + for (auto joint : model->joints(jointNames)) { + for (size_t dof = 0; dof < joint->dofs(); ++dof) { + if (!setJointData(joint, *it++, dof)) { + gymppError << "Failed to set force of joint '" << joint->name() + << "'" << std::endl; + return false; + } + } + } + assert(it == data.end()); + return true; +} diff --git a/cpp/scenario/gazebo/src/World.cpp b/cpp/scenario/gazebo/src/World.cpp new file mode 100644 index 000000000..188a08139 --- /dev/null +++ b/cpp/scenario/gazebo/src/World.cpp @@ -0,0 +1,327 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "scenario/gazebo/World.h" +#include "scenario/gazebo/Link.h" +#include "scenario/gazebo/Log.h" +#include "scenario/gazebo/Model.h" +#include "scenario/gazebo/components/SimulatedTime.h" +#include "scenario/gazebo/exceptions.h" +#include "scenario/gazebo/helpers.h" +#include "scenario/gazebo/utils.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace scenario::gazebo; + +class World::Impl +{ +public: + ignition::gazebo::EventManager* eventManager = nullptr; + ignition::gazebo::EntityComponentManager* ecm = nullptr; + + ignition::gazebo::Entity worldEntity = ignition::gazebo::kNullEntity; + std::shared_ptr sdfEntityCreator; + + using ModelName = std::string; + std::unordered_map models; +}; + +World::World() + : pImpl{std::make_unique()} +{} + +uint64_t World::id() const +{ + return pImpl->worldEntity; +} + +World::~World() = default; + +bool World::initialize(const ignition::gazebo::Entity worldEntity, + ignition::gazebo::EntityComponentManager* ecm, + ignition::gazebo::EventManager* eventManager) +{ + if (worldEntity == ignition::gazebo::kNullEntity || !ecm || !eventManager) { + return false; + } + + // Store the ECM resources + pImpl->ecm = ecm; + pImpl->worldEntity = worldEntity; + pImpl->eventManager = eventManager; + + // Create the SdfEntityCreator + pImpl->sdfEntityCreator = std::make_unique< // + ignition::gazebo::SdfEntityCreator>(*ecm, *eventManager); + + return true; +} + +bool World::createECMResources() +{ + return true; +} + +bool World::insertWorldPlugin(const std::string& libName, + const std::string& className, + const std::string& context) +{ + // Create a new element without context + sdf::ElementPtr pluginElement = + utils::getPluginSDFElement(libName, className); + + // Insert the context into the plugin element + if (!context.empty()) { + + // Try to get the sdf::Root (it will alredy print an error if it fails) + auto contextRoot = utils::getSdfRootFromString(context); + + if (!contextRoot) { + return false; + } + + // Get the first element of the context + // (stripping out the container) + auto contextNextElement = contextRoot->Element()->GetFirstElement(); + + // Insert the plugin context elements + while (contextNextElement) { + pluginElement->InsertElement(contextNextElement); + contextNextElement = contextNextElement->GetNextElement(); + } + } + + // The plugin element must be wrapped in another element, otherwise + // who receives it does not get the additional context + auto wrapped = sdf::SDF::WrapInRoot(pluginElement); + + // Trigger the plugin loading + pImpl->eventManager->Emit( + pImpl->worldEntity, wrapped); + + return true; +} + +double World::time() const +{ + auto simTime = utils::getExistingComponentData< // + ignition::gazebo::components::SimulatedTime>(pImpl->ecm, + pImpl->worldEntity); + + return std::chrono::duration(simTime).count(); +} + +std::string World::name() const +{ + std::string worldName = utils::getExistingComponentData< // + ignition::gazebo::components::Name>(pImpl->ecm, pImpl->worldEntity); + + return worldName; +} + +std::vector World::modelNames() const +{ + std::vector modelNames; + + pImpl->ecm->Each( + [&](const ignition::gazebo::Entity& /*entity*/, + ignition::gazebo::components::Name* nameComponent, + ignition::gazebo::components::Model* /*modelComponent*/, + ignition::gazebo::components::ParentEntity* parentEntityComponent) + -> bool { + assert(nameComponent); + assert(parentEntityComponent); + + // Discard models not belonging to this world + if (parentEntityComponent->Data() != pImpl->worldEntity) { + return true; + } + + modelNames.push_back(nameComponent->Data()); + return true; + }); + + return modelNames; +} + +scenario::gazebo::ModelPtr World::getModel(const std::string& modelName) const +{ + // Find the model entity + auto modelEntity = pImpl->ecm->EntityByComponents( + ignition::gazebo::components::Name(modelName), + ignition::gazebo::components::Model(), + ignition::gazebo::components::ParentEntity(pImpl->worldEntity)); + + if (modelEntity == ignition::gazebo::kNullEntity) { + throw exceptions::ModelNotFound(modelName); + } + + if (pImpl->models.find(modelName) != pImpl->models.end()) { + assert(pImpl->models.at(modelName)); + return pImpl->models.at(modelName); + } + + // Create the model + auto model = std::make_shared(); + model->initialize(modelEntity, pImpl->ecm, pImpl->eventManager); + + return model; +} + +bool World::insertModel(const std::string& modelFile, + const std::string& overrideModelName) +{ + // Parse the SDF string + sdf::Root modelSdfRoot; + auto errors = modelSdfRoot.Load(modelFile); + + if (!errors.empty()) { + for (const auto& err : errors) { + gymppError << err << std::endl; + } + return false; + } + + if (modelSdfRoot.ModelCount() != 1) { + gymppError << "The SDF file contains more than one model" << std::endl; + return false; + } + + constexpr size_t ModelIndex = 0; + + // Every SDF model has a name. In order to insert multiple models from the + // same SDF file, the modelData struct allows providing a scoped name. + std::string finalModelEntityName; + + // Get the final name of the model + if (overrideModelName.empty()) { + finalModelEntityName = modelSdfRoot.ModelByIndex(ModelIndex)->Name(); + } + else { + finalModelEntityName = overrideModelName; + } + + // Check for model name clash + const std::vector modelNames = this->modelNames(); + if (std::find(modelNames.begin(), modelNames.end(), finalModelEntityName) + != modelNames.end()) { + gymppError << "Failed to insert model '" << finalModelEntityName + << "'. Another entity with the same name already exists." + << std::endl; + return false; + } + + // Rename the model. + // NOTE: The following is not enough because the name is not serialized to + // string. We need also to operate directly on the raw element. + const_cast(modelSdfRoot.ModelByIndex(ModelIndex)) + ->SetName(finalModelEntityName); + + // Update the name in the sdf model. This is necessary because model plugins + // are loaded right before the creation of the model entity and, instead of + // receiving the model entity name, they receive the model sdf name. + if (!utils::renameSDFModel( + modelSdfRoot, finalModelEntityName, ModelIndex)) { + gymppError << "Failed to rename SDF model" << std::endl; + return false; + } + + if (utils::verboseFromEnvironment()) { + gymppDebug << "Inserting a model from the following SDF:" << std::endl; + std::cout << modelSdfRoot.Element()->ToString("") << std::endl; + } + + // Create the model entity + ignition::gazebo::Entity modelEntity; + modelEntity = pImpl->sdfEntityCreator->CreateEntities( + modelSdfRoot.ModelByIndex(ModelIndex)); + + // Attach the model entity to the world entity + pImpl->sdfEntityCreator->SetParent(modelEntity, pImpl->worldEntity); + + { + // Check that the model name is correct + assert(modelSdfRoot.ModelCount() == 1); + std::string modelNameSDF = + modelSdfRoot.ModelByIndex(ModelIndex)->Name(); + std::string modelNameEntity = utils::getExistingComponentData< // + ignition::gazebo::components::Name>(pImpl->ecm, modelEntity); + assert(modelNameSDF == modelNameEntity); + } + + // Create the model + auto model = std::make_shared(); + model->initialize(modelEntity, pImpl->ecm, pImpl->eventManager); + + // Create required model resources. This call prepares all the necessary + // components in the ECM to make our bindings work. + if (!model->createECMResources()) { + gymppError << "Failed to initialize ECM model resources" << std::endl; + return false; + } + + return true; +} + +bool World::removeModel(const std::string& modelName) +{ + auto modelEntity = pImpl->ecm->EntityByComponents( + ignition::gazebo::components::Name(modelName), + ignition::gazebo::components::Model(), + ignition::gazebo::components::ParentEntity(pImpl->worldEntity)); + + if (modelEntity == ignition::gazebo::kNullEntity) { + gymppError << "Model '" << modelName << "' not found in the world" + << std::endl; + return false; + } + + // Request the removal of the model + gymppDebug << "Requesting removal of entity [" << modelEntity << "]" + << std::endl; + pImpl->sdfEntityCreator->RequestRemoveEntity(modelEntity); + + // Remove the cached model + pImpl->models.erase(modelName); + + return true; +} diff --git a/cpp/scenario/gazebo/src/helpers.cpp b/cpp/scenario/gazebo/src/helpers.cpp new file mode 100644 index 000000000..e0061b333 --- /dev/null +++ b/cpp/scenario/gazebo/src/helpers.cpp @@ -0,0 +1,284 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "scenario/gazebo/helpers.h" +#include "ignition/common/Util.hh" +#include "scenario/gazebo/Log.h" + +#include +#include + +using namespace scenario::gazebo; + +std::shared_ptr +utils::getSdfRootFromFile(const std::string& sdfFileName) +{ + // NOTE: there's a double free error if we use std::optional + // auto root = std::make_optional(); + + auto root = std::make_shared(); + auto errors = root->Load(sdfFileName); + + if (!errors.empty()) { + gymppError << "Failed to load sdf file " << sdfFileName << std::endl; + + for (const auto& error : errors) { + gymppError << error << std::endl; + } + return {}; + } + + return root; +} + +std::shared_ptr +utils::getSdfRootFromString(const std::string& sdfString) +{ + // NOTE: there's a double free error if we use std::optional + // auto root = std::make_optional(); + + auto root = std::make_shared(); + auto errors = root->LoadSdfString(sdfString); + + if (!errors.empty()) { + gymppError << "Failed to load sdf string" << std::endl; + + for (const auto& error : errors) { + gymppError << error << std::endl; + } + return {}; + } + + return root; +} + +bool utils::verboseFromEnvironment() +{ + std::string envVarContent; + ignition::common::env(ScenarioVerboseEnvVar, envVarContent); + + return envVarContent == "1"; +} + +scenario::base::Pose +utils::fromIgnitionPose(const ignition::math::Pose3d& ignitionPose) +{ + base::Pose pose; + + pose.position[0] = ignitionPose.Pos().X(); + pose.position[1] = ignitionPose.Pos().Y(); + pose.position[2] = ignitionPose.Pos().Z(); + + pose.orientation[0] = ignitionPose.Rot().W(); + pose.orientation[1] = ignitionPose.Rot().X(); + pose.orientation[2] = ignitionPose.Rot().Y(); + pose.orientation[3] = ignitionPose.Rot().Z(); + + return pose; +} + +ignition::math::Pose3d utils::toIgnitionPose(const scenario::base::Pose& pose) +{ + ignition::math::Pose3d ignitionPose; + + ignitionPose.Pos() = ignition::math::Vector3d( + pose.position[0], pose.position[1], pose.position[2]); + + ignitionPose.Rot() = ignition::math::Quaterniond(pose.orientation[0], + pose.orientation[1], + pose.orientation[2], + pose.orientation[3]); + + return ignitionPose; +} + +std::vector +utils::fromIgnitionContactsMsgs(ignition::gazebo::EntityComponentManager* ecm, + const ignition::msgs::Contacts& contactsMsg) +{ + auto getEntityName = + [&](const ignition::gazebo::Entity entity) -> std::string { + auto nameComponent = + ecm->Component(entity); + assert(nameComponent); + return nameComponent->Data(); + }; + + std::vector contacts; + + for (int i = 0; i < contactsMsg.contact_size(); ++i) { + // Extract the contact object + const ignition::msgs::Contact& contactData = contactsMsg.contact(i); + + auto collisionEntityA = contactData.collision1().id(); + auto collisionEntityB = contactData.collision2().id(); + + auto linkEntityA = ecm->ParentEntity(collisionEntityA); + auto linkEntityB = ecm->ParentEntity(collisionEntityB); + std::string linkNameA = getEntityName(linkEntityA); + std::string linkNameB = getEntityName(linkEntityB); + + auto modelEntityA = ecm->ParentEntity(linkEntityA); + auto modelEntityB = ecm->ParentEntity(linkEntityB); + std::string modelNameA = getEntityName(modelEntityA); + std::string modelNameB = getEntityName(modelEntityB); + + std::string scopedBodyA = modelNameA + "::" + linkNameA; + std::string scopedBodyB = modelNameB + "::" + linkNameB; + + // Fill the contact data + base::ContactData contact; + contact.bodyA = scopedBodyA; + contact.bodyB = scopedBodyB; + contact.position[0] = contactData.position(i).x(); + contact.position[1] = contactData.position(i).y(); + contact.position[2] = contactData.position(i).z(); + + // TODO: fill the normal, contacty depth, wrench magnitude + assert(contactData.depth_size() == 0); + assert(contactData.normal_size() == 0); + assert(contactData.wrench_size() == 0); + + // Add the new contact data to the contacts vector + contacts.push_back(contact); + } + + return contacts; +} + +bool utils::renameSDFModel(sdf::Root& sdfRoot, + const std::string& newModelName, + size_t modelIndex) +{ + const size_t initialNrOfModels = sdfRoot.ModelCount(); + + // Create a new model with the scoped name + sdf::ElementPtr renamedModel(new sdf::Element); + renamedModel->SetName("model"); + renamedModel->AddAttribute("name", "string", newModelName, true); + + // Get the first child of the original model element + sdf::ElementPtr child = + sdfRoot.ModelByIndex(modelIndex)->Element()->GetFirstElement(); + + // Add all the children to the renamed model element + while (child) { + renamedModel->InsertElement(child); + child->SetParent(renamedModel); + child = child->GetNextElement(); + } + + // Remove the old model + auto originalModelElement = sdfRoot.ModelByIndex(modelIndex)->Element(); + originalModelElement->RemoveFromParent(); + + // Insert the renamed model + renamedModel->SetParent(sdfRoot.Element()); + sdfRoot.Element()->InsertElement(renamedModel); + + if (sdfRoot.ModelCount() != initialNrOfModels) { + gymppError << "Failed to rename SDF model" << std::endl; + return false; + } + + if (!sdfRoot.ModelNameExists(newModelName)) { + gymppError << "Failed to insert rename model in SDF root" << std::endl; + return false; + } + + return true; +} + +sdf::ElementPtr utils::getPluginSDFElement(const std::string& libName, + const std::string& className) +{ + // Create the plugin SDF element + auto pluginElement = std::make_shared(); + + // Initialize the attributes + pluginElement->SetName("plugin"); + pluginElement->AddAttribute( + "name", "string", className, true, "plugin name"); + pluginElement->AddAttribute( + "filename", "string", libName, true, "pluginfilename"); + + // Create the plugin description + pluginElement->AddElementDescription(pluginElement->Clone()); + + return pluginElement; +} + +sdf::JointType utils::toSdf(const scenario::base::JointType type) +{ + sdf::JointType sdfType; + + switch (type) { + case base::JointType::Fixed: + sdfType = sdf::JointType::FIXED; + break; + case base::JointType::Revolute: + sdfType = sdf::JointType::REVOLUTE; + break; + case base::JointType::Prismatic: + sdfType = sdf::JointType::PRISMATIC; + break; + case base::JointType::Ball: + sdfType = sdf::JointType::BALL; + break; + default: + gymppError << "Joint type not recognized" << std::endl; + sdfType = sdf::JointType::INVALID; + break; + } + + return sdfType; +} + +scenario::base::JointType utils::fromSdf(const sdf::JointType sdfType) +{ + base::JointType type; + + switch (sdfType) { + case sdf::JointType::FIXED: + type = base::JointType::Fixed; + break; + case sdf::JointType::REVOLUTE: + type = base::JointType::Revolute; + break; + case sdf::JointType::PRISMATIC: + type = base::JointType::Prismatic; + break; + case sdf::JointType::BALL: + type = base::JointType::Ball; + break; + default: + gymppError << "Joint type not recognized" << std::endl; + type = base::JointType::Invalid; + break; + } + + return type; +} diff --git a/cpp/scenario/gazebo/src/utils.cpp b/cpp/scenario/gazebo/src/utils.cpp new file mode 100644 index 000000000..0b818552e --- /dev/null +++ b/cpp/scenario/gazebo/src/utils.cpp @@ -0,0 +1,140 @@ +/* + * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This project is dual licensed under LGPL v2.1+ or Apachi License. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + * + * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "scenario/gazebo/utils.h" +#include "scenario/gazebo/Log.h" +#include "scenario/gazebo/helpers.h" + +#include +#include +#include +#include +#include + +#include + +using namespace scenario::gazebo; + +void utils::setVerbosity(const int level) +{ + ignition::common::Console::SetVerbosity(level); +} + +std::string utils::findSdfFile(const std::string& fileName) +{ + if (fileName.empty()) { + gymppError << "The SDF file name is empty" << std::endl; + return {}; + } + + ignition::common::SystemPaths systemPaths; + systemPaths.SetFilePathEnv("IGN_GAZEBO_RESOURCE_PATH"); + systemPaths.AddFilePaths(IGN_GAZEBO_WORLD_INSTALL_DIR); + + // Find the file + std::string sdfFilePath = systemPaths.FindFile(fileName); + + if (sdfFilePath.empty()) { + gymppError << "Failed to find " << fileName << std::endl; + gymppError << "Check that it is part of IGN_GAZEBO_RESOURCE_PATH" + << std::endl; + return {}; + } + + return sdfFilePath; +} + +bool utils::sdfStringValid(const std::string& sdfString) +{ + return bool(getSdfRootFromString(sdfString)); +} + +std::string utils::getSdfString(const std::string& fileName) +{ + auto path = std::filesystem::path(fileName); + + if (!std::filesystem::exists(path)) { + path = std::filesystem::path(findSdfFile(fileName)); + } + + if (path.empty()) { + return {}; + } + + auto root = getSdfRootFromString(path.string()); + + if (!root) { + return {}; + } + + return root->Element()->ToString(""); +} + +std::string utils::getModelNameFromSdf(const std::string& fileName, + const size_t modelIndex) +{ + auto root = utils::getSdfRootFromFile(fileName); + + if (!root) { + return {}; + } + + if (root->ModelCount() == 0) { + gymppError << "Didn't find any model in file " << fileName << std::endl; + return {}; + } + + return root->ModelByIndex(modelIndex)->Name(); +} + +std::string utils::getEmptyWorld() +{ + const std::string world = R""""( + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + )""""; + + assert(sdfStringValid(world)); + return world; +}