-
Notifications
You must be signed in to change notification settings - Fork 26
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #167 from diegoferigo/refactor/fixed_base_computed…
…_torque_controller Custom controllers: base interfaces, plugins, and fixed-base Computed Torque example
- Loading branch information
Showing
11 changed files
with
1,514 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() |
77 changes: 77 additions & 0 deletions
77
cpp/scenario/controllers/include/scenario/controllers/ComputedTorqueFixedBase.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,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 |
100 changes: 100 additions & 0 deletions
100
cpp/scenario/controllers/include/scenario/controllers/Controller.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,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 |
71 changes: 71 additions & 0 deletions
71
cpp/scenario/controllers/include/scenario/controllers/References.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,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 |
Oops, something went wrong.