Unverified Commit dfb9c560 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1288 from jcarpent/topic/devel

Cover Python bindings inside the unit tests
parents 820d0f85 4ca19956
Pipeline #11044 passed with stage
in 263 minutes and 5 seconds
......@@ -4,28 +4,41 @@
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/aba-derivatives.hpp"
#include "pinocchio/bindings/python/utils/eigen.hpp"
#include <eigenpy/eigen-to-python.hpp>
namespace pinocchio
{
namespace python
{
void computeABADerivativesDefault(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau)
namespace bp = boost::python;
bp::tuple computeABADerivativesDefault(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau)
{
computeABADerivatives(model,data,q,v,tau);
make_symmetric(data.Minv);
return bp::make_tuple(make_ref(data.ddq_dq),
make_ref(data.ddq_dv),
make_ref(data.Minv));
}
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
void computeABADerivatives_fext(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
const ForceAlignedVector & fext)
bp::tuple computeABADerivatives_fext(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & tau,
const ForceAlignedVector & fext)
{
computeABADerivatives(model,data,q,v,tau,fext);
make_symmetric(data.Minv);
return bp::make_tuple(make_ref(data.ddq_dq),
make_ref(data.ddq_dv),
make_ref(data.Minv));
}
void exposeABADerivatives()
......@@ -52,8 +65,7 @@ namespace pinocchio
"Computes the ABA derivatives with external contact foces,\n"
"store the result in data.ddq_dq, data.ddq_dv and data.Minv\n"
"which correspond to the partial derivatives of the acceleration output with respect to the joint configuration,\n"
"velocity and torque vectors.\n"
"The forces are of type StdVec_Force.");
"velocity and torque vectors.\n");
}
} // namespace python
} // namespace pinocchio
//
// Copyright (c) 2015-2016 CNRS
// Copyright (c) 2015-2020 CNRS INRIA
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/bindings/python/utils/eigen.hpp"
namespace pinocchio
{
......@@ -14,8 +15,7 @@ namespace pinocchio
computeMinverse_proxy(const Model & model, Data & data, const Eigen::VectorXd & q)
{
computeMinverse(model,data,q);
data.Minv.triangularView<Eigen::StrictlyLower>() =
data.Minv.transpose().triangularView<Eigen::StrictlyLower>();
make_symmetric(data.Minv);
return data.Minv;
}
......
......@@ -56,6 +56,7 @@ namespace pinocchio
"a: acceleration vector (size model.nv)"),
"Computes the analytical derivatives of the centroidal dynamics\n"
"with respect to the joint configuration vector, velocity and acceleration.");
bp::def("getCentroidalDynamicsDerivatives",
getCentroidalDynamicsDerivatives_proxy,
bp::args("Model","Data"),
......
......@@ -11,13 +11,6 @@ namespace pinocchio
namespace python
{
Data::RowMatrixXs computeMinv(const Model & model, const Data & data)
{
Data::RowMatrixXs res(model.nv,model.nv);
pinocchio::cholesky::computeMinv(model,data,res);
return res;
}
void exposeCholesky()
{
using namespace Eigen;
......@@ -46,12 +39,6 @@ namespace pinocchio
"Returns the inverse of the inverse of the joint space inertia matrix using the results of the Cholesky decomposition\n"
"performed by cholesky.decompose. The result is stored in data.Minv.",
bp::return_value_policy<bp::return_by_value>());
bp::def("computeMinv",
&computeMinv,
bp::args("Model","Data"),
"Returns the inverse of the inverse of the joint space inertia matrix using the results of the Cholesky decomposition\n"
"performed by cholesky.decompose.");
}
}
......
......@@ -50,8 +50,7 @@ namespace pinocchio
bp::def("computeDistance",&computeDistance,
bp::args("geometry_model", "geometry_data", "pair_index"),
"Compute the distance between the two geometry objects of a given collision pair for a GeometryModel and associated GeometryData.",
bp::return_value_policy<bp::return_by_value>()
// bp::with_custodian_and_ward_postcall<0,1>()
bp::with_custodian_and_ward_postcall<0,2,bp::return_value_policy<bp::reference_existing_object> >()
);
bp::def("computeDistances",
......
......@@ -4,12 +4,14 @@
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/rnea-derivatives.hpp"
#include "pinocchio/bindings/python/utils/eigen.hpp"
namespace pinocchio
{
namespace python
{
namespace bp = boost::python;
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
Data::MatrixXs computeGeneralizedGravityDerivatives(const Model & model, Data & data,
......@@ -31,27 +33,29 @@ namespace pinocchio
return res;
}
void computeRNEADerivatives(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
bp::tuple computeRNEADerivatives(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
pinocchio::computeRNEADerivatives(model,data,q,v,a);
// Symmetrize M
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
make_symmetric(data.M);
return bp::make_tuple(make_ref(data.dtau_dq),
make_ref(data.dtau_dv),
make_ref(data.M));
}
void computeRNEADerivatives_fext(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const ForceAlignedVector & fext)
bp::tuple computeRNEADerivatives_fext(const Model & model, Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a,
const ForceAlignedVector & fext)
{
pinocchio::computeRNEADerivatives(model,data,q,v,a,fext);
// Symmetrize M
data.M.triangularView<Eigen::StrictlyLower>()
= data.M.transpose().triangularView<Eigen::StrictlyLower>();
make_symmetric(data.M);
return bp::make_tuple(make_ref(data.dtau_dq),
make_ref(data.dtau_dv),
make_ref(data.M));
}
void exposeRNEADerivatives()
......
......@@ -7,6 +7,7 @@
#include <boost/python.hpp>
#include <eigenpy/memory.hpp>
#include <eigenpy/eigen-to-python.hpp>
#include "pinocchio/multibody/geometry.hpp"
......@@ -45,11 +46,8 @@ namespace pinocchio
bp::args("self","otherGeometryObject"),
"Copy constructor"
))
.add_property("meshScale",
bp::make_getter(&GeometryObject::meshScale,
bp::return_value_policy<bp::return_by_value>()),
bp::make_setter(&GeometryObject::meshScale),
"Scaling parameter of the mesh.")
.def_readwrite("meshScale",&GeometryObject::meshScale,
"Scaling parameter of the mesh.")
.add_property("meshColor",
bp::make_getter(&GeometryObject::meshColor,
bp::return_value_policy<bp::return_by_value>()),
......
......@@ -10,6 +10,8 @@
#include "pinocchio/multibody/joint/joint-collection.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include <eigenpy/eigen-to-python.hpp>
namespace pinocchio
{
namespace python
......@@ -30,11 +32,10 @@ namespace pinocchio
{
return cl
.def(bp::init<double, double, double> (bp::args("x", "y", "z"), "Init JointModelRevoluteUnaligned from the components x, y, z of the axis"))
.def(bp::init<Eigen::Vector3d> (bp::args("axis"), "Init JointModelRevoluteUnaligned from an axis with x-y-z components"))
.add_property("axis",
make_getter(&JointModelRevoluteUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
make_setter(&JointModelRevoluteUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
"Rotation axis of the JointModelRevoluteUnaligned.")
.def(bp::init<Eigen::Vector3d> (bp::args("axis"),
"Init JointModelRevoluteUnaligned from an axis with x-y-z components"))
.def_readwrite("axis",&JointModelRevoluteUnaligned::axis,
"Rotation axis of the JointModelRevoluteUnaligned.")
;
}
......@@ -43,12 +44,12 @@ namespace pinocchio
inline bp::class_<JointModelPrismaticUnaligned>& expose_joint_model<JointModelPrismaticUnaligned> (bp::class_<JointModelPrismaticUnaligned> & cl)
{
return cl
.def(bp::init<double, double, double> (bp::args("x", "y", "z"), "Init JointModelPrismaticUnaligned from the components x, y, z of the axis"))
.def(bp::init<Eigen::Vector3d> (bp::args("axis"), "Init JointModelPrismaticUnaligned from an axis with x-y-z components"))
.add_property("axis",
make_getter(&JointModelPrismaticUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
make_setter(&JointModelPrismaticUnaligned::axis, bp::return_value_policy<bp::return_by_value>()),
"Translation axis of the JointModelPrismaticUnaligned.")
.def(bp::init<double, double, double> (bp::args("x", "y", "z"),
"Init JointModelPrismaticUnaligned from the components x, y, z of the axis"))
.def(bp::init<Eigen::Vector3d> (bp::args("axis"),
"Init JointModelPrismaticUnaligned from an axis with x-y-z components"))
.def_readwrite("axis",&JointModelPrismaticUnaligned::axis,
"Translation axis of the JointModelPrismaticUnaligned.")
;
}
......
//
// Copyright (c) 2020 INRIA
//
#ifndef __pinocchio_python_utils_eigen_hpp__
#define __pinocchio_python_utils_eigen_hpp__
#include "pinocchio/bindings/python/fwd.hpp"
#include <eigenpy/eigen-to-python.hpp>
namespace pinocchio
{
namespace python
{
template<typename Matrix>
Eigen::Ref<Matrix> make_ref(Eigen::PlainObjectBase<Matrix> & mat)
{
typedef Eigen::Ref<Matrix> ReturnType;
return ReturnType(mat);
}
template<typename Matrix>
void make_symmetric(Eigen::MatrixBase<Matrix> & mat)
{
mat.template triangularView<Eigen::StrictlyLower>() =
mat.transpose().template triangularView<Eigen::StrictlyLower>();
}
template<typename Matrix>
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix) make_copy(const Eigen::MatrixBase<Matrix> & mat)
{
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix) ReturnType;
return ReturnType(mat);
}
} // namespace python
} // namespace pinocchio
#endif // ifndef __pinocchio_python_utils_eigen_hpp__
......@@ -4,25 +4,38 @@
SET(${PROJECT_NAME}_PYTHON_TESTS
bindings
# Multibody
bindings_joint_composite
bindings_model
bindings_data
bindings_com
bindings_com_velocity_derivatives
bindings_regressor
bindings_dynamics
bindings_geometry_model
bindings_liegroups
# Spatial
bindings_force
bindings_frame
bindings_kinematics
bindings_inertia
bindings_spatial
bindings_joint_composite
bindings_motion
bindings_SE3
bindings_geometry_model
bindings_rnea
bindings_liegroups
explog
rpy
# Algo
bindings_com
bindings_com_velocity_derivatives
bindings_regressor
bindings_dynamics
bindings_kinematics
bindings_rnea
bindings_aba
# Algo derivatives
bindings_forward_dynamics_derivatives
bindings_inverse_dynamics_derivatives
bindings_centroidal_dynamics_derivatives
utils
serialization
version
......
import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
import numpy as np
class TestABA(TestCase):
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.full((self.model.nq,1),np.pi)
self.q = pin.randomConfiguration(self.model,-qmax,qmax)
self.v = np.random.rand(self.model.nv)
self.ddq = np.random.rand(self.model.nv)
self.fext = []
for _ in range(self.model.njoints):
self.fext.append(pin.Force.Random())
def test_aba(self):
model = self.model
ddq = pin.aba(self.model,self.data,self.q,self.v,self.ddq)
null_fext = pin.StdVec_Force()
for _ in range(model.njoints):
null_fext.append(pin.Force.Zero())
ddq_null_fext = pin.aba(self.model,self.data,self.q,self.v,self.ddq,null_fext)
self.assertApprox(ddq_null_fext,ddq)
null_fext_list = []
for f in null_fext:
null_fext_list.append(f)
print('size:',len(null_fext_list))
ddq_null_fext_list = pin.aba(self.model,self.data,self.q,self.v,self.ddq,null_fext_list)
self.assertApprox(ddq_null_fext_list,ddq)
def test_computeMinverse(self):
model = self.model
Minv = pin.computeMinverse(model,self.data,self.q)
data2 = model.createData()
M = pin.crba(model,data2,self.q)
self.assertApprox(np.linalg.inv(M),Minv)
if __name__ == '__main__':
unittest.main()
import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
import numpy as np
class TestDeriavtives(TestCase):
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.full((self.model.nq,1),np.pi)
self.q = pin.randomConfiguration(self.model,-qmax,qmax)
self.v = np.random.rand((self.model.nv))
self.a = np.random.rand((self.model.nv))
def test_centroidal_derivatives(self):
res = pin.computeCentroidalDynamicsDerivatives(self.model,self.data,self.q,self.v,self.a)
self.assertTrue(len(res) == 4)
data2 = self.model.createData()
pin.computeCentroidalMomentumTimeVariation(self.model,data2,self.q,self.v,self.a)
self.assertApprox(self.data.hg,data2.hg)
self.assertApprox(self.data.dhg,data2.dhg)
data3 = self.model.createData()
pin.computeRNEADerivatives(self.model,data3,self.q,self.v,self.a)
res2 = pin.getCentroidalDynamicsDerivatives(self.model,data3)
for k in range(4):
self.assertApprox(res[k],res2[k])
if __name__ == '__main__':
unittest.main()
import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
import numpy as np
class TestDeriavtives(TestCase):
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.full((self.model.nq,1),np.pi)
self.q = pin.randomConfiguration(self.model,-qmax,qmax)
self.v = np.random.rand((self.model.nv))
self.tau = np.random.rand((self.model.nv))
self.fext = []
for _ in range(self.model.njoints):
self.fext.append(pin.Force.Random())
def test_aba_derivatives(self):
res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau)
self.assertTrue(len(res) == 3)
data2 = self.model.createData()
pin.aba(self.model,data2,self.q,self.v,self.tau)
self.assertApprox(self.data.ddq,data2.ddq)
# With external forces
res = pin.computeABADerivatives(self.model,self.data,self.q,self.v,self.tau,self.fext)
self.assertTrue(len(res) == 3)
pin.aba(self.model,data2,self.q,self.v,self.tau,self.fext)
self.assertApprox(self.data.ddq,data2.ddq)
if __name__ == '__main__':
unittest.main()
import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
import numpy as np
class TestDeriavtives(TestCase):
def setUp(self):
self.model = pin.buildSampleModelHumanoidRandom()
self.data = self.model.createData()
qmax = np.full((self.model.nq,1),np.pi)
self.q = pin.randomConfiguration(self.model,-qmax,qmax)
self.v = np.random.rand((self.model.nv))
self.a = np.random.rand((self.model.nv))
self.fext = []
for _ in range(self.model.njoints):
self.fext.append(pin.Force.Random())
def test_rnea_derivatives(self):
res = pin.computeRNEADerivatives(self.model,self.data,self.q,self.v,self.a)
self.assertTrue(len(res) == 3)
data2 = self.model.createData()
pin.rnea(self.model,data2,self.q,self.v,self.a)
self.assertApprox(self.data.ddq,data2.ddq)
# With external forces
res = pin.computeRNEADerivatives(self.model,self.data,self.q,self.v,self.a,self.fext)
self.assertTrue(len(res) == 3)
pin.rnea(self.model,data2,self.q,self.v,self.a,self.fext)
self.assertApprox(self.data.ddq,data2.ddq)
def test_generalized_gravity_derivatives(self):
res = pin.computeGeneralizedGravityDerivatives(self.model,self.data,self.q)
data2 = self.model.createData()
ref,_,_ = pin.computeRNEADerivatives(self.model,data2,self.q,self.v*0,self.a*0)
self.assertApprox(res,ref)
def test_static_torque_derivatives(self):
res = pin.computeStaticTorqueDerivatives(self.model,self.data,self.q,self.fext)
data2 = self.model.createData()
ref,_,_ = pin.computeRNEADerivatives(self.model,data2,self.q,self.v*0,self.a*0,self.fext)
self.assertApprox(res,ref)
if __name__ == '__main__':
unittest.main()
......@@ -8,8 +8,10 @@ class TestLiegroupBindings(unittest.TestCase):
R3 = pin.liegroups.R3()
SO3 = pin.liegroups.SO3()
R3xSO3 = R3 * SO3
R10 = pin.liegroups.Rn(10)
self.assertEqual(R3.name, "R^3")
self.assertEqual(R10.name, "R^10")
self.assertEqual(SO3.name, "SO(3)")
self.assertEqual(R3xSO3.name, R3.name + " x " + SO3.name)
......
......@@ -2,8 +2,6 @@ import unittest
from test_case import PinocchioTestCase as TestCase
import pinocchio as pin
from pinocchio.utils import rand, zero
import numpy as np
class TestRNEA(TestCase):
......@@ -17,6 +15,10 @@ class TestRNEA(TestCase):
self.v = np.random.rand(self.model.nv)
self.a = np.random.rand(self.model.nv)
self.fext = []
for _ in range(self.model.njoints):
self.fext.append(pin.Force.Random())
def test_rnea(self):
model = self.model
tau = pin.rnea(self.model,self.data,self.q,self.v,self.a)
......@@ -36,5 +38,45 @@ class TestRNEA(TestCase):
tau_null_fext_list = pin.rnea(self.model,self.data,self.q,self.v,self.a,null_fext_list)
self.assertApprox(tau_null_fext_list,tau)
def test_nle(self):
model = self.model