Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Introduce the python bindings for TSID component #428

Merged
merged 13 commits into from
Oct 20, 2021
Merged
Show file tree
Hide file tree
Changes from 7 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions bindings/python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ add_subdirectory(RobotInterface)
add_subdirectory(Math)
add_subdirectory(FloatingBaseEstimators)
add_subdirectory(IK)
add_subdirectory(TSID)
add_subdirectory(TextLogging)

include(ConfigureFileWithCMakeIf)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_COM_TASK_H
#define BIPEDAL_LOCOMOTION_IK_COM_TASK_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_COM_TASK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_COM_TASK_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,4 @@ void CreateCoMTask(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_COM_TASK_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_COM_TASK_H
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H
#define BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_LINEAR_TASK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_LINEAR_TASK_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,4 @@ void CreateIKLinearTask(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_LINEAR_TASK_H
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H
#define BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_INTEGRATION_BASE_IK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_INTEGRATION_BASE_IK_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,5 @@ void CreateIntegrationBasedIK(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_INTEGRATION_BASE_IK_H

Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H
#define BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_TRACKING_TASK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_TRACKING_TASK_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,4 @@ void CreateJointTrackingTask(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_JOINT_TRACKING_TASK_H
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H
#define BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_QP_INVERSE_KINEMATICS_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_QP_INVERSE_KINEMATICS_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,4 @@ void CreateQPInverseKinematics(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_QP_INVERSE_KINEMATICS_H
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_SE3_TASK_H
#define BIPEDAL_LOCOMOTION_IK_SE3_TASK_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_SE3_TASK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_SE3_TASK_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,4 @@ void CreateSE3Task(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_SE3_TASK_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_SE3_TASK_H
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_IK_SO3_TASK_H
#define BIPEDAL_LOCOMOTION_IK_SO3_TASK_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_IK_SO3_TASK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_SO3_TASK_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,4 @@ void CreateSO3Task(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_IK_SO3_TASK_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_SO3_TASK_H
7 changes: 0 additions & 7 deletions bindings/python/IK/src/CoMTask.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,7 @@ void CreateCoMTask(pybind11::module& module)

py::class_<CoMTask, std::shared_ptr<CoMTask>, IKLinearTask>(module, "CoMTask")
.def(py::init())
.def(
"initialize",
[](CoMTask& impl,
std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
paramHandler) -> bool { return impl.initialize(paramHandler); },
py::arg("param_handler"))
.def("set_kin_dyn", &CoMTask::setKinDyn, py::arg("kin_dyn"))
.def("set_variables_handler", &CoMTask::setVariablesHandler, py::arg("variables_handler"))
.def("set_set_point", &CoMTask::setSetPoint, py::arg("position"), py::arg("velocity"));
}

Expand Down
29 changes: 28 additions & 1 deletion bindings/python/IK/src/IntegrationBasedIK.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
#include <pybind11/stl.h>

#include <BipedalLocomotion/IK/IntegrationBasedIK.h>
#include <BipedalLocomotion/System/ILinearTaskSolver.h>
#include <BipedalLocomotion/System/Source.h>
#include <BipedalLocomotion/bindings/IK/IntegrationBasedIK.h>

Expand All @@ -33,7 +34,33 @@ void CreateIntegrationBasedIK(pybind11::module& module)

py::class_<Source<IntegrationBasedIKState>>(module, "IntegrationBasedIKStateSource");

py::class_<IntegrationBasedIK, Source<IntegrationBasedIKState>>(module, "IntegrationBasedIK");
py::class_<ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>,
Source<IntegrationBasedIKState>>(module, "ILinearTaskSolverIK")
.def("add_task",
&ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::addTask,
py::arg("task"),
py::arg("task_name"),
py::arg("priority"),
py::arg("weight") = Eigen::VectorXd())
.def("get_task_names",
&ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::getTaskNames)
.def("finalize",
&ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::finalize,
py::arg("handler"))
.def("advance", &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::advance)
.def("get_output", &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::getOutput)
.def("is_output_valid",
&ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::isOutputValid)
.def(
"initialize",
[](ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>& impl,
std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
handler) -> bool { return impl.initialize(handler); },
py::arg("handler"));

py::class_<IntegrationBasedIK, //
ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>>(module,
"IntegrationBasedIK");
}

} // namespace IK
Expand Down
10 changes: 0 additions & 10 deletions bindings/python/IK/src/JointTrackingTask.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,18 +28,8 @@ void CreateJointTrackingTask(pybind11::module& module)
py::class_<JointTrackingTask, std::shared_ptr<JointTrackingTask>, IKLinearTask>( //
module,
"JointTrackingTask")

.def(py::init())
.def(
"initialize",
[](JointTrackingTask& impl,
std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
paramHandler) -> bool { return impl.initialize(paramHandler); },
py::arg("param_handler"))
.def("set_kin_dyn", &JointTrackingTask::setKinDyn, py::arg("kin_dyn"))
.def("set_variables_handler",
&JointTrackingTask::setVariablesHandler,
py::arg("variables_handler"))
.def("set_set_point",
py::overload_cast<Eigen::Ref<const Eigen::VectorXd>>(&JointTrackingTask::setSetPoint),
py::arg("joint_position"))
Expand Down
20 changes: 1 addition & 19 deletions bindings/python/IK/src/QPInverseKinematics.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,6 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

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

Expand All @@ -27,24 +26,7 @@ void CreateQPInverseKinematics(pybind11::module& module)
using namespace BipedalLocomotion::IK;

py::class_<QPInverseKinematics, IntegrationBasedIK>(module, "QPInverseKinematics")
.def(py::init())
.def(
"initialize",
[](QPInverseKinematics& impl,
std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
handler) -> bool { return impl.initialize(handler); },
py::arg("handler"))
.def("add_task",
&QPInverseKinematics::addTask,
py::arg("task"),
py::arg("taskName"),
py::arg("priority"),
py::arg("weight") = Eigen::VectorXd())
.def("get_task_names", &QPInverseKinematics::getTaskNames)
.def("finalize", &QPInverseKinematics::finalize, py::arg("handler"))
.def("advance", &QPInverseKinematics::advance)
.def("get_output", &QPInverseKinematics::getOutput)
.def("is_output_valid", &QPInverseKinematics::isOutputValid);
.def(py::init());
}

} // namespace IK
Expand Down
7 changes: 0 additions & 7 deletions bindings/python/IK/src/SE3Task.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,7 @@ void CreateSE3Task(pybind11::module& module)

py::class_<SE3Task, std::shared_ptr<SE3Task>, IKLinearTask>(module, "SE3Task")
.def(py::init())
.def(
"initialize",
[](SE3Task& impl,
std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
paramHandler) -> bool { return impl.initialize(paramHandler); },
py::arg("param_handler"))
.def("set_kin_dyn", &SE3Task::setKinDyn, py::arg("kin_dyn"))
.def("set_variables_handler", &SE3Task::setVariablesHandler, py::arg("variables_handler"))
.def("set_set_point", &SE3Task::setSetPoint, py::arg("I_H_F"), py::arg("mixed_velocity"));
}

Expand Down
7 changes: 0 additions & 7 deletions bindings/python/IK/src/SO3Task.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,7 @@ void CreateSO3Task(pybind11::module& module)

py::class_<SO3Task, std::shared_ptr<SO3Task>, IKLinearTask>(module, "SO3Task")
.def(py::init())
.def(
"initialize",
[](SO3Task& impl,
std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler>
paramHandler) -> bool { return impl.initialize(paramHandler); },
py::arg("param_handler"))
.def("set_kin_dyn", &SO3Task::setKinDyn, py::arg("kin_dyn"))
.def("set_variables_handler", &SO3Task::setVariablesHandler, py::arg("variables_handler"))
.def("set_set_point", &SO3Task::setSetPoint, py::arg("I_R_F"), py::arg("angular_velocity"));
}

Expand Down
6 changes: 3 additions & 3 deletions bindings/python/IK/tests/test_QP_inverse_kinematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -235,7 +235,7 @@ def test_qp_inverse_kinematics():
assert com_task.set_set_point(position=np.array([0.,0.,0.53]), velocity=np.array([0.,0.,0.]))

# Add CoM task as hard constraint
assert qp_ik.add_task(task=com_task, taskName="Com_task", priority=0)
assert qp_ik.add_task(task=com_task, task_name="Com_task", priority=0)

# Configure SE3 task (right foot)
se3_param_handler = blf.parameters_handler.StdParametersHandler()
Expand All @@ -254,7 +254,7 @@ def test_qp_inverse_kinematics():
assert se3_task.set_set_point(I_H_F=I_H_F, mixed_velocity=mixed_velocity)

# Add SE3 task as hard constraint
assert qp_ik.add_task(task=se3_task, taskName="se3_task", priority=0)
assert qp_ik.add_task(task=se3_task, task_name="se3_task", priority=0)

# Configure joint tracking task (regularization)
joint_tracking_param_handler = blf.parameters_handler.StdParametersHandler()
Expand All @@ -277,7 +277,7 @@ def test_qp_inverse_kinematics():
assert joint_tracking_task.set_set_point(joint_position=np.add(joint_values,joint_values_delta))

# Add joint tracking task as soft constraint
assert qp_ik.add_task(task=joint_tracking_task, taskName="joint_tracking_task", priority=1, weight=[1.0]*kindyn_desc.kindyn.get_nr_of_dofs())
assert qp_ik.add_task(task=joint_tracking_task, task_name="joint_tracking_task", priority=1, weight=[1.0]*kindyn_desc.kindyn.get_nr_of_dofs())

# Check that all the tasks have been added
task_names = qp_ik.get_task_names()
Expand Down
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
/**
* @file LinearTask.h
* @authors Paolo Maria Viceconte
* @authors Paolo Maria Viceconte, Giulio Romualdi
* @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

#ifndef BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TASK_H
#define BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TASK_H
#ifndef BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_LINEAR_TASK_H
#define BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_LINEAR_TASK_H

#include <pybind11/pybind11.h>

Expand All @@ -23,4 +23,4 @@ void CreateLinearTask(pybind11::module& module);
} // namespace bindings
} // namespace BipedalLocomotion

#endif // BIPEDAL_LOCOMOTION_SYSTEM_LINEAR_TASK_H
#endif // BIPEDAL_LOCOMOTION_BINDINGS_SYSTEM_LINEAR_TASK_H
28 changes: 27 additions & 1 deletion bindings/python/System/src/LinearTask.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
* distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
*/

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

Expand All @@ -24,7 +25,32 @@ void CreateLinearTask(pybind11::module& module)

using namespace BipedalLocomotion::System;

py::class_<LinearTask, std::shared_ptr<LinearTask>>(module, "LinearTask");
py::class_<LinearTask, std::shared_ptr<LinearTask>> linearTask(module, "LinearTask");

py::enum_<LinearTask::Type>(linearTask, "Type")
.value("equality", LinearTask::Type::equality)
.value("inequality", LinearTask::Type::inequality)
.export_values();

linearTask
.def(
"initialize",
[](LinearTask& impl,
std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
handler) -> bool { return impl.initialize(handler); },
py::arg("param_handler"))
.def("set_variables_handler",
&LinearTask::setVariablesHandler,
py::arg("variables_handler"))
.def("update", &LinearTask::update)
.def("get_A", &LinearTask::getA)
.def("get_b", &LinearTask::getB)
.def("size", &LinearTask::size)
.def("get_description", &LinearTask::getDescription)
.def("type", &LinearTask::type)
.def("is_valid", &LinearTask::isValid)
.def("__repr__", &LinearTask::getDescription)
.def("__len__", &LinearTask::size);
}

} // namespace System
Expand Down
20 changes: 20 additions & 0 deletions bindings/python/TSID/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
# Copyright (C) 2021 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.

# src/IntegrationBasedTSID.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/Module.cpp src/TSIDLinearTask.cpp

# ${H_PREFIX}/IntegrationBasedTSID.h ${H_PREFIX}/QPInverseKinematics.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/TSIDLinearTask.h

if(FRAMEWORK_COMPILE_TSID)

set(H_PREFIX include/BipedalLocomotion/bindings/TSID)

add_bipedal_locomotion_python_module(
NAME TSIDBindings
SOURCES src/BaseDynamicsTask.cpp src/CoMTask.cpp src/FeasibleContactWrenchTask.cpp src/JointDynamicsTask.cpp src/JointTrackingTask.cpp src/Module.cpp src/QPTSID.cpp src/SE3Task.cpp src/SO3Task.cpp src/TaskSpaceInverseDynamics.cpp src/TSIDLinearTask.cpp
HEADERS ${H_PREFIX}/BaseDynamicsTask.h ${H_PREFIX}/CoMTask.h ${H_PREFIX}/FeasibleContactWrenchTask.h ${H_PREFIX}/JointDynamicsTask.h ${H_PREFIX}/JointTrackingTask.h ${H_PREFIX}/Module.h ${H_PREFIX}/QPTSID.h ${H_PREFIX}/SE3Task.h ${H_PREFIX}/SO3Task.h ${H_PREFIX}/TaskSpaceInverseDynamics.h ${H_PREFIX}/TSIDLinearTask.h
LINK_LIBRARIES BipedalLocomotion::TSID
)

endif()
Loading