Skip to content

GitLab

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

Merge pull request #1100 from jcarpent/devel 

Update Inertia class and fix Python doc
parents a2770918 53126cdb
Hide whitespace changes
Inline Side-by-side
bindings/python/multibody/geometry-object.hpp
View file @ edf6529e
//
// Copyright (c) 2017-2019 CNRS INRIA
// Copyright (c) 2017-2020 CNRS INRIA
//
#ifndef __pinocchio_python_geometry_object_hpp__
......@@ -30,14 +30,14 @@ namespace pinocchio
.def(bp::init<std::string,FrameIndex,JointIndex,CollisionGeometryPtr,SE3,
bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
(
bp::args("self","name","parent_frame index","parent_joint index","collision_geometry",
"placement", "meshPath", "meshScale", "overrideMaterial", "meshColor", "meshTexturePath"),
bp::args("self","name","parent_frame","parent_joint","collision_geometry",
"placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
"Full constructor of a GeometryObject."))
.def(bp::init<std::string,JointIndex,CollisionGeometryPtr,SE3,
bp::optional<std::string,Eigen::Vector3d,bool,Eigen::Vector4d,std::string> >
(
bp::args("self","name","parent_joint index","collision_geometry",
"placement", "meshPath", "meshScale", "overrideMaterial", "meshColor", "meshTexturePath"),
bp::args("self","name","parent_joint","collision_geometry",
"placement", "mesh_path", "mesh_scale", "override_material", "mesh_color", "mesh_texture_path"),
"Reduced constructor of a GeometryObject. This constructor does not require to specify the parent frame index."
))
.add_property("meshScale",
......@@ -78,10 +78,10 @@ namespace pinocchio
}
#ifdef PINOCCHIO_WITH_HPP_FCL
static GeometryObject maker_capsule( const double radius , const double length)
static GeometryObject maker_capsule(const double radius, const double length)
{
return GeometryObject("",FrameIndex(0),JointIndex(0),
boost::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule (radius, length)),
boost::shared_ptr<fcl::CollisionGeometry>(new fcl::Capsule(radius, length)),
SE3::Identity());
}
......
bindings/python/spatial/force.hpp
View file @ edf6529e
......@@ -81,13 +81,13 @@ namespace pinocchio
.add_property("np",&ForcePythonVisitor::getVector)
.def("se3Action",&Force::template se3Action<Scalar,Options>,
bp::args("M"),"Returns the result of the dual action of M on *this.")
bp::args("self","M"),"Returns the result of the dual action of M on *this.")
.def("se3ActionInverse",&Force::template se3ActionInverse<Scalar,Options>,
bp::args("M"),"Returns the result of the dual action of the inverse of M on *this.")
bp::args("self","M"),"Returns the result of the dual action of the inverse of M on *this.")
.def("setZero",&ForcePythonVisitor::setZero,
.def("setZero",&ForcePythonVisitor::setZero,bp::arg("self"),
"Set the linear and angular components of *this to zero.")
.def("setRandom",&ForcePythonVisitor::setRandom,
.def("setRandom",&ForcePythonVisitor::setRandom,bp::arg("self"),
"Set the linear and angular components of *this to random values.")
.def(bp::self + bp::self)
......@@ -105,12 +105,12 @@ namespace pinocchio
.def("isApprox",
&call<Force>::isApprox,
isApproxForce_overload(bp::args("other","prec"),
isApproxForce_overload(bp::args("self","other","prec"),
"Returns true if *this is approximately equal to other, within the precision given by prec."))
.def("isZero",
&call<Force>::isZero,
isZero_overload(bp::args("prec"),
isZero_overload(bp::args("self","prec"),
"Returns true if *this is approximately equal to the zero Force, within the precision given by prec."))
.def("Random",&Force::Random,"Returns a random Force.")
......
bindings/python/spatial/inertia.hpp
View file @ edf6529e
......@@ -82,36 +82,45 @@ namespace pinocchio
&InertiaPythonVisitor::setInertia,
"Rotational part of the Spatial Inertia, i.e. a symmetric matrix representing the rotational inertia around the center of mass.")
.def("matrix",&Inertia::matrix)
.def("matrix",&Inertia::matrix,bp::arg("self"))
.def("se3Action",&Inertia::se3Action,
bp::args("M"),"Returns the result of the action of M on *this.")
bp::args("self","M"),"Returns the result of the action of M on *this.")
.def("se3ActionInverse",&Inertia::se3ActionInverse,
bp::args("M"),"Returns the result of the action of the inverse of M on *this.")
bp::args("self","M"),"Returns the result of the action of the inverse of M on *this.")
.def("setIdentity",&Inertia::setIdentity,"Set *this to be the Identity inertia.")
.def("setZero",&Inertia::setZero,"Set all the components of *this to zero.")
.def("setRandom",&Inertia::setRandom,"Set all the components of *this to random values.")
.def("setIdentity",&Inertia::setIdentity,bp::arg("self"),
"Set *this to be the Identity inertia.")
.def("setZero",&Inertia::setZero,bp::arg("self"),
"Set all the components of *this to zero.")
.def("setRandom",&Inertia::setRandom,bp::arg("self"),
"Set all the components of *this to random values.")
.def(bp::self + bp::self)
.def(bp::self * bp::other<Motion>() )
.add_property("np",&Inertia::matrix)
.def("vxiv",&Inertia::vxiv,bp::arg("Motion v"),"Returns the result of v x Iv.")
.def("vtiv",&Inertia::vtiv,bp::arg("Motion v"),"Returns the result of v.T * Iv.")
.def("vxi",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::vxi,bp::arg("Motion v"),"Returns the result of v x* I, a 6x6 matrix.")
.def("ivx",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::ivx,bp::arg("Motion v"),"Returns the result of I vx, a 6x6 matrix.")
.def("variation",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::variation,bp::arg("Motion v"),"Returns the time derivative of the inertia.")
.def("vxiv",&Inertia::vxiv,bp::args("self","v"),"Returns the result of v x Iv.")
.def("vtiv",&Inertia::vtiv,bp::args("self","v"),"Returns the result of v.T * Iv.")
.def("vxi",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::vxi,
bp::args("self","v"),
"Returns the result of v x* I, a 6x6 matrix.")
.def("ivx",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::ivx,
bp::args("self","v"),
"Returns the result of I vx, a 6x6 matrix.")
.def("variation",(Matrix6 (Inertia::*)(const Motion &) const)&Inertia::variation,
bp::args("self","v"),
"Returns the time derivative of the inertia.")
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def("isApprox",
call<Inertia>::isApprox,
isApproxInertia_overload(bp::args("other","prec"),
isApproxInertia_overload(bp::args("self","other","prec"),
"Returns true if *this is approximately equal to other, within the precision given by prec."))
.def("isZero",
call<Inertia>::isZero,
isZero_overload(bp::args("prec"),
isZero_overload(bp::args("self","prec"),
"Returns true if *this is approximately equal to the zero Inertia, within the precision given by prec."))
.def("Identity",&Inertia::Identity,"Returns the identity Inertia.")
......@@ -121,22 +130,26 @@ namespace pinocchio
.def("Random",&Inertia::Random,"Returns a random Inertia.")
.staticmethod("Random")
.def("toDynamicParameters",&InertiaPythonVisitor::toDynamicParameters_proxy,
.def("toDynamicParameters",&InertiaPythonVisitor::toDynamicParameters_proxy,bp::arg("self"),
"Returns the representation of the matrix as a vector of dynamic parameters."
"\nThe parameters are given as v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
"where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter"
)
.def("FromDynamicParameters",&Inertia::template FromDynamicParameters<Eigen::VectorXd>,
bp::args("Dynamic parameters (size 10)"),
bp::args("dynamic_parameters"),
"Builds and inertia matrix from a vector of dynamic parameters."
"\nThe parameters are given as v = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
"where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter"
"\nThe parameters are given as dynamic_parameters = [m, mc_x, mc_y, mc_z, I_{xx}, I_{xy}, I_{yy}, I_{xz}, I_{yz}, I_{zz}]^T "
"where I = I_C + mS^T(c)S(c) and I_C has its origin at the barycenter."
)
.staticmethod("FromDynamicParameters")
.def("FromSphere", &Inertia::FromSphere,
bp::args("mass","radius"),
"Returns the Inertia of an sphere with a given mass and of radius.")
.staticmethod("FromSphere")
.def("FromEllipsoid", &Inertia::FromEllipsoid,
bp::args("mass","length_x","length_y","length_z"),
"Returns an Inertia of an ellipsoid shape with a mass and of dimension the semi axis of length_{x,y,z}.")
"Returns the Inertia of an ellipsoid shape with a given mass and of given dimensions the semi-axis of values length_{x,y,z}.")
.staticmethod("FromEllipsoid")
.def("FromCylinder", &Inertia::FromCylinder,
bp::args("mass","radius","length"),
......@@ -144,7 +157,7 @@ namespace pinocchio
.staticmethod("FromCylinder")
.def("FromBox", &Inertia::FromBox,
bp::args("mass","length_x","length_y","length_z"),
"Returns an Inertia of a box shape with a mass and of dimension the semi axis of length_{x,y,z}.")
"Returns the Inertia of a box shape with a mass and of dimension the semi axis of length_{x,y,z}.")
.staticmethod("FromBox")
.def("__array__",&Inertia::matrix)
......
bindings/python/spatial/motion.hpp
View file @ edf6529e
......@@ -85,22 +85,22 @@ namespace pinocchio
.add_property("np",&MotionPythonVisitor::getVector)
.def("se3Action",&Motion::template se3Action<Scalar,Options>,
bp::args("M"),"Returns the result of the action of M on *this.")
bp::args("self","M"),"Returns the result of the action of M on *this.")
.def("se3ActionInverse",&Motion::template se3ActionInverse<Scalar,Options>,
bp::args("M"),"Returns the result of the action of the inverse of M on *this.")
bp::args("self","M"),"Returns the result of the action of the inverse of M on *this.")
.add_property("action",&Motion::toActionMatrix,"Returns the action matrix of *this (acting on Motion).")
.add_property("dualAction",&Motion::toDualActionMatrix,"Returns the dual action matrix of *this (acting on Force).")
.def("setZero",&MotionPythonVisitor::setZero,
.def("setZero",&MotionPythonVisitor::setZero,bp::arg("self"),
"Set the linear and angular components of *this to zero.")
.def("setRandom",&MotionPythonVisitor::setRandom,
.def("setRandom",&MotionPythonVisitor::setRandom,bp::arg("self"),
"Set the linear and angular components of *this to random values.")
.def("cross",(Motion (Motion::*)(const Motion &) const) &Motion::cross,
bp::args("m"),"Action of *this onto another Motion m. Returns ¨*this x m.")
bp::args("self","m"),"Action of *this onto another Motion m. Returns ¨*this x m.")
.def("cross",(Force (Motion::*)(const Force &) const) &Motion::cross,
bp::args("f"),"Dual action of *this onto a Force f. Returns *this x* f.")
bp::args("self","f"),"Dual action of *this onto a Force f. Returns *this x* f.")
.def(bp::self + bp::self)
.def(bp::self += bp::self)
......@@ -119,12 +119,12 @@ namespace pinocchio
.def("isApprox",
call<Motion>::isApprox,
isApproxMotion_overload(bp::args("other","prec"),
isApproxMotion_overload(bp::args("self","other","prec"),
"Returns true if *this is approximately equal to other, within the precision given by prec."))
.def("isZero",
call<Motion>::isZero,
isZero_overload(bp::args("prec"),
isZero_overload(bp::args("self","prec"),
"Returns true if *this is approximately equal to the zero Motion, within the precision given by prec."))
.def("Random",&Motion::Random,"Returns a random Motion.")
......
bindings/python/spatial/se3.hpp
View file @ edf6529e
......@@ -87,59 +87,62 @@ namespace pinocchio
"Returns the homegeneous matrix of *this (acting on SE3).")
.add_property("action",&SE3::toActionMatrix,
"Returns the action matrix of *this (acting on Motion).")
.def("toActionMatrix",&SE3::toActionMatrix,
.def("toActionMatrix",&SE3::toActionMatrix,bp::arg("self"),
"Returns the action matrix of *this (acting on Motion).")
.add_property("actionInverse",&SE3::toActionMatrixInverse,
"Returns the inverse of the action matrix of *this (acting on Motion).\n"
"This is equivalent to do m.inverse().action")
.def("toActionMatrixInverse",&SE3::toActionMatrixInverse,
.def("toActionMatrixInverse",&SE3::toActionMatrixInverse,bp::arg("self"),
"Returns the inverse of the action matrix of *this (acting on Motion).\n"
"This is equivalent to do m.inverse().toActionMatrix()")
.add_property("dualAction",&SE3::toDualActionMatrix,
"Returns the dual action matrix of *this (acting on Force).")
.def("toDualActionMatrix",&SE3::toDualActionMatrix,
.def("toDualActionMatrix",&SE3::toDualActionMatrix,bp::arg("self"),
"Returns the dual action matrix of *this (acting on Force).")
.def("setIdentity",&SE3PythonVisitor::setIdentity,"Set *this to the identity placement.")
.def("setRandom",&SE3PythonVisitor::setRandom,"Set *this to a random placement.")
.def("setIdentity",&SE3PythonVisitor::setIdentity,bp::arg("self"),
"Set *this to the identity placement.")
.def("setRandom",&SE3PythonVisitor::setRandom,bp::arg("self"),
"Set *this to a random placement.")
.def("inverse", &SE3::inverse)
.def("inverse", &SE3::inverse, bp::arg("self"),
"Returns the inverse transform")
.def("act", (Vector3 (SE3::*)(const Vector3 &) const) &SE3::act,
bp::args("point"),
bp::args("self","point"),
"Returns a point which is the result of the entry point transforms by *this.")
.def("actInv", (Vector3 (SE3::*)(const Vector3 &) const) &SE3::actInv,
bp::args("point"),
bp::args("self","point"),
"Returns a point which is the result of the entry point by the inverse of *this.")
.def("act", (SE3 (SE3::*)(const SE3 & other) const) &SE3::act,
bp::args("M"), "Returns the result of *this * M.")
bp::args("self","M"), "Returns the result of *this * M.")
.def("actInv", (SE3 (SE3::*)(const SE3 & other) const) &SE3::actInv,
bp::args("M"), "Returns the result of the inverse of *this times M.")
bp::args("self","M"), "Returns the result of the inverse of *this times M.")
.def("act", (Motion (SE3::*)(const Motion &) const) &SE3::act,
bp::args("motion"), "Returns the result action of *this onto a Motion.")
bp::args("self","motion"), "Returns the result action of *this onto a Motion.")
.def("actInv", (Motion (SE3::*)(const Motion &) const) &SE3::actInv,
bp::args("motion"), "Returns the result of the inverse of *this onto a Motion.")
bp::args("self","motion"), "Returns the result of the inverse of *this onto a Motion.")
.def("act", (Force (SE3::*)(const Force &) const) &SE3::act,
bp::args("force"), "Returns the result of *this onto a Force.")
bp::args("self","force"), "Returns the result of *this onto a Force.")
.def("actInv", (Force (SE3::*)(const Force &) const) &SE3::actInv,
bp::args("force"), "Returns the result of the inverse of *this onto an Inertia.")
bp::args("self","force"), "Returns the result of the inverse of *this onto an Inertia.")
.def("act", (Inertia (SE3::*)(const Inertia &) const) &SE3::act,
bp::args("inertia"), "Returns the result of *this onto a Force.")
bp::args("self","inertia"), "Returns the result of *this onto a Force.")
.def("actInv", (Inertia (SE3::*)(const Inertia &) const) &SE3::actInv,
bp::args("inertia"), "Returns the result of the inverse of *this onto an Inertia.")
bp::args("self","inertia"), "Returns the result of the inverse of *this onto an Inertia.")
.def("isApprox",
call<SE3>::isApprox,
isApproxSE3_overload(bp::args("other","prec"),
isApproxSE3_overload(bp::args("self","other","prec"),
"Returns true if *this is approximately equal to other, within the precision given by prec."))
.def("isIdentity",
&SE3::isIdentity,
isIdentity_overload(bp::args("prec"),
isIdentity_overload(bp::args("self","prec"),
"Returns true if *this is approximately equal to the identity placement, within the precision given by prec."))
.def("__invert__",&SE3::inverse,"Returns the inverse of *this.")
......
src/multibody/geometry.hxx
View file @ edf6529e
......@@ -19,7 +19,7 @@ namespace pinocchio
#ifdef PINOCCHIO_WITH_HPP_FCL
, activeCollisionPairs(geom_model.collisionPairs.size(), true)
, distanceRequest(true, 0, 0, fcl::GST_INDEP)
, distanceRequest(true)
, distanceResults(geom_model.collisionPairs.size())
, collisionRequest(::hpp::fcl::NO_REQUEST,1)
, collisionResults(geom_model.collisionPairs.size())
......
src/spatial/inertia.hpp
View file @ edf6529e
......@@ -228,6 +228,11 @@ namespace pinocchio
Vector3::Random(),
Symmetric3::RandomPositive());
}
static InertiaTpl FromSphere(const Scalar m, const Scalar radius)
{
return FromEllipsoid(m,radius,radius,radius);
}
static InertiaTpl FromEllipsoid(const Scalar m, const Scalar x,
const Scalar y, const Scalar z)
......
unittest/spatial.cpp
View file @ edf6529e
......@@ -543,6 +543,15 @@ BOOST_AUTO_TEST_CASE ( test_Inertia )
// Test constructor (Matrix6)
Inertia I1_bis(I1.matrix());
BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix()));
// Test Inertia from ellipsoid
const double sphere_mass = 5.;
const double sphere_radius = 2.;
I1 = Inertia::FromSphere(sphere_mass, sphere_radius);
const double L_sphere = 2./5. * sphere_mass * sphere_radius * sphere_radius;
BOOST_CHECK_SMALL(I1.mass() - sphere_mass, 1e-12);
BOOST_CHECK(I1.lever().isZero());
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(L_sphere, 0., L_sphere , 0., 0., L_sphere).matrix()));
// Test Inertia from ellipsoid
I1 = Inertia::FromEllipsoid(2., 3., 4., 5.);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment