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.

Sign up for GitHub

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

Implement QPInverseKinematics python bindings #303

Merged
merged 21 commits into from
Jul 21, 2021
Merged

Implement QPInverseKinematics python bindings #303

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
21 commits
Select commit Hold shift + click to select a range
7474e98
Update ModelComputationsHelper class and python bindings
paolo-viceconte May 7, 2021
161a990
Add bindings for LinearTask
paolo-viceconte May 7, 2021
6536fde
Add python bindings for the derived tasks
paolo-viceconte May 7, 2021
e39e2f9
Add bindings for IntegrationBasedIK and QPInverseKinematics
paolo-viceconte May 10, 2021
a878731
Fix QPInverseKinematics test
paolo-viceconte May 10, 2021
68f27ac
Fix QPInverseKinematics test v2
paolo-viceconte May 10, 2021
86aba53
Fix QPInverseKinematics test v3
paolo-viceconte May 10, 2021
3d36530
Extend the pybind11 bindings of kindyn computations in blf
paolo-viceconte May 17, 2021
4076530
Refactor the code + update the IK test
paolo-viceconte May 17, 2021
ab5bb3d
Clang formatting
paolo-viceconte May 18, 2021
f1fe0a3
Minor fixes
paolo-viceconte May 19, 2021
086597a
Extend kindyn computations bindings
paolo-viceconte Jun 7, 2021
2f412b8
Use gym-ignition-models for testing
paolo-viceconte Jun 10, 2021
38eb1ad
Replace dictionary with struct in get_robot_state()
paolo-viceconte Jul 12, 2021
a2bbd08
Update urdf model retrieval in the python bindings tests
paolo-viceconte Jul 12, 2021
a9caffa
Extend the bindings of kindyn computations
paolo-viceconte Jul 12, 2021
a16e0b1
Update changelog file
paolo-viceconte Jul 12, 2021
7e57363
Avoid to close tmp file
paolo-viceconte Jul 12, 2021
2e93327
Merge branch 'master' into feature/bindings_IK
GiulioRomualdi Jul 20, 2021
a8022f1
Update the IK python bindings accordingly to a818387ac5d96bcdf9f8b44b…
GiulioRomualdi Jul 21, 2021
2b46c27
Merge branch 'master' into feature/bindings_IK
GiulioRomualdi Jul 21, 2021
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
83 changes: 44 additions & 39 deletions bindings/python/FloatingBaseEstimators/src/FloatingBaseEstimators.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,47 +27,51 @@ void CreateKinDynComputations(pybind11::module& module)
using namespace iDynTree;
py::class_<KinDynComputations, std::shared_ptr<KinDynComputations>>(module,
"KinDynComputations")
.def("get_nr_of_dofs", [](const KinDynComputations& impl) {
return impl.getNrOfDegreesOfFreedom();
})
.def("get_joint_pos", [](const KinDynComputations& impl) {
Eigen::VectorXd s(impl.getNrOfDegreesOfFreedom());
if (!impl.getJointPos(s))
{
throw py::value_error("Failed to get the joint position.");
}
return s;
})
.def("set_joint_pos", [](KinDynComputations& impl, Eigen::Ref<const Eigen::VectorXd> s) {
.def("get_nr_of_dofs",
[](const KinDynComputations& impl) { return impl.getNrOfDegreesOfFreedom(); })
.def("get_joint_pos",
[](const KinDynComputations& impl) {
Eigen::VectorXd s(impl.getNrOfDegreesOfFreedom());
if (!impl.getJointPos(s))
{
throw py::value_error("Failed to get the joint position.");
}
return s;
})
.def(
"set_joint_pos",
[](KinDynComputations& impl, Eigen::Ref<const Eigen::VectorXd> s) {
return impl.setJointPos(iDynTree::make_span(s.data(), s.size()));
},
py::arg("s"))
.def("get_robot_state", [](KinDynComputations& impl) {
Eigen::Matrix<double, 4, 4> world_T_base;
Eigen::VectorXd s(impl.getNrOfDegreesOfFreedom());
Eigen::VectorXd base_velocity(6);
Eigen::VectorXd s_dot(impl.getNrOfDegreesOfFreedom());
Eigen::VectorXd world_gravity(3);
if (!impl.getRobotState(world_T_base,s,base_velocity,s_dot,world_gravity))
{
throw py::value_error("Failed to get the robot state.");
}
py::dict robot_state;
robot_state["world_T_base"] = world_T_base;
robot_state["s"] = s;
robot_state["base_velocity"] = base_velocity;
robot_state["s_dot"] = s_dot;
robot_state["world_gravity"] = world_gravity;
return robot_state;
})
.def("set_robot_state", [](KinDynComputations& impl,
Eigen::Ref<const Eigen::Matrix<double, 4, 4>> world_T_base,
Eigen::Ref<const Eigen::VectorXd> s,
Eigen::Ref<const Eigen::VectorXd> base_velocity,
Eigen::Ref<const Eigen::VectorXd> s_dot,
Eigen::Ref<const Eigen::VectorXd> world_gravity) {
return impl.setRobotState(world_T_base,s,base_velocity,s_dot,world_gravity);
});
.def("get_robot_state",
[](KinDynComputations& impl) {
Eigen::Matrix<double, 4, 4> world_T_base;
Eigen::VectorXd s(impl.getNrOfDegreesOfFreedom());
Eigen::VectorXd base_velocity(6);
Eigen::VectorXd s_dot(impl.getNrOfDegreesOfFreedom());
Eigen::VectorXd world_gravity(3);
if (!impl.getRobotState(world_T_base, s, base_velocity, s_dot, world_gravity))
{
throw py::value_error("Failed to get the robot state.");
}
py::dict robot_state;
robot_state["world_T_base"] = world_T_base;
robot_state["s"] = s;
robot_state["base_velocity"] = base_velocity;
robot_state["s_dot"] = s_dot;
robot_state["world_gravity"] = world_gravity;
return robot_state;
})
.def("set_robot_state",
[](KinDynComputations& impl,
Eigen::Ref<const Eigen::Matrix<double, 4, 4>> world_T_base,
Eigen::Ref<const Eigen::VectorXd> s,
Eigen::Ref<const Eigen::VectorXd> base_velocity,
Eigen::Ref<const Eigen::VectorXd> s_dot,
Eigen::Ref<const Eigen::VectorXd> world_gravity) {
return impl.setRobotState(world_T_base, s, base_velocity, s_dot, world_gravity);
});
}

void CreateKinDynComputationsDescriptor(pybind11::module& module)
Expand Down Expand Up @@ -168,7 +172,8 @@ void CreateFloatingBaseEstimator(pybind11::module& module)
py::class_<Source<Output>>(module, "FloatingBaseEstimatorOutputSource");

py::class_<FloatingBaseEstimator, Source<Output>> floatingBaseEstimator(module,
"FloatingBaseEstimator");
"FloatingBaseEstimato"
"r");
paolo-viceconte marked this conversation as resolved.
Show resolved Hide resolved

floatingBaseEstimator.def(py::init())
.def("set_imu_measurement",
Expand Down
3 changes: 1 addition & 2 deletions bindings/python/FloatingBaseEstimators/tests/test_legged_odometry.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,7 @@ def test_legged_odometry():
assert updated_joint_pos == pytest.approx(joint_values)

# Set the robot state
# TODO: world_T_base.flags['F_CONTIGUOUS'] is required to be True
updated_world_T_base = np.asfortranarray([[1., 0., 0., 0.],[0., 0., -1., 0.],[0., 1., 0., 0.],[0., 0., 0., 1.]])
updated_world_T_base = np.asarray([[1., 0., 0., 0.],[0., 0., -1., 0.],[0., 1., 0., 0.],[0., 0., 0., 1.]])
paolo-viceconte marked this conversation as resolved.
Show resolved Hide resolved
updated_s = [np.random.uniform(-0.5,0.5) for _ in range(kindyn_desc.kindyn.get_nr_of_dofs())]
updated_base_velocity = [np.random.uniform(-0.5,0.5) for _ in range(6)]
updated_s_dot = [np.random.uniform(-0.5,0.5) for _ in range(kindyn_desc.kindyn.get_nr_of_dofs())]
Expand Down
18 changes: 6 additions & 12 deletions bindings/python/IK/src/CoMTask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

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

namespace BipedalLocomotion
Expand All @@ -28,21 +28,15 @@ void CreateCoMTask(pybind11::module& module)

py::class_<CoMTask, std::shared_ptr<CoMTask>, LinearTask>(module, "CoMTask")
.def(py::init())
.def("initialize",
.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"));
.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"));
}

} // namespace IK
Expand Down
16 changes: 8 additions & 8 deletions bindings/python/IK/src/JointTrackingTask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

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

namespace BipedalLocomotion
Expand All @@ -26,22 +26,22 @@ void CreateJointTrackingTask(pybind11::module& module)
using namespace BipedalLocomotion::IK;
using namespace BipedalLocomotion::System;

py::class_<JointTrackingTask, std::shared_ptr<JointTrackingTask>, LinearTask>(module, "JointTrackingTask")
py::class_<JointTrackingTask, std::shared_ptr<JointTrackingTask>, LinearTask>(module,
"JointTrackingTas"
"k")
paolo-viceconte marked this conversation as resolved.
Show resolved Hide resolved
.def(py::init())
.def("initialize",
.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_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::overload_cast<Eigen::Ref<const Eigen::VectorXd>>(&JointTrackingTask::setSetPoint),
py::arg("joint_position"))
.def("set_set_point",
py::overload_cast<Eigen::Ref<const Eigen::VectorXd>, Eigen::Ref<const Eigen::VectorXd>>(
Expand Down
8 changes: 4 additions & 4 deletions bindings/python/IK/src/Module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,13 +7,13 @@

#include <pybind11/pybind11.h>

#include <BipedalLocomotion/bindings/IK/Module.h>
#include <BipedalLocomotion/bindings/IK/CoMTask.h>
#include <BipedalLocomotion/bindings/IK/SE3Task.h>
#include <BipedalLocomotion/bindings/IK/SO3Task.h>
#include <BipedalLocomotion/bindings/IK/JointTrackingTask.h>
#include <BipedalLocomotion/bindings/IK/IntegrationBasedIK.h>
#include <BipedalLocomotion/bindings/IK/JointTrackingTask.h>
#include <BipedalLocomotion/bindings/IK/Module.h>
#include <BipedalLocomotion/bindings/IK/QPInverseKinematics.h>
#include <BipedalLocomotion/bindings/IK/SE3Task.h>
#include <BipedalLocomotion/bindings/IK/SO3Task.h>

namespace BipedalLocomotion
{
Expand Down
15 changes: 7 additions & 8 deletions bindings/python/IK/src/QPInverseKinematics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,15 +34,14 @@ void CreateQPInverseKinematics(pybind11::module& module)
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("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("finalize", &QPInverseKinematics::finalize, py::arg("handler"))
.def("advance", &QPInverseKinematics::advance)
.def("get_output", &QPInverseKinematics::getOutput)
.def("is_output_valid", &QPInverseKinematics::isOutputValid);
Expand Down
18 changes: 6 additions & 12 deletions bindings/python/IK/src/SE3Task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

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

namespace BipedalLocomotion
Expand All @@ -28,21 +28,15 @@ void CreateSE3Task(pybind11::module& module)

py::class_<SE3Task, std::shared_ptr<SE3Task>, LinearTask>(module, "SE3Task")
.def(py::init())
.def("initialize",
.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"));
.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"));
}

} // namespace IK
Expand Down
18 changes: 6 additions & 12 deletions bindings/python/IK/src/SO3Task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,8 @@
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

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

namespace BipedalLocomotion
Expand All @@ -28,21 +28,15 @@ void CreateSO3Task(pybind11::module& module)

py::class_<SO3Task, std::shared_ptr<SO3Task>, LinearTask>(module, "SO3Task")
.def(py::init())
.def("initialize",
.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"));
.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"));
}

} // namespace IK
Expand Down
1 change: 0 additions & 1 deletion bindings/python/System/include/BipedalLocomotion/bindings/System/LinearTask.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@ namespace System

void CreateLinearTask(pybind11::module& module);


} // namespace System
} // namespace bindings
} // namespace BipedalLocomotion
Expand Down
1 change: 0 additions & 1 deletion bindings/python/System/src/LinearTask.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@ void CreateLinearTask(pybind11::module& module)
using namespace BipedalLocomotion::System;

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

}

} // namespace System
Expand Down
2 changes: 1 addition & 1 deletion bindings/python/System/src/Module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,9 @@

#include <pybind11/pybind11.h>

#include <BipedalLocomotion/bindings/System/LinearTask.h>
#include <BipedalLocomotion/bindings/System/Module.h>
#include <BipedalLocomotion/bindings/System/VariablesHandler.h>
#include <BipedalLocomotion/bindings/System/LinearTask.h>

namespace BipedalLocomotion
{
Expand Down