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

Implement QPInverseKinematics python bindings #303

Merged
merged 21 commits into from
Jul 21, 2021
Merged
Show file tree
Hide file tree
Changes from all commits
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
1 change: 1 addition & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@ All notable changes to this project are documented in this file.
- Implement `QPTSID` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/366)
- Implement motor pwm, motor encoders, wbd joint torque estimates, pid reading in `YarpSensorBridge`(https://github.com/dic-iit/bipedal-locomotion-framework/pull/359).
- Implement FeasibleContactWrenchTask for TSID component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/369).
- Implement python bindings for QPInverseKinematics class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/303)

### Changed
- Add common Python files to gitignore (https://github.com/dic-iit/bipedal-locomotion-framework/pull/338)
Expand Down
1 change: 1 addition & 0 deletions bindings/python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ add_subdirectory(Planners)
add_subdirectory(RobotInterface)
add_subdirectory(Math)
add_subdirectory(FloatingBaseEstimators)
add_subdirectory(IK)

include(ConfigureFileWithCMakeIf)

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,12 @@
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <BipedalLocomotion/Conversions/ManifConversions.h>
#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h>
#include <BipedalLocomotion/FloatingBaseEstimators/ModelComputationsHelper.h>
#include <BipedalLocomotion/Math/Constants.h>
#include <iDynTree/Model/Model.h>

#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h>

namespace BipedalLocomotion
{
Expand All @@ -25,8 +27,116 @@ void CreateKinDynComputations(pybind11::module& module)
{
namespace py = ::pybind11;
using namespace iDynTree;

struct KinDynKinematicsRobotState
{
KinDynKinematicsRobotState( //
const size_t dofs = 0,
const Eigen::Vector3d gravity
= {0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation})
: worldGravity(gravity)
{
this->jointPositions = Eigen::VectorXd::Zero(dofs);
this->jointVelocities = Eigen::VectorXd::Zero(dofs);
}

Eigen::VectorXd jointPositions;
Eigen::VectorXd jointVelocities;

Eigen::VectorXd baseVelocity = Eigen::VectorXd::Zero(6);
Eigen::Matrix<double, 4, 4> baseTransform = Eigen::MatrixXd::Zero(4,4);

Eigen::Vector3d worldGravity = {0, 0, -Math::StandardAccelerationOfGravitation};
};

py::class_<KinDynKinematicsRobotState>(module, "KinDynKinematicsRobotState")
.def(py::init())
.def_readwrite("joint_positions", &KinDynKinematicsRobotState::jointPositions)
.def_readwrite("joint_velocities", &KinDynKinematicsRobotState::jointVelocities)
.def_readwrite("base_velocity", &KinDynKinematicsRobotState::baseVelocity)
.def_readwrite("base_transform", &KinDynKinematicsRobotState::baseTransform)
.def_readwrite("world_gravity", &KinDynKinematicsRobotState::worldGravity);

py::class_<KinDynComputations, std::shared_ptr<KinDynComputations>>(module,
"KinDynComputations");
"KinDynComputations")
.def("__repr__",
[](const KinDynComputations& impl) { return impl.model().toString(); })
.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("get_center_of_mass_position",
[](KinDynComputations& impl) {
Eigen::VectorXd pos(3);
if (!impl.getCenterOfMassPosition(pos))
{
throw py::value_error("Failed to get the center of mass position.");
}
return pos;
})
.def("get_center_of_mass_velocity",
[](KinDynComputations& impl) {
Eigen::VectorXd vel(3);
if (!impl.getCenterOfMassVelocity(vel))
{
throw py::value_error("Failed to get the center of mass velocity.");
}
return vel;
})
.def("get_world_transform",
[](KinDynComputations& impl, std::string name) {
Eigen::Matrix<double, 4, 4> world_T_frame;
if (!impl.getWorldTransform(name, world_T_frame))
{
throw py::value_error("Failed to get the trasform for the specified frame.");
}
return world_T_frame;
})
.def("get_world_base_transform",
[](KinDynComputations& impl) {
Eigen::Matrix<double, 4, 4> world_T_base;
if (!impl.getWorldBaseTransform(world_T_base))
{
throw py::value_error("Failed to get the trasform for the base.");
}
return world_T_base;
})
.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(
"set_floating_base",
[](KinDynComputations& impl, const std::string & s) {
return impl.setFloatingBase(s);
})
.def("get_robot_state",
[](KinDynComputations& impl) {
KinDynKinematicsRobotState robot_state(impl.getNrOfDegreesOfFreedom());
if (!impl.getRobotState(robot_state.baseTransform, robot_state.jointPositions, robot_state.baseVelocity, robot_state.jointVelocities, robot_state.worldGravity))
{
throw py::value_error("Failed to get the robot state.");
}
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 @@ -126,8 +236,9 @@ void CreateFloatingBaseEstimator(pybind11::module& module)

py::class_<Source<Output>>(module, "FloatingBaseEstimatorOutputSource");

py::class_<FloatingBaseEstimator, Source<Output>> floatingBaseEstimator(module,
"FloatingBaseEstimator");
py::class_<FloatingBaseEstimator, Source<Output>> floatingBaseEstimator( //
module,
"FloatingBaseEstimator");

floatingBaseEstimator.def(py::init())
.def("set_imu_measurement",
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,19 +11,44 @@ def test_legged_odometry():
# create KinDynComputationsDescriptor
kindyn_handler = blf.parameters_handler.StdParametersHandler()
kindyn_handler.set_parameter_string("model_file_name", "./model.urdf")
kindyn_handler.set_parameter_vector_string("joints_list", ["neck_pitch", "neck_roll", "neck_yaw",
"torso_pitch", "torso_roll", "torso_yaw",
"l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw",
"l_elbow",
"r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw",
"r_elbow",
"l_hip_pitch", "l_hip_roll", "l_hip_yaw",
"l_knee", "l_ankle_pitch", "l_ankle_roll",
"r_hip_pitch", "r_hip_roll", "r_hip_yaw",
"r_knee", "r_ankle_pitch", "r_ankle_roll"])
joints_list = ["neck_pitch", "neck_roll", "neck_yaw",
"torso_pitch", "torso_roll", "torso_yaw",
"l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw","l_elbow",
"r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw","r_elbow",
"l_hip_pitch", "l_hip_roll", "l_hip_yaw","l_knee", "l_ankle_pitch", "l_ankle_roll",
"r_hip_pitch", "r_hip_roll", "r_hip_yaw","r_knee", "r_ankle_pitch", "r_ankle_roll"]
kindyn_handler.set_parameter_vector_string("joints_list", joints_list)
kindyn_desc = blf.floating_base_estimators.construct_kindyncomputations_descriptor(kindyn_handler)
assert kindyn_desc.is_valid()

# Check the number of degrees of freedom of the model
assert kindyn_desc.kindyn.get_nr_of_dofs() == len(joints_list)

# Set the joint positions to random values
joint_values = [np.random.uniform(-0.5,0.5) for _ in range(kindyn_desc.kindyn.get_nr_of_dofs())]
assert kindyn_desc.kindyn.set_joint_pos(joint_values)

# Check that the joint positions are all set to the desired values
updated_joint_pos = kindyn_desc.kindyn.get_joint_pos()
assert updated_joint_pos == pytest.approx(joint_values)

# Set the robot state
updated_world_T_base = np.array([[1., 0., 0., 0.],[0., 0., -1., 0.],[0., 1., 0., 0.],[0., 0., 0., 1.]])
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())]
updated_world_gravity = [np.random.uniform(-0.5,0.5) for _ in range(3)]
assert kindyn_desc.kindyn.set_robot_state(updated_world_T_base,updated_s,updated_base_velocity,
updated_s_dot,updated_world_gravity)

# Check that the robot state is set to the desired values
updated_state = kindyn_desc.kindyn.get_robot_state()
assert updated_state.base_transform == pytest.approx(updated_world_T_base)
assert updated_state.joint_positions == pytest.approx(updated_s)
assert updated_state.base_velocity == pytest.approx(updated_base_velocity)
assert updated_state.joint_velocities == pytest.approx(updated_s_dot)
assert updated_state.world_gravity == pytest.approx(updated_world_gravity)

dt = 0.01
# create configuration parameters handler for legged odometry
lo_params_handler = blf.parameters_handler.StdParametersHandler()
Expand Down
17 changes: 17 additions & 0 deletions bindings/python/IK/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
# 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.

if(FRAMEWORK_COMPILE_IK)

set(H_PREFIX include/BipedalLocomotion/bindings/IK)

add_bipedal_locomotion_python_module(
NAME IKBindings
SOURCES src/IntegrationBasedIK.cpp src/QPInverseKinematics.cpp src/CoMTask.cpp src/SE3Task.cpp src/SO3Task.cpp src/JointTrackingTask.cpp src/Module.cpp src/IKLinearTask.cpp
HEADERS ${H_PREFIX}/IntegrationBasedIK.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}/IKLinearTask.h
LINK_LIBRARIES BipedalLocomotion::IK
TESTS tests/test_QP_inverse_kinematics.py
)

endif()
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file CoMTask.h
* @authors Paolo Maria Viceconte
* @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_IK_COM_TASK_H
#define BIPEDAL_LOCOMOTION_IK_COM_TASK_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateCoMTask(pybind11::module& module);

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

#endif // BIPEDAL_LOCOMOTION_IK_COM_TASK_H
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file IKLinearTask.h
* @authors 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_IK_LINEAR_TASK_H
#define BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateIKLinearTask(pybind11::module& module);

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

#endif // BIPEDAL_LOCOMOTION_IK_LINEAR_TASK_H
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file IntegrationBasedIK.h
* @authors Paolo Maria Viceconte
* @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_IK_INTEGRATION_BASE_IK_H
#define BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateIntegrationBasedIK(pybind11::module& module);

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

#endif // BIPEDAL_LOCOMOTION_IK_INTEGRATION_BASE_IK_H
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file JointTrackingTask.h
* @authors Paolo Maria Viceconte
* @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_IK_JOINT_TRACKING_TASK_H
#define BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateJointTrackingTask(pybind11::module& module);

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

#endif // BIPEDAL_LOCOMOTION_IK_JOINT_TRACKING_TASK_H
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file Module.h
* @authors Paolo Maria Viceconte
* @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_BINDINGS_IK_MODULE_H
#define BIPEDAL_LOCOMOTION_BINDINGS_IK_MODULE_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateModule(pybind11::module& module);

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

#endif // BIPEDAL_LOCOMOTION_BINDINGS_IK_MODULE_H
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
/**
* @file QPInverseKinematics.h
* @authors Paolo Maria Viceconte
* @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_IK_QP_INVERSE_KINEMATICS_H
#define BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H

#include <pybind11/pybind11.h>

namespace BipedalLocomotion
{
namespace bindings
{
namespace IK
{

void CreateQPInverseKinematics(pybind11::module& module);

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

#endif // BIPEDAL_LOCOMOTION_IK_QP_INVERSE_KINEMATICS_H
Loading