Skip to content

Commit d414aaf

Browse files
GiulioRomualdiGiulioRomualdi
authored
Add PositionToJointController and PositionToTorqueController (#1003)
* Add PositionToJointController and PositionToTorqueController - Introduced PositionToJointController as a base class for joint controllers. - Implemented PositionToCurrentController and PositionToTorqueController inheriting from PositionToJointController. - Updated input structures to use PositionToJointControllerInput for consistency. - Added necessary bindings for Python integration. - Created unit tests for PositionToTorqueController covering initialization, basic control, torque limiting, and input validation. - Removed obsolete PositionToCurrentController header from RobotInterface. * Fix formatting in PositionToTorqueController documentation
1 parent 38bb145 commit d414aaf

14 files changed

Lines changed: 769 additions & 60 deletions

File tree

‎bindings/python/JointLevelControllers/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,8 @@ if(TARGET BipedalLocomotion::JointLevelControllers)
77

88
add_bipedal_locomotion_python_module(
99
NAME JointLevelControllersBindings
10-
SOURCES src/PositionToCurrentController.cpp src/EMAWithLimits.cpp src/Module.cpp
11-
HEADERS ${H_PREFIX}/PositionToCurrentController.h ${H_PREFIX}/EMAWithLimits.h ${H_PREFIX}/Module.h
10+
SOURCES src/PositionToJointController.cpp src/EMAWithLimits.cpp src/Module.cpp
11+
HEADERS ${H_PREFIX}/PositionToJointController.h ${H_PREFIX}/EMAWithLimits.h ${H_PREFIX}/Module.h
1212
LINK_LIBRARIES BipedalLocomotion::JointLevelControllers
1313
)
1414
endif()

bindings/python/JointLevelControllers/include/BipedalLocomotion/bindings/JointLevelControllers/PositionToCurrentController.h renamed to bindings/python/JointLevelControllers/include/BipedalLocomotion/bindings/JointLevelControllers/PositionToJointController.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,9 @@ namespace bindings
1717
namespace JointLevelControllers
1818
{
1919

20+
void CreatePositionToTorqueController(pybind11::module& module);
2021
void CreatePositionToCurrentController(pybind11::module& module);
22+
void CreatePositionToJointController(pybind11::module& module);
2123

2224
} // namespace JointLevelControllers
2325
} // namespace bindings

‎bindings/python/JointLevelControllers/src/Module.cpp‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99

1010
#include <BipedalLocomotion/bindings/JointLevelControllers/EMAWithLimits.h>
1111
#include <BipedalLocomotion/bindings/JointLevelControllers/Module.h>
12-
#include <BipedalLocomotion/bindings/JointLevelControllers/PositionToCurrentController.h>
12+
#include <BipedalLocomotion/bindings/JointLevelControllers/PositionToJointController.h>
1313

1414
namespace BipedalLocomotion
1515
{
@@ -21,7 +21,9 @@ void CreateModule(pybind11::module& module)
2121
{
2222
module.doc() = "Controllers for robot joints";
2323

24+
CreatePositionToJointController(module);
2425
CreatePositionToCurrentController(module);
26+
CreatePositionToTorqueController(module);
2527
CreateEMAWithLimits(module);
2628
}
2729
} // namespace JointLevelControllers

bindings/python/JointLevelControllers/src/PositionToCurrentController.cpp renamed to bindings/python/JointLevelControllers/src/PositionToJointController.cpp

Lines changed: 31 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,11 @@
1010
#include <pybind11/stl.h>
1111

1212
#include <BipedalLocomotion/JointLevelControllers/PositionToCurrentController.h>
13+
#include <BipedalLocomotion/JointLevelControllers/PositionToJointController.h>
14+
#include <BipedalLocomotion/JointLevelControllers/PositionToTorqueController.h>
1315
#include <BipedalLocomotion/System/Advanceable.h>
1416

15-
#include <BipedalLocomotion/bindings/JointLevelControllers/PositionToCurrentController.h>
17+
#include <BipedalLocomotion/bindings/JointLevelControllers/PositionToJointController.h>
1618
#include <BipedalLocomotion/bindings/System/Advanceable.h>
1719

1820
namespace BipedalLocomotion
@@ -22,28 +24,47 @@ namespace bindings
2224
namespace JointLevelControllers
2325
{
2426

25-
void CreatePositionToCurrentController(pybind11::module& module)
27+
void CreatePositionToJointController(pybind11::module& module)
2628
{
2729
using namespace ::BipedalLocomotion::JointLevelControllers;
2830
using namespace ::BipedalLocomotion::System;
2931
namespace py = ::pybind11;
3032

31-
py::class_<PositionToCurrentControllerInput>(module, "PositionToCurrentControllerInput")
33+
py::class_<PositionToJointControllerInput>(module, "PositionToJointControllerInput")
3234
.def(py::init())
33-
.def_readwrite("reference_position", &PositionToCurrentControllerInput::referencePosition)
34-
.def_readwrite("feedback_position", &PositionToCurrentControllerInput::feedbackPosition)
35-
.def_readwrite("feedback_velocity", &PositionToCurrentControllerInput::feedbackVelocity);
35+
.def_readwrite("reference_position", &PositionToJointControllerInput::referencePosition)
36+
.def_readwrite("feedback_position", &PositionToJointControllerInput::feedbackPosition)
37+
.def_readwrite("feedback_velocity", &PositionToJointControllerInput::feedbackVelocity);
3638

37-
BipedalLocomotion::bindings::System::CreateAdvanceable<PositionToCurrentControllerInput,
39+
BipedalLocomotion::bindings::System::CreateAdvanceable<PositionToJointControllerInput,
3840
::Eigen::VectorXd>( //
3941
module,
40-
"PositionToCurrentController");
42+
"PositionToJointController");
43+
py::class_<::BipedalLocomotion::JointLevelControllers::PositionToJointController,
44+
::BipedalLocomotion::System::Advanceable<PositionToJointControllerInput,
45+
::Eigen::VectorXd>> //
46+
(module, "PositionToJointController");
47+
}
48+
49+
void CreatePositionToCurrentController(pybind11::module& module)
50+
{
4151

52+
using namespace ::BipedalLocomotion::JointLevelControllers;
53+
namespace py = ::pybind11;
4254
py::class_<::BipedalLocomotion::JointLevelControllers::PositionToCurrentController,
43-
::BipedalLocomotion::System::Advanceable<PositionToCurrentControllerInput,
44-
::Eigen::VectorXd>> //
55+
::BipedalLocomotion::JointLevelControllers::PositionToJointController> //
4556
(module, "PositionToCurrentController").def(py::init());
4657
}
58+
59+
void CreatePositionToTorqueController(pybind11::module& module)
60+
{
61+
62+
using namespace ::BipedalLocomotion::JointLevelControllers;
63+
namespace py = ::pybind11;
64+
py::class_<::BipedalLocomotion::JointLevelControllers::PositionToTorqueController,
65+
::BipedalLocomotion::JointLevelControllers::PositionToJointController> //
66+
(module, "PositionToTorqueController").def(py::init());
67+
}
4768
} // namespace JointLevelControllers
4869
} // namespace bindings
4970
} // namespace BipedalLocomotion

‎src/JointLevelControllers/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,8 @@ if(FRAMEWORK_COMPILE_JointLevelControllers)
88

99
add_bipedal_locomotion_library(
1010
NAME JointLevelControllers
11-
PUBLIC_HEADERS ${H_PREFIX}/PositionToCurrentController.h ${H_PREFIX}/EMAWithLimits.h
12-
SOURCES src/PositionToCurrentController.cpp src/EMAWithLimits.cpp
11+
PUBLIC_HEADERS ${H_PREFIX}/PositionToJointController.h ${H_PREFIX}/PositionToCurrentController.h ${H_PREFIX}/PositionToTorqueController.h ${H_PREFIX}/EMAWithLimits.h
12+
SOURCES src/PositionToCurrentController.cpp src/PositionToTorqueController.cpp src/EMAWithLimits.cpp
1313
PUBLIC_LINK_LIBRARIES Eigen3::Eigen BipedalLocomotion::ParametersHandler BipedalLocomotion::System
1414
SUBDIRECTORIES tests)
1515

‎src/JointLevelControllers/include/BipedalLocomotion/JointLevelControllers/PositionToCurrentController.h‎

Lines changed: 3 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,7 @@
1313

1414
#include <Eigen/Dense>
1515

16+
#include <BipedalLocomotion/JointLevelControllers/PositionToJointController.h>
1617
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
1718
#include <BipedalLocomotion/System/Advanceable.h>
1819

@@ -21,20 +22,6 @@ namespace BipedalLocomotion
2122
namespace JointLevelControllers
2223
{
2324

24-
/**
25-
* @brief Input data structure for the PositionToCurrentController
26-
*
27-
* This structure contains all the feedback and reference signals required by the controller
28-
* to compute the appropriate motor current commands. All vectors must have the same size
29-
* corresponding to the number of controlled joints.
30-
*/
31-
struct PositionToCurrentControllerInput
32-
{
33-
Eigen::VectorXd referencePosition; /**< Desired joint positions [rad] */
34-
Eigen::VectorXd feedbackPosition; /**< Current joint positions from encoders [rad] */
35-
Eigen::VectorXd feedbackVelocity; /**< Current joint velocities from encoders [rad/s] */
36-
};
37-
3825
// clang-format off
3926
/**
4027
* @brief Position-to-Current Controller with Friction Compensation and TN-Curve Limiting
@@ -96,9 +83,7 @@ struct PositionToCurrentControllerInput
9683
* - \f$ b = I_{max} - m \cdot \omega_{rated} \f$ is the intercept
9784
*
9885
*/
99-
class PositionToCurrentController
100-
: public BipedalLocomotion::System::Advanceable<PositionToCurrentControllerInput,
101-
Eigen::VectorXd>
86+
class PositionToCurrentController: public PositionToJointController
10287
{
10388
// clang-format on
10489
public:
@@ -154,7 +139,7 @@ class PositionToCurrentController
154139
* @param input the input of the controller
155140
* @return True in case of success and false otherwise
156141
*/
157-
bool setInput(const PositionToCurrentControllerInput& input) override;
142+
bool setInput(const PositionToJointControllerInput& input) override;
158143

159144
/**
160145
* Get the output of the controller

‎src/JointLevelControllers/include/BipedalLocomotion/JointLevelControllers/PositionToJointController.h‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
/**
2+
* @file PositionToJointController.h
3+
* @authors Giulio Romualdi
4+
* @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
5+
* distributed under the terms of the BSD-3-Clause license.
6+
*/
7+
8+
#ifndef BIPEDAL_LOCOMOTION_JOINT_LEVEL_CONTROLLERS_POSITION_TO_JOINT_CONTROLLER_H
9+
#define BIPEDAL_LOCOMOTION_JOINT_LEVEL_CONTROLLERS_POSITION_TO_JOINT_CONTROLLER_H
10+
11+
#include <memory>
12+
#include <optional>
13+
14+
#include <Eigen/Dense>
15+
16+
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
17+
#include <BipedalLocomotion/System/Advanceable.h>
18+
19+
namespace BipedalLocomotion
20+
{
21+
namespace JointLevelControllers
22+
{
23+
24+
/**
25+
* @brief Input data structure for the PositionToJointController
26+
*
27+
* This structure contains all the feedback and reference signals required by the controller
28+
* to compute the appropriate motor joint commands. All vectors must have the same size
29+
* corresponding to the number of controlled joints.
30+
*/
31+
struct PositionToJointControllerInput
32+
{
33+
Eigen::VectorXd referencePosition; /**< Desired joint positions [rad] */
34+
Eigen::VectorXd feedbackPosition; /**< Current joint positions from encoders [rad] */
35+
Eigen::VectorXd feedbackVelocity; /**< Current joint velocities from encoders [rad/s] */
36+
};
37+
38+
/**
39+
* Base class for Position-to-Joint Controllers.
40+
*/
41+
class PositionToJointController
42+
: public BipedalLocomotion::System::Advanceable<PositionToJointControllerInput, Eigen::VectorXd>
43+
{
44+
};
45+
46+
} // namespace JointLevelControllers
47+
} // namespace BipedalLocomotion
48+
49+
#endif // BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_POSITION_TO_JOINT_CONTROLLER_H

‎src/JointLevelControllers/include/BipedalLocomotion/JointLevelControllers/PositionToTorqueController.h‎

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
/**
2+
* @file PositionToTorqueController.h
3+
* @authors Giulio Romualdi
4+
* @copyright 2025 Istituto Italiano di Tecnologia (IIT). This software may be modified and
5+
* distributed under the terms of the BSD-3-Clause license.
6+
*/
7+
8+
#ifndef BIPEDAL_LOCOMOTION_JOINT_LEVEL_CONTROLLERS_POSITION_TO_TORQUE_CONTROLLER_H
9+
#define BIPEDAL_LOCOMOTION_JOINT_LEVEL_CONTROLLERS_POSITION_TO_TORQUE_CONTROLLER_H
10+
11+
#include <memory>
12+
#include <optional>
13+
14+
#include <Eigen/Dense>
15+
16+
#include <BipedalLocomotion/JointLevelControllers/PositionToJointController.h>
17+
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
18+
#include <BipedalLocomotion/System/Advanceable.h>
19+
20+
namespace BipedalLocomotion
21+
{
22+
namespace JointLevelControllers
23+
{
24+
25+
// clang-format off
26+
/**
27+
* @brief Position-to-torque Controller
28+
*
29+
* The PositionToTorqueController implements a position control strategy that directly
30+
* computes joint torque commands from position errors.
31+
*
32+
* The controller implements a proportional-derivative position control law:
33+
* \f[
34+
* \tau = K_p \cdot (q_{ref} - q_{fb}) + K_d \cdot (0 - \dot{q}_{fb})
35+
* \f]
36+
* where:
37+
* - \f$ \tau \f$ is the output torque [Nm]
38+
* - \f$ K_p \f$ is the proportional gain [Nm/rad]
39+
* - \f$ K_d \f$ is the derivative gain [Nm/(rad/s)]
40+
* - \f$ q_{ref} \f$ is the reference position [rad]
41+
* - \f$ q_{fb} \f$ is the feedback position [rad]
42+
* - \f$ \dot{q}_{fb} \f$ is the feedback velocity [rad/s]
43+
*
44+
*/
45+
class PositionToTorqueController: public PositionToJointController
46+
{
47+
// clang-format on
48+
public:
49+
/**
50+
* @brief Default constructor
51+
*/
52+
PositionToTorqueController();
53+
54+
/**
55+
* @brief Default destructor
56+
*/
57+
virtual ~PositionToTorqueController();
58+
59+
/**
60+
* @brief Advance the controller
61+
* @param input The input to the controller
62+
* @return The output of the controller
63+
*/
64+
bool advance() override;
65+
66+
// clang-format off
67+
/**
68+
* @brief Initialize the advanceable
69+
* @param handler A weak pointer to the parameters handler
70+
* @note The parameters handler should contain the following parameters:
71+
* | Parameter Name | Type | Description | Mandatory |
72+
* |:-----------------------:|:----------------:|:-----------------------------------------------------------------:|:---------:|
73+
* | `joints_list` | `vector<string>` | List of joints to control | Yes |
74+
* Moreover the following group parameters are required:
75+
* | Group Name | Description | Mandatory |
76+
* |:--------------:|:------------------------------------------------------------------------------------------------------------:|:---------:|
77+
* | `kp` | An element for each joint contained in the `joints_list` parameter. Proportional gain for each joint | Yes |
78+
* | `kd` | An element for each joint contained in the `joints_list` parameter. Derivative gain for each joint | Yes |
79+
*
80+
* Furthermore, the following optional parameters are supported:
81+
* | Group Name | Description | Mandatory |
82+
* |:---------------------:|:---------------------------------------------------------------------------------------------------------:|:---------:|
83+
* | `torque_limit` | An element for each joint contained in the `joints_list` parameter. Torque limit for each joint [Nm] | No |
84+
* @return True if the initialization is successful.
85+
*/
86+
bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler) override;
87+
// clang-format on
88+
89+
/**
90+
* Set the input of the port
91+
* @param input the input of the controller
92+
* @return True in case of success and false otherwise
93+
*/
94+
bool setInput(const PositionToJointControllerInput& input) override;
95+
96+
/**
97+
* Get the output of the controller
98+
* @return The output of the controller
99+
*/
100+
Eigen::VectorXd& getOutput() const override;
101+
102+
/**
103+
* Check if the output is valid
104+
* @return True if the output is valid, false otherwise
105+
*/
106+
bool isOutputValid() const override;
107+
108+
private:
109+
struct Impl; /**< Forward declaration of the implementation details */
110+
std::unique_ptr<Impl> m_pimpl; /**< Pointer to the implementation details */
111+
};
112+
113+
} // namespace JointLevelControllers
114+
} // namespace BipedalLocomotion
115+
116+
#endif // BIPEDAL_LOCOMOTION_ROBOT_INTERFACE_POSITION_TO_TORQUE_CONTROLLER_H

‎src/JointLevelControllers/src/PositionToCurrentController.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ using namespace BipedalLocomotion::JointLevelControllers;
1414
struct PositionToCurrentController::Impl
1515
{
1616
Eigen::VectorXd output; /**< Output current for each joint */
17-
PositionToCurrentControllerInput input; /**< Input data for the controller */
17+
PositionToJointControllerInput input; /**< Input data for the controller */
1818
bool outputValid{false}; /**< Flag indicating if the output is valid */
1919

2020
Eigen::VectorXd kp; /**< Proportional gain for each joint */
@@ -266,7 +266,7 @@ bool PositionToCurrentController::initialize(
266266
return true;
267267
}
268268

269-
bool PositionToCurrentController::setInput(const PositionToCurrentControllerInput& input)
269+
bool PositionToCurrentController::setInput(const PositionToJointControllerInput& input)
270270
{
271271
constexpr auto logPrefix = "[PositionToCurrentController::setInput] ";
272272
const auto sz = m_pimpl->kp.size();

0 commit comments

Comments
 (0)