Skip to content

Commit

Permalink
Add joint velocity limits task (ami-iit#879)
Browse files Browse the repository at this point in the history
  • Loading branch information
davidegorbani authored Sep 4, 2024
1 parent 4e85d4d commit 40b9eb1
Show file tree
Hide file tree
Showing 11 changed files with 519 additions and 4 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ All notable changes to this project are documented in this file.
- Add launch parameter to `blf-logger-with-audio.sh` script to set logger launch file (https://github.com/ami-iit/bipedal-locomotion-framework/pull/867)
- Add `getJointLimits` function to set get actuated joints position limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/868)
- Add the possibility to disable streaming of joint encoder acceleration measurements (https://github.com/ami-iit/bipedal-locomotion-framework/pull/876)
- Add a task in the IK to set the joint velocity limits (https://github.com/ami-iit/bipedal-locomotion-framework/pull/879)

### Changed
- 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810)
Expand Down
4 changes: 2 additions & 2 deletions bindings/python/IK/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,8 @@ if(TARGET BipedalLocomotion::IK)

add_bipedal_locomotion_python_module(
NAME IKBindings
SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp src/GravityTask.cpp
HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h
SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/JointLimitsTask.cpp src/AngularMomentumTask.cpp src/Module.cpp src/IKLinearTask.cpp src/DistanceTask.cpp src/GravityTask.cpp src/JointVelocityLimitsTask.cpp
HEADERS ${H_PREFIX}/IntegrationBasedIK.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h ${H_PREFIX}/JointVelocityLimitsTask.h
LINK_LIBRARIES BipedalLocomotion::IK MANIF::manif
TESTS tests/test_QP_inverse_kinematics.py
TESTS_RUNTIME_CONDITIONS FRAMEWORK_USE_icub-models
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file JointVelocityLimitsTask.h
* @authors Davide Gorbani
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_VELOCITY_LIMITS_TASK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_VELOCITY_LIMITS_TASK_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateJointVelocityLimitsTask(pybind11::module& module);

} // namespace IK
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_VELOCITY_LIMITS_TASK_H
38 changes: 38 additions & 0 deletions bindings/python/IK/src/JointVelocityLimitsTask.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
/**
* @file JointVelocityLimitsTask.cpp
* @authors Davide Gorbani
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <BipedalLocomotion/IK/IKLinearTask.h>
#include <BipedalLocomotion/IK/JointVelocityLimitsTask.h>

#include <BipedalLocomotion/bindings/IK/JointVelocityLimitsTask.h>
#include <BipedalLocomotion/bindings/System/LinearTask.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateJointVelocityLimitsTask(pybind11::module& module)
{
namespace py = ::pybind11;
using namespace BipedalLocomotion::IK;

py::class_<JointVelocityLimitsTask,
std::shared_ptr<JointVelocityLimitsTask>,
IKLinearTask>(module, "JointVelocityLimitsTask")
.def(py::init());
}

} // namespace IK
} // namespace bindings
} // namespace BipedalLocomotion
2 changes: 2 additions & 0 deletions bindings/python/IK/src/Module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include <BipedalLocomotion/bindings/IK/IntegrationBasedIK.h>
#include <BipedalLocomotion/bindings/IK/JointLimitsTask.h>
#include <BipedalLocomotion/bindings/IK/JointTrackingTask.h>
#include <BipedalLocomotion/bindings/IK/JointVelocityLimitsTask.h>
#include <BipedalLocomotion/bindings/IK/Module.h>
#include <BipedalLocomotion/bindings/IK/QPInverseKinematics.h>
#include <BipedalLocomotion/bindings/IK/R3Task.h>
Expand All @@ -41,6 +42,7 @@ void CreateModule(pybind11::module& module)
CreateAngularMomentumTask(module);
CreateDistanceTask(module);
CreateGravityTask(module);
CreateJointVelocityLimitsTask(module);
CreateIntegrationBasedIK(module);
CreateQPInverseKinematics(module);
}
Expand Down
20 changes: 20 additions & 0 deletions bindings/python/IK/tests/test_QP_inverse_kinematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -260,6 +260,26 @@ def test_gravity_task():
assert gravity_task.set_feedforward_velocity_in_target_frame(feedforward_velocity=[np.random.uniform(-0.5,0.5) for _ in range(3)])
assert gravity_task.set_set_point(desired_gravity_direction=[np.random.uniform(-0.5,0.5) for _ in range(3)])

def test_joint_velocity_limits_task():

# create KinDynComputationsDescriptor
joints_list, kindyn = get_kindyn()

joint_values = [np.random.uniform(-0.5,0.5) for _ in range(kindyn.getNrOfDegreesOfFreedom())]
# Set the parameters
joint_velocity_limits_param_handler = blf.parameters_handler.StdParametersHandler()
joint_velocity_limits_param_handler.set_parameter_string(name="robot_velocity_variable_name", value="robotVelocity")
joint_velocity_limits_param_handler.set_parameter_vector_float(name="upper_limits",value=np.array(joint_values) + 0.01)
joint_velocity_limits_param_handler.set_parameter_vector_float(name="lower_limits",value=np.array(joint_values) - 0.01)

# Initialize the task
joint_velocity_limits_task = blf.ik.JointVelocityLimitsTask()
assert joint_velocity_limits_task.set_kin_dyn(kindyn)
assert joint_velocity_limits_task.initialize(param_handler=joint_velocity_limits_param_handler)
joint_limits_var_handler = blf.system.VariablesHandler()
assert joint_limits_var_handler.add_variable("robotVelocity", 32) is True # robot velocity size = 26 (joints) + 6 (base)
assert joint_velocity_limits_task.set_variables_handler(variables_handler=joint_limits_var_handler)

def test_integration_based_ik_state():

state = blf.ik.IntegrationBasedIKState()
Expand Down
4 changes: 2 additions & 2 deletions src/IK/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,9 @@ if(FRAMEWORK_COMPILE_IK)
NAME IK
PUBLIC_HEADERS ${H_PREFIX}/R3Task.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/IntegrationBasedIK.h
${H_PREFIX}/AngularMomentumTask.h ${H_PREFIX}/JointLimitsTask.h ${H_PREFIX}/QPFixedBaseInverseKinematics.h
${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h
${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/IKLinearTask.h ${H_PREFIX}/DistanceTask.h ${H_PREFIX}/GravityTask.h ${H_PREFIX}/JointVelocityLimitsTask.h
SOURCES src/R3Task.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/CoMTask.cpp src/AngularMomentumTask.cpp src/JointLimitsTask.cpp
src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp src/GravityTask.cpp
src/QPInverseKinematics.cpp src/QPFixedBaseInverseKinematics.cpp src/IKLinearTask.cpp src/IntegrationBasedIK.cpp src/DistanceTask.cpp src/GravityTask.cpp src/JointVelocityLimitsTask.cpp
PUBLIC_LINK_LIBRARIES Eigen3::Eigen
BipedalLocomotion::ParametersHandler BipedalLocomotion::System
LieGroupControllers::LieGroupControllers
Expand Down
118 changes: 118 additions & 0 deletions src/IK/include/BipedalLocomotion/IK/JointVelocityLimitsTask.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,118 @@
/**
* @file JointVelocityLimitsTask.h
* @authors Davide Gorbani
* @copyright 2023 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the BSD-3-Clause license.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_JOINT_VELOCITY_LIMITS_TASK_H
#define BIPEDAL_LOCOMOTION_IK_JOINT_VELOCITY_LIMITS_TASK_H

#include <chrono>
#include <memory>
#include <vector>

#include <iDynTree/KinDynComputations.h>

#include <BipedalLocomotion/IK/IKLinearTask.h>

namespace BipedalLocomotion
{
namespace IK
{

/**
* JointVelocityLimitsTask is a concrete implementation of the Task. Please use this element if you
* want to ensure that the generated joint velocity will consider the joints velocity limits.
* The task represents the following equation
* \f[
* \dot{s}_{-} \preceq \begin{bmatrix} 0_6 & I_n \end{bmatrix} \nu \preceq \dot{s}_{+}
* \f]
* where \f$0_6\f$ and \f$I_n\f$ are the zero and the identity matrix. \f$\dot{s} _{-}\f$ and
* \f$\dot{s} _{+}\f$ are the lower and upper limits of the joints velocity.
*/
class JointVelocityLimitsTask : public IKLinearTask
{
Eigen::VectorXd m_upperLimits; /**< Upper limits in radians. */
Eigen::VectorXd m_lowerLimits; /**< Lower limits in radians. */

std::string m_robotVelocityVariableName;

bool m_isInitialized{false}; /**< True if the task has been initialized. */
bool m_isValid{false}; /**< True if the task is valid. */

std::shared_ptr<iDynTree::KinDynComputations> m_kinDyn; /**< Pointer to a KinDynComputations
object */

public:
// clang-format off
/**
* Initialize the task.
* @param paramHandler pointer to the parameters handler.
* @note the following parameters are required by the class
* | Parameter Name | Type | Description | Mandatory |
* |:----------------------------------:|:--------:|:-----------------------------------------------------------------------------------------------------------:|:---------:|
* | `robot_velocity_variable_name` | `string` | Name of the variable contained in `VariablesHandler` describing the robot velocity | Yes |
* | `upper_limits` | `vector` | Vector containing the upper limit of the joints velocity | Yes |
* | `lower_limits` | `vector` | Vector containing the lower limit of the joints velocity | Yes |
* @return True in case of success, false otherwise.
* Where the generalized robot velocity is a vector containing the base spatial-velocity
* (expressed in mixed representation) and the joint velocities.
* @return True in case of success, false otherwise.
*/
// clang-format on
bool
initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> paramHandler) override;

/**
* Set the kinDynComputations object.
* @param kinDyn pointer to a kinDynComputations object.
* @return True in case of success, false otherwise.
*/
bool setKinDyn(std::shared_ptr<iDynTree::KinDynComputations> kinDyn) override;

/**
* Set the set of variables required by the task. The variables are stored in the
* System::VariablesHandler.
* @param variablesHandler reference to a variables handler.
* @note The handler must contain a variable named as the parameter
* `robot_velocity_variable_name` stored in the parameter handler. The variable represents the
* generalized velocity of the robot. Where the generalized robot velocity is a vector
* containing the base spatial-velocity (expressed in mixed representation) and the joints
* velocity.
* @return True in case of success, false otherwise.
*/
bool setVariablesHandler(const System::VariablesHandler& variablesHandler) override;

/**
* Update the content of the element.
* For this task the matrices A and b are constant and do not need to be updated.
* @return True in case of success, false otherwise.
*/
bool update() override;

/**
* Get the size of the task. (I.e the number of rows of the vector b)
* @return the size of the task.
*/
std::size_t size() const override;

/**
* The JointsLimitsTask is an inequality task.
* @return the size of the task.
*/
Type type() const override;

/**
* Determines the validity of the objects retrieved with getA() and getB()
* @return True if the objects are valid, false otherwise.
*/
bool isValid() const override;
};

BLF_REGISTER_IK_TASK(JointVelocityLimitsTask);

} // namespace IK
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_JOINT_VELOCITY_LIMITS_TASK_H
Loading

0 comments on commit 40b9eb1

Please sign in to comment.