Merge pull request #167 from diegoferigo/refactor/fixed_base_computed…

…_torque_controller

Custom controllers: base interfaces, plugins, and fixed-base Computed Torque example

cpp/scenario/controllers/CMakeLists.txt

@@ -0,0 +1,86 @@
+# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+# All rights reserved.
+#
+#  This project is dual licensed under LGPL v2.1+ or Apache 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.
+
+find_package(iDynTree REQUIRED)
+find_package (Eigen3 3.3 REQUIRED NO_MODULE)
+
+# ==========
+# Controller
+# ==========
+
+set(CONTROLLER_HDRS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/controllers/Controller.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/scenario/controllers/References.h)
+
+add_library(Controller INTERFACE)
+target_sources(Controller INTERFACE ${CONTROLLER_HDRS})
+
+target_include_directories(Controller INTERFACE
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+set_target_properties(Controller PROPERTIES PUBLIC_HEADER "${CONTROLLER_HDRS}")
+
+# =======================
+# ComputedTorqueFixedBase
+# =======================
+
+add_library(ComputedTorqueFixedBase
+    include/scenario/controllers/ComputedTorqueFixedBase.h
+    src/ComputedTorqueFixedBase.cpp)
+
+target_link_libraries(ComputedTorqueFixedBase
+    PUBLIC
+    Controller
+    PRIVATE
+    ScenarioGazebo
+    Eigen3::Eigen
+    ignition-gazebo3::core
+    iDynTree::idyntree-core
+    iDynTree::idyntree-model
+    iDynTree::idyntree-modelio-urdf
+    iDynTree::idyntree-high-level)
+
+target_include_directories(ComputedTorqueFixedBase PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+set_target_properties(ComputedTorqueFixedBase PROPERTIES
+    PUBLIC_HEADER include/scenario/controllers/ComputedTorqueFixedBase.h)
+
+# ===================
+# Install the targets
+# ===================
+
+if(NOT CMAKE_BUILD_TYPE STREQUAL "PyPI")
+    install(
+        TARGETS
+        Controller
+        ComputedTorqueFixedBase
+        EXPORT scenario
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+        PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/scenario/controllers)
+endif()

cpp/scenario/controllers/include/scenario/controllers/ComputedTorqueFixedBase.h

@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache 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_CONTROLLERS_COMPUTEDTORQUEFIXEDBASE_H
+#define SCENARIO_CONTROLLERS_COMPUTEDTORQUEFIXEDBASE_H
+
+#include "scenario/controllers/Controller.h"
+#include "scenario/controllers/References.h"
+
+#include <array>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace scenario {
+    namespace gazebo {
+        class Model;
+    } // namespace gazebo
+    namespace controllers {
+        class ControllersFactory;
+        class ComputedTorqueFixedBase;
+    } // namespace controllers
+} // namespace scenario
+
+class scenario::controllers::ComputedTorqueFixedBase final
+    : public scenario::controllers::Controller
+    , public scenario::controllers::UseScenarioModel
+    , public scenario::controllers::SetJointReferences
+{
+public:
+    ComputedTorqueFixedBase() = delete;
+    ComputedTorqueFixedBase(const std::string& urdfFile,
+                            std::shared_ptr<gazebo::Model> model,
+                            const std::vector<double>& kp,
+                            const std::vector<double>& kd,
+                            const std::vector<std::string>& controlledJoints,
+                            const std::array<double, 3> gravity = g);
+    ~ComputedTorqueFixedBase() override;
+
+    bool initialize() override;
+    bool step(const StepSize& dt) override;
+    bool terminate() override;
+
+    bool updateStateFromModel() override;
+
+    const std::vector<std::string>& controlledJoints() override;
+    bool setJointReferences(const JointReferences& jointReferences) override;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pImpl;
+};
+
+#endif // SCENARIO_CONTROLLERS_COMPUTEDTORQUEFIXEDBASE_H

cpp/scenario/controllers/include/scenario/controllers/Controller.h

@@ -0,0 +1,100 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache 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_CONTROLLERS_CONTROLLER_H
+#define SCENARIO_CONTROLLERS_CONTROLLER_H
+
+#include "scenario/controllers/References.h"
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <vector>
+
+namespace scenario {
+    namespace controllers {
+        class Controller;
+        class UseScenarioModel;
+        class SetBaseReferences;
+        class SetJointReferences;
+        using ControllerPtr = std::shared_ptr<Controller>;
+        constexpr std::array<double, 3> g = {0, 0, -9.80665};
+    } // namespace controllers
+    namespace gazebo {
+        class Model;
+        using ModelPtr = std::shared_ptr<Model>;
+    } // namespace gazebo
+} // namespace scenario
+
+class scenario::controllers::Controller
+    : public std::enable_shared_from_this<scenario::controllers::Controller>
+{
+public:
+    using StepSize = std::chrono::duration<double>;
+
+    Controller() = default;
+    virtual ~Controller() = default;
+
+    virtual bool initialize() = 0;
+    virtual bool step(const StepSize& dt) = 0;
+    virtual bool terminate() = 0;
+};
+
+class scenario::controllers::UseScenarioModel
+{
+public:
+    UseScenarioModel() = default;
+    virtual ~UseScenarioModel() = default;
+
+    virtual bool updateStateFromModel() = 0;
+
+protected:
+    gazebo::ModelPtr m_model;
+};
+
+class scenario::controllers::SetBaseReferences
+{
+public:
+    SetBaseReferences() = default;
+    virtual ~SetBaseReferences() = default;
+
+    virtual bool setBaseReferences(const BaseReferences& jointReferences) = 0;
+};
+
+class scenario::controllers::SetJointReferences
+{
+public:
+    SetJointReferences() = default;
+    virtual ~SetJointReferences() = default;
+
+    virtual const std::vector<std::string>& controlledJoints() = 0;
+    virtual bool setJointReferences(const JointReferences& jointReferences) = 0;
+
+protected:
+    std::vector<std::string> m_controlledJoints;
+};
+
+#endif // SCENARIO_CONTROLLERS_CONTROLLER_H

cpp/scenario/controllers/include/scenario/controllers/References.h

@@ -0,0 +1,71 @@
+/*
+ * Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This project is dual licensed under LGPL v2.1+ or Apache 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_CONTROLLERS_REFERENCES_H
+#define SCENARIO_CONTROLLERS_REFERENCES_H
+
+#include <array>
+#include <vector>
+
+namespace scenario {
+    namespace controllers {
+        struct BaseReferences;
+        struct JointReferences;
+    } // namespace controllers
+} // namespace scenario
+
+struct scenario::controllers::BaseReferences
+{
+    std::array<double, 3> position = {0, 0, 0};
+    std::array<double, 4> orientation = {1, 0, 0, 0};
+    std::array<double, 3> linearVelocity = {0, 0, 0};
+    std::array<double, 3> angularVelocity = {0, 0, 0};
+    std::array<double, 3> linearAcceleration = {0, 0, 0};
+    std::array<double, 3> angularAcceleration = {0, 0, 0};
+};
+
+struct scenario::controllers::JointReferences
+{
+    JointReferences(const size_t controlledDofs = 0)
+    {
+        position = std::vector<double>(controlledDofs, 0.0);
+        velocity = std::vector<double>(controlledDofs, 0.0);
+        acceleration = std::vector<double>(controlledDofs, 0.0);
+    }
+
+    inline bool valid() const
+    {
+        size_t dofs = position.size();
+        return dofs > 0 && velocity.size() == dofs
+               && acceleration.size() == dofs;
+    }
+
+    std::vector<double> position;
+    std::vector<double> velocity;
+    std::vector<double> acceleration;
+};
+
+#endif // SCENARIO_CONTROLLERS_REFERENCES_H