...
 
Commits (150)
......@@ -3,3 +3,4 @@ Xcode*
*~
*.pyc
coverage*
.vscode*
......@@ -39,7 +39,7 @@ allow_failures:
- compiler:
before_install: ./travis_custom/custom_before_install
install:
- pip install --user coveralls
- pip install --user pyopenssl
- pip install --user numpy
script:
- export CMAKE_ADDITIONAL_OPTIONS="-DCMAKE_BUILD_TYPE=${BUILDTYPE} -DBUILD_WITH_COLLISION_SUPPORT=${BUILD_WITH_COLLISION_SUPPORT}"
......@@ -51,6 +51,3 @@ after_success:
#- export PYTHONPATH=$install_dir/lib/python2.7/site-packages
#- coveralls-lcov -v -n $build_dir/coverage.info > coverage.json
- export PYTHONPATH=$PYHTONPATH:/tmp/_ci/install/lib/python2.7/site-packages:/usr/lib/python2.7/dist-packages
- coveralls-lcov -v -n /tmp/_ci/build/coverage.info > coverage.json
- coverage2 run ./python/tests.py
- coveralls --merge=coverage.json
......@@ -36,7 +36,7 @@ If you want to learn more on **Pinocchio** internal behaviors and main features,
**Pinocchio** is multi-thread friendly.
**Pinocchio** is reliable and extensively tested (unit-tests, simulations and real robotics applications).
**Pinocchio** is supported on Mac Os X and many Linux distributions ([See build status here](http://robotpkg.openrobots.org/rbulk/robotpkg/math/pinocchio/index.html)).
**Pinocchio** is supported on Mac Os X and many Linux distributions ([see build status here](http://robotpkg.openrobots.org/rbulk/robotpkg/math/pinocchio/index.html)).
## Ongoing developments
......@@ -48,9 +48,14 @@ If you want to follow the current developments, you can directly refer to the [d
**Pinocchio** is also being deployed on ROS, you may follow its deployment status on [Melodic](https://index.ros.org/r/pinocchio/#melodic) or [Kinetic](https://index.ros.org/r/pinocchio/#kinetic).
## Documentation
The online **Pinocchio** documentation of the last release is available [here](https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/).
## Examples
We provide some basic examples on how to use **Pinocchio** in Python in the [examples/python](./examples/python) directory.
Additional examples introducing **Pinocchio** are also available in the [documentation](https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_d-practical-exercises_intro.html)
## Tutorials
......@@ -101,9 +106,10 @@ The following people have been involved in the development of **Pinocchio**:
- [Guilhem Saurel](http://projects.laas.fr/gepetto/index.php/Members/GuilhemSaurel) (LAAS-CNRS): continuous integration and deployment
- [Joseph Mirabel](http://jmirabel.github.io/) (LAAS-CNRS): Lie groups support
- [Antonio El Khoury](https://www.linkedin.com/in/antonioelkhoury) (Wandercraft): bug fixes
- [Gabriele Buondono](http://projects.laas.fr/gepetto/index.php/Members/GabrieleBuondonno) (LAAS-CNRS): bug fixes
- [Gabriele Buondono](http://projects.laas.fr/gepetto/index.php/Members/GabrieleBuondonno) (LAAS-CNRS): features extension, bug fixes and Python bindings
- [Florian Valenza](https://fr.linkedin.com/in/florian-valenza-1b274082) (Astek): core developments and FCL support
- [Wolfgang Merkt](http://www.wolfgangmerkt.com/) (University of Edinburgh): ROS integration and support
- [Rohan Budhiraja](https://scholar.google.com/citations?user=NW9Io9AAAAAJ) (LAAS-CNRS): features extension
If you have taken part to the development of **Pinocchio**, feel free to add your name and contribution here.
......
......@@ -80,6 +80,7 @@ IF(NOT HPP_FCL_FOUND)
multibody/fcl/collision-result.hpp
multibody/fcl/distance-result.hpp
multibody/fcl/collision-geometry.hpp
multibody/fcl/mesh-loader.hpp
)
LIST(REMOVE_ITEM ${PROJECT_NAME}_PYTHON_SOURCES
......@@ -167,6 +168,21 @@ FOREACH(python ${PYTHON_FILES})
DESTINATION ${${PYWRAP}_INSTALL_DIR})
ENDFOREACH(python)
# --- INSTALL VISUALIZATION SCRIPTS
SET(PYTHON_VISUALIZE_FILES
__init__.py
base_visualizer.py
gepetto_visualizer.py
meshcat_visualizer.py
)
FOREACH(python ${PYTHON_VISUALIZE_FILES})
PYTHON_BUILD(${PROJECT_NAME}/visualize ${python})
INSTALL(FILES
"${${PROJECT_NAME}_SOURCE_DIR}/bindings/python/pinocchio/visualize/${python}"
DESTINATION ${${PYWRAP}_INSTALL_DIR}/visualize)
ENDFOREACH(python)
# --- PACKAGING --- #
# Format string
......
......@@ -33,6 +33,7 @@ namespace pinocchio
void exposeRNEADerivatives();
void exposeABADerivatives();
void exposeKinematicsDerivatives();
void exposeCentroidalDerivatives();
void exposeAlgorithms();
......
......@@ -51,7 +51,7 @@ namespace pinocchio
"put 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 StdVect_Force.");
"The forces are of type StdVec_Force.");
}
} // namespace python
} // namespace pinocchio
......@@ -26,7 +26,7 @@ namespace pinocchio
exposeGeometryAlgo();
exposeRegressor();
exposeCholesky();
exposeCentroidalDerivatives();
// expose derivative version of the algorithms
exposeRNEADerivatives();
exposeABADerivatives();
......
//
// Copyright (c) 2019 CNRS
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/algorithm/centroidal-derivatives.hpp"
#include <boost/python/tuple.hpp>
namespace pinocchio
{
namespace python
{
bp::tuple computeCentroidalDynamicsDerivatives_proxy(const Model & model,
Data & data,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v,
const Eigen::VectorXd & a)
{
typedef Data::Matrix6x Matrix6x;
Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
Matrix6x partial_da(Matrix6x::Zero(6,model.nv));
computeCentroidalDynamicsDerivatives(model,data,q, v, a,
partialh_dq, partial_dq, partial_dv, partial_da);
return bp::make_tuple(partialh_dq, partial_dq,partial_dv,partial_da);
}
bp::tuple getCentroidalDynamicsDerivatives_proxy(const Model & model,
Data & data)
{
typedef Data::Matrix6x Matrix6x;
Matrix6x partialh_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
Matrix6x partial_da(Matrix6x::Zero(6,model.nv));
getCentroidalDynamicsDerivatives(model,data, partialh_dq, partial_dq, partial_dv, partial_da);
return bp::make_tuple(partialh_dq,partial_dq,partial_dv,partial_da);
}
void exposeCentroidalDerivatives()
{
using namespace Eigen;
bp::def("computeCentroidalDynamicsDerivatives",
computeCentroidalDynamicsDerivatives_proxy,
bp::args("Model","Data",
"q: configuration vector (size model.nq)",
"v: velocity vector (size model.nv)",
"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"),
"Retrive the analytical derivatives of the centroidal dynamics\n"
"from the RNEA derivatives.\n"
"pinocchio.computeRNEADerivatives should have been called first.");
}
} // namespace python
} // namespace pinocchio
......@@ -13,6 +13,23 @@ namespace pinocchio
void exposeCentroidal()
{
using namespace Eigen;
bp::def("computeCentroidalDynamics",
&computeCentroidalDynamics<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)"),
"Computes the Centroidal dynamics, a.k.a. the total momenta of the system expressed around the center of mass.",
bp::return_value_policy<bp::return_by_value>());
bp::def("computeCentroidalDynamics",
&computeCentroidalDynamics<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
bp::args("Model","Data",
"Joint configuration q (size Model::nq)",
"Joint velocity v (size Model::nv)",
"Joint acceleration a (size Model::nv)"),
"Computes the Centroidal dynamics and its time derivatives, a.k.a. the total momenta of the system and its time derivative expressed around the center of mass.",
bp::return_value_policy<bp::return_by_value>());
bp::def("ccrba",
&ccrba<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
......
......@@ -26,7 +26,19 @@ namespace pinocchio
bp::args("model", "data", "geometry model", "geometry data"),
"Update the placement of the collision objects according to the current joint placement stored in data."
);
bp::def("setGeometryMeshScales",
(void (*)(GeometryModel &, const Vector3d &))&setGeometryMeshScales<Vector3d>,
bp::args("geometry model", "scale"),
"Set a mesh scaling vector to each GeometryObject contained in the the GeometryModel."
);
bp::def("setGeometryMeshScales",
(void (*)(GeometryModel &, const double))&setGeometryMeshScales,
bp::args("geometry model", "scale"),
"Set an isotropic mesh scaling to each GeometryObject contained in the the GeometryModel."
);
#ifdef PINOCCHIO_WITH_HPP_FCL
bp::def("computeCollision",computeCollision,
bp::args("geometry model", "geometry data", "collision pair index"),
......
......@@ -9,19 +9,65 @@ namespace pinocchio
{
namespace python
{
Eigen::MatrixXd bodyRegressor_proxy(const Motion & v, const Motion & a)
{
return bodyRegressor(v,a);
}
Eigen::MatrixXd jointBodyRegressor_proxy(const Model & model, Data & data, const JointIndex jointId)
{
return jointBodyRegressor(model,data,jointId);
}
Eigen::MatrixXd frameBodyRegressor_proxy(const Model & model, Data & data, const FrameIndex frameId)
{
return frameBodyRegressor(model,data,frameId);
}
void exposeRegressor()
{
using namespace Eigen;
bp::def("computeStaticRegressor",
&regressor::computeStaticRegressor<double,0,JointCollectionDefaultTpl,VectorXd>,
&computeStaticRegressor<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)"),
"Compute the static regressor that links the inertia parameters of the system to its center of mass position\n"
",put the result in Data and return it.",
"Compute the static regressor that links the inertia parameters of the system to its center of mass position,\n"
"put the result in Data and return it.",
bp::return_value_policy<bp::return_by_value>());
bp::def("bodyRegressor",
&bodyRegressor_proxy,
bp::args("velocity","acceleration"),
"Computes the regressor for the dynamic parameters of a single rigid body.\n"
"The result is such that "
"Ia + v x Iv = bodyRegressor(v,a) * I.toDynamicParameters()");
bp::def("jointBodyRegressor",
&jointBodyRegressor_proxy,
bp::args("Model","Data",
"jointId (int)"),
"Compute the regressor for the dynamic parameters of a rigid body attached to a given joint.\n"
"This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects.");
bp::def("frameBodyRegressor",
&frameBodyRegressor_proxy,
bp::args("Model","Data",
"frameId (int)"),
"Computes the regressor for the dynamic parameters of a rigid body attached to a given frame.\n"
"This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects.");
bp::def("computeJointTorqueRegressor",
&computeJointTorqueRegressor<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
bp::args("Model","Data",
"Configuration q (size Model::nq)",
"Velocity v (size Model::nv)"
"Acceleration a (size Model::nv)"),
"Compute the joint torque regressor that links the joint torque "
"to the dynamic parameters of each link according to the current the robot motion,\n"
"put the result in Data and return it.",
bp::return_value_policy<bp::return_by_value>());
}
} // namespace python
......
//
// Copyright (c) 2015-2018 CNRS
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
......@@ -12,6 +12,7 @@
#include "pinocchio/bindings/python/utils/version.hpp"
#include "pinocchio/bindings/python/utils/dependencies.hpp"
#include "pinocchio/bindings/python/utils/conversions.hpp"
#include "pinocchio/bindings/python/utils/registration.hpp"
namespace bp = boost::python;
using namespace pinocchio::python;
......@@ -21,8 +22,11 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
bp::scope().attr("__version__") = pinocchio::printVersion();
eigenpy::enableEigenPy();
eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();
if(not register_symbolic_link_to_registered_type<Eigen::Quaterniond>())
eigenpy::exposeQuaternion();
if(not register_symbolic_link_to_registered_type<Eigen::AngleAxisd>())
eigenpy::exposeAngleAxis();
typedef Eigen::Matrix<double,6,6> Matrix6d;
typedef Eigen::Matrix<double,6,1> Vector6d;
......@@ -44,6 +48,7 @@ BOOST_PYTHON_MODULE(libpinocchio_pywrap)
bp::enum_< ::pinocchio::ReferenceFrame >("ReferenceFrame")
.value("WORLD",::pinocchio::WORLD)
.value("LOCAL",::pinocchio::LOCAL)
.value("LOCAL_WORLD_ALIGNED",::pinocchio::LOCAL_WORLD_ALIGNED)
;
exposeModel();
......
......@@ -12,6 +12,8 @@
#include "pinocchio/bindings/python/utils/std-vector.hpp"
#include "pinocchio/bindings/python/utils/std-aligned-vector.hpp"
#include "pinocchio/bindings/python/utils/copyable.hpp"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::Data)
namespace pinocchio
......@@ -29,22 +31,22 @@ namespace pinocchio
public:
#define ADD_DATA_PROPERTY(TYPE,NAME,DOC) \
def_readwrite(#NAME, \
&Data::NAME, \
DOC)
#define ADD_DATA_PROPERTY_READONLY(TYPE,NAME,DOC) \
def_readonly(#NAME, \
&Data::NAME, \
#define ADD_DATA_PROPERTY(TYPE,NAME,DOC) \
def_readwrite(#NAME, \
&Data::NAME, \
DOC)
#define ADD_DATA_PROPERTY_READONLY_BYVALUE(TYPE,NAME,DOC) \
add_property(#NAME, \
make_getter(&Data::NAME,bp::return_value_policy<bp::return_by_value>()), \
#define ADD_DATA_PROPERTY_READONLY(TYPE,NAME,DOC) \
def_readonly(#NAME, \
&Data::NAME, \
DOC)
#define ADD_DATA_PROPERTY_READONLY_BYVALUE(TYPE,NAME,DOC) \
add_property(#NAME, \
make_getter(&Data::NAME,bp::return_value_policy<bp::return_by_value>()), \
DOC)
/* --- Exposing C++ API to python through the handler ----------------- */
template<class PyClass>
void visit(PyClass& cl) const
......@@ -64,7 +66,7 @@ namespace pinocchio
.ADD_DATA_PROPERTY(container::aligned_vector<Force>,f,"Joint spatial force expresssed in the joint frame.")
.ADD_DATA_PROPERTY(container::aligned_vector<Force>,of,"Joint spatial force expresssed at the origin of the world frame.")
.ADD_DATA_PROPERTY(container::aligned_vector<Force>,h,"Vector of spatial momenta expressed in the local frame of the joint.")
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMi,"Body absolute placement (wrt world)")
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,oMf,"frames absolute placement (wrt world)")
.ADD_DATA_PROPERTY(container::aligned_vector<SE3>,liMi,"Body relative placement (wrt parent)")
......@@ -114,6 +116,8 @@ namespace pinocchio
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,impulse_c,"Lagrange Multipliers linked to contact impulses")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::VectorXd,dq_after,"Generalized velocity after the impact.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Matrix3x,staticRegressor,"Static regressor.")
.ADD_DATA_PROPERTY_READONLY_BYVALUE(Eigen::MatrixXd,jointTorqueRegressor,"Joint Torque Regressor.")
;
}
......@@ -124,7 +128,8 @@ namespace pinocchio
"Articulated rigid body data.\n"
"It contains all the data that can be modified by the algorithms.",
bp::no_init)
.def(DataPythonVisitor());
.def(DataPythonVisitor())
.def(CopyableVisitor<Data>());
StdAlignedVectorPythonVisitor<Vector3, true>::expose("StdVec_vec3d");
StdAlignedVectorPythonVisitor<Matrix6x, true>::expose("StdVec_Matrix6x");
StdVectorPythonVisitor<int>::expose("StdVec_int");
......
......@@ -14,7 +14,7 @@ namespace pinocchio
void exposeFrame()
{
FramePythonVisitor::expose();
StdAlignedVectorPythonVisitor<Frame>::expose("StdVect_Frame");
StdAlignedVectorPythonVisitor<Frame>::expose("StdVec_Frame");
}
} // namespace python
......
......@@ -16,7 +16,7 @@ namespace pinocchio
void exposeGeometry()
{
GeometryObjectPythonVisitor::expose();
StdAlignedVectorPythonVisitor<GeometryObject>::expose("StdVect_GeometryObject");
StdAlignedVectorPythonVisitor<GeometryObject>::expose("StdVec_GeometryObject");
CollisionPairPythonVisitor::expose();
GeometryModelPythonVisitor::expose();
......
......@@ -19,13 +19,13 @@ namespace pinocchio
{
using namespace pinocchio::python::fcl;
ContactPythonVisitor::expose();
StdVectorPythonVisitor<ContactPythonVisitor::Contact>::expose("StdVect_Contact");
StdVectorPythonVisitor<ContactPythonVisitor::Contact>::expose("StdVec_Contact");
CollisionResultPythonVisitor::expose();
StdVectorPythonVisitor<CollisionResultPythonVisitor::CollisionResult>::expose("StdVect_CollisionResult");
StdVectorPythonVisitor<CollisionResultPythonVisitor::CollisionResult>::expose("StdVec_CollisionResult");
DistanceResultPythonVisitor::expose();
StdVectorPythonVisitor<DistanceResultPythonVisitor::DistanceResult>::expose("StdVect_DistanceResult");
StdVectorPythonVisitor<DistanceResultPythonVisitor::DistanceResult>::expose("StdVec_DistanceResult");
CollisionGeometryPythonVisitor::expose();
......
......@@ -47,8 +47,8 @@ namespace pinocchio
;
bp::class_<Frame>("Frame",
"A Plucker coordinate frame related to a parent joint inside a kinematic tree.\n\n",
bp::no_init
"A Plucker coordinate frame related to a parent joint inside a kinematic tree.\n\n",
bp::no_init
)
.def(FramePythonVisitor())
.def(CopyableVisitor<Frame>())
......
......@@ -10,6 +10,7 @@
#include "pinocchio/bindings/python/utils/eigen_container.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
#include "pinocchio/bindings/python/utils/copyable.hpp"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::GeometryData)
......@@ -34,6 +35,9 @@ namespace pinocchio
.def(bp::init<const GeomIndex &, const GeomIndex &>(bp::args("co1 (index)", "co2 (index)"),
"Initializer of collision pair."))
.def(PrintableVisitor<CollisionPair>())
.def(CopyableVisitor<CollisionPair>())
.def(bp::self == bp::self)
.def(bp::self != bp::self)
.def_readwrite("first",&CollisionPair::first)
.def_readwrite("second",&CollisionPair::second);
......@@ -104,6 +108,7 @@ namespace pinocchio
bp::no_init)
.def(GeometryDataPythonVisitor())
.def(PrintableVisitor<GeometryData>())
.def(CopyableVisitor<GeometryData>())
;
}
......
......@@ -9,6 +9,7 @@
#include "pinocchio/bindings/python/utils/eigen_container.hpp"
#include "pinocchio/bindings/python/utils/printable.hpp"
#include "pinocchio/bindings/python/utils/copyable.hpp"
#include "pinocchio/multibody/geometry.hpp"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(pinocchio::GeometryModel)
......@@ -43,6 +44,7 @@ namespace pinocchio
"Add a GeometryObject to a GeometryModel and set its parent joint by reading its value in model")
.def("getGeometryId",&GeometryModel::getGeometryId)
.def("existGeometryName",&GeometryModel::existGeometryName)
.def("createData",&GeometryModelPythonVisitor::createData)
#ifdef PINOCCHIO_WITH_HPP_FCL
.add_property("collisionPairs",
&GeometryModel::collisionPairs,
......@@ -71,7 +73,7 @@ namespace pinocchio
;
}
static GeometryData createData(const GeometryModel & geomModel) { return GeometryData(geomModel); }
/* --- Expose --------------------------------------------------------- */
static void expose()
......@@ -81,6 +83,7 @@ namespace pinocchio
bp::no_init)
.def(GeometryModelPythonVisitor())
.def(PrintableVisitor<GeometryModel>())
.def(CopyableVisitor<GeometryModel>())
;
}
......
......@@ -48,7 +48,7 @@ namespace pinocchio
const JointIndex m_parent_id;
const SE3 & m_joint_placement;
const std::string & m_joint_name;
addJointVisitor(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
......@@ -58,13 +58,49 @@ namespace pinocchio
, m_joint_placement(joint_placement)
, m_joint_name(joint_name)
{}
template <typename JointModelDerived>
JointIndex operator()(JointModelDerived & jmodel) const
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name);
}
}; // struct addJointVisitor
struct addJointWithLimitsVisitor : public boost::static_visitor<Model::Index>
{
Model & m_model;
const JointIndex m_parent_id;
const SE3 & m_joint_placement;
const std::string & m_joint_name;
const Eigen::VectorXd & m_max_effort;
const Eigen::VectorXd & m_max_velocity;
const Eigen::VectorXd & m_min_config;
const Eigen::VectorXd & m_max_config;
addJointWithLimitsVisitor(Model & model,
const JointIndex parent_id,
const SE3 & joint_placement,
const std::string & joint_name,
const Eigen::VectorXd & max_effort,
const Eigen::VectorXd & max_velocity,
const Eigen::VectorXd & min_config,
const Eigen::VectorXd & max_config)
: m_model(model)
, m_parent_id(parent_id)
, m_joint_placement(joint_placement)
, m_joint_name(joint_name)
, m_max_effort(max_effort)
, m_max_velocity(max_velocity)
, m_min_config(min_config)
, m_max_config(max_config)
{}
template <typename JointModelDerived>
JointIndex operator()(JointModelDerived & jmodel) const
{
return m_model.addJoint(m_parent_id,jmodel,m_joint_placement,m_joint_name,m_max_effort,m_max_velocity,m_min_config,m_max_config);
}
}; // struct addJointWithLimitsVisitor
public:
......@@ -132,10 +168,11 @@ namespace pinocchio
// Class Methods
.def("addJoint",&ModelPythonVisitor::addJoint,bp::args("parent_id","joint_model","joint_placement","joint_name"),"Adds a joint to the kinematic tree. The joint is defined by its placement relative to its parent joint and its name.")
.def("addJoint",&ModelPythonVisitor::addJointWithLimits,bp::args("parent_id","joint_model","joint_placement","joint_name","max_effort","max_velocity","min_config","max_config"),"Adds a joint to the kinematic tree with given bounds. The joint is defined by its placement relative to its parent joint and its name.")
.def("addJointFrame", &Model::addJointFrame, bp::args("jointIndex", "frameIndex"), "add the joint at index jointIndex as a frame to the frame tree")
.def("appendBodyToJoint",&Model::appendBodyToJoint,bp::args("joint_id","body_inertia","body_placement"),"Appends a body to the joint given by its index. The body is defined by its inertia, its relative placement regarding to the joint and its name.")
.def("addBodyFrame", &Model::addBodyFrame, bp::args("body_name", "parentJoint", "body_plaement", "previous_frame(parent frame)"), "add a body to the frame tree")
.def("addBodyFrame", &Model::addBodyFrame, bp::args("body_name", "parentJoint", "body_placement", "previous_frame(parent frame)"), "add a body to the frame tree")
.def("getBodyId",&Model::getBodyId, bp::args("name"), "Return the index of a frame of type BODY given by its name")
.def("existBodyName", &Model::existBodyName, bp::args("name"), "Check if a frame of type BODY exists, given its name")
.def("getJointId",&Model::getJointId, bp::args("name"), "Return the index of a joint given by its name")
......@@ -159,7 +196,7 @@ namespace pinocchio
;
}
static JointIndex addJoint(Model & model,
JointIndex parent_id,
bp::object jmodel,
......@@ -169,7 +206,21 @@ namespace pinocchio
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointVisitor(model,parent_id,joint_placement,joint_name), jmodel_variant);
}
static JointIndex addJointWithLimits(Model & model,
JointIndex parent_id,
bp::object jmodel,
const SE3 & joint_placement,
const std::string & joint_name,
const Eigen::VectorXd & max_effort,
const Eigen::VectorXd & max_velocity,
const Eigen::VectorXd & min_config,
const Eigen::VectorXd & max_config)
{
JointModelVariant jmodel_variant = bp::extract<JointModelVariant> (jmodel)();
return boost::apply_visitor(addJointWithLimitsVisitor(model,parent_id,joint_placement,joint_name,max_effort,max_velocity,min_config,max_config), jmodel_variant);
}
static Data createData(const Model & model) { return Data(model); }
///
......
......@@ -95,9 +95,8 @@ namespace pinocchio
const GeometryType type
)
{
std::vector<std::string> hints;
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,hints);
pinocchio::urdf::buildGeom(model,filename,type,geometry_model);
return geometry_model;
}
......@@ -126,6 +125,19 @@ namespace pinocchio
return buildGeomFromUrdf(model,filename,package_dirs_,type);
}
static GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const std::string & package_dir,
const GeometryType type
)
{
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dir);
return geometry_model;
}
#ifdef PINOCCHIO_WITH_HPP_FCL
static GeometryModel
buildGeomFromUrdf(const Model & model,
......@@ -166,7 +178,21 @@ namespace pinocchio
std::vector<std::string> package_dirs_ = extractList<std::string>(package_dirs);
return buildGeomFromUrdf(model,filename,package_dirs_,type,meshLoader);
}
static GeometryModel
buildGeomFromUrdf(const Model & model,
const std::string & filename,
const std::string & package_dir,
const GeometryType type,
const fcl::MeshLoaderPtr& meshLoader
)
{
GeometryModel geometry_model;
pinocchio::urdf::buildGeom(model,filename,type,geometry_model,package_dir,meshLoader);
return geometry_model;
}
BOOST_PYTHON_FUNCTION_OVERLOADS(removeCollisionPairs_overload,
srdf::removeCollisionPairs,
3,4)
......@@ -193,7 +219,7 @@ namespace pinocchio
static void loadReferenceConfigurationsFromXML (Model& model,
const std::string & xmlStream,
const bool verbose = false) throw (std::invalid_argument)
const bool verbose = false)
{
std::istringstream iss (xmlStream);
pinocchio::srdf::loadReferenceConfigurationsFromXML(model, iss, verbose);
......@@ -253,34 +279,44 @@ namespace pinocchio
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const std::vector<std::string> &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","URDF filename (string)", "package_dirs (vector of strings)", "Geometry type (COLLISION or VISUAL)"),
bp::args("Model to associate the Geometry","URDF filename (string)", "package_dirs (vector of strings)", "Geometry type (COLLISION or VISUAL)"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const bp::list &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","URDF filename (string)", "package_dirs (list of strings)", "Geometry type (COLLISION or VISUAL)"),
bp::args("Model to associate the Geometry","URDF filename (string)", "package_dirs (list of strings)", "Geometry type (COLLISION or VISUAL)"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","URDF filename (string)","Geometry type (COLLISION or VISUAL)"),
bp::args("Model to associate the Geometry","URDF filename (string)","Geometry type (COLLISION or VISUAL)"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const std::string &, const GeometryType)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to associate the Geometry","URDF filename (string)", "package_dir (string)","Geometry type (COLLISION or VISUAL)"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
#ifdef PINOCCHIO_WITH_HPP_FCL
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const std::vector<std::string> &, const GeometryType, const fcl::MeshLoaderPtr&)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","URDF filename (string)", "package_dirs (vector of strings)","Geometry type (COLLISION or VISUAL)", "Mesh loader"),
bp::args("Model to associate the Geometry","URDF filename (string)", "package_dirs (vector of strings)","Geometry type (COLLISION or VISUAL)", "Mesh loader"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const bp::list &, const GeometryType, const fcl::MeshLoaderPtr&)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","URDF filename (string)", "package_dirs (list of strings)","Geometry type (COLLISION or VISUAL)", "Mesh loader"),
bp::args("Model to associate the Geometry","URDF filename (string)", "package_dirs (list of strings)","Geometry type (COLLISION or VISUAL)", "Mesh loader"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const std::string &, const GeometryType, const fcl::MeshLoaderPtr&)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to associate the Geometry","URDF filename (string)", "package_dir (string)","Geometry type (COLLISION or VISUAL)", "Mesh loader"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
bp::def("buildGeomFromUrdf",
static_cast <GeometryModel (*) (const Model &, const std::string &, const GeometryType, const fcl::MeshLoaderPtr&)> (&ParsersPythonVisitor::buildGeomFromUrdf),
bp::args("Model to assosiate the Geometry","URDF filename (string)","Geometry type (COLLISION or VISUAL)", "Mesh loader"),
bp::args("Model to associate the Geometry","URDF filename (string)","Geometry type (COLLISION or VISUAL)", "Mesh loader"),
"Parse the URDF file given in input looking for the geometry of the given Model and return a proper pinocchio geometry model ");
bp::def("removeCollisionPairs",
......
......@@ -24,7 +24,7 @@ namespace pinocchio
/// \returns The model constructed by the Python script.
///
// TODO: look inside the context of Python and find an occurence of object Model
Model buildModel(const std::string & filename, const std::string & var_name = "model", bool verbose = false) throw (boost::python::error_already_set);
Model buildModel(const std::string & filename, const std::string & var_name = "model", bool verbose = false);
} // namespace python
......
......@@ -21,7 +21,7 @@ namespace pinocchio
{
namespace bp = boost::python;
Model buildModel(const std::string & filename, const std::string & model_name, bool verbose) throw (bp::error_already_set)
Model buildModel(const std::string & filename, const std::string & model_name, bool verbose)
{
Py_Initialize();
......
......@@ -4,10 +4,11 @@
import numpy as np
from .robot_wrapper import RobotWrapper
from libpinocchio_pywrap import __version__
from .libpinocchio_pywrap import __version__
from . import libpinocchio_pywrap as pin
from . import utils
from . import visualize
from .explog import exp, log
from .libpinocchio_pywrap import *
from .deprecated import *
......
......@@ -11,6 +11,17 @@ import warnings as _warnings
from . import libpinocchio_pywrap as pin
from .deprecation import deprecated, DeprecatedWarning
# deprecated StdVect_ (since: 19/04/2019)
StdVect_Frame = deprecated("Please use StdVec_Frame")(pin.StdVec_Frame)
StdVect_GeometryObject = deprecated("Please use StdVec_GeometryObject")(pin.StdVec_GeometryObject)
if pin.WITH_FCL_SUPPORT():
StdVect_Contact = deprecated("Please use StdVec_Contact")(pin.StdVec_Contact)
StdVect_CollisionResult = deprecated("Please use StdVec_CollisionResult")(pin.StdVec_CollisionResult)
StdVect_DistanceResult = deprecated("Please use StdVec_DistanceResult")(pin.StdVec_DistanceResult)
StdVect_SE3 = deprecated("Please use StdVec_SE3")(pin.StdVec_SE3)
StdVect_Force = deprecated("Please use StdVec_Force")(pin.StdVec_Force)
StdVect_Motion = deprecated("Please use StdVec_Motion")(pin.StdVec_Motion)
@deprecated("This function has been renamed to impulseDynamics for consistency with the C++ interface. Please change for impulseDynamics.")
def impactDynamics(model, data, q, v_before, J, r_coeff=0.0, update_kinematics=True):
return pin.impulseDynamics(model, data, q, v_before, J, r_coeff, update_kinematics)
......
This diff is collapsed.
......@@ -7,3 +7,47 @@
from . import libpinocchio_pywrap as pin
nle = pin.nonLinearEffects
def buildModelsFromUrdf(filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None, geometry_types=[pin.GeometryType.COLLISION,pin.GeometryType.VISUAL]):
"""Parse the URDF file given in input and return a Pinocchio Model followed by corresponding GeometryModels of types specified by geometry_types, in the same order as listed.
Examples of usage:
# load model, collision model, and visual model, in this order (default)
model, collision_model, visual_model = buildModelsFromUrdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION,pin.GeometryType.VISUAL])
model, collision_model, visual_model = buildModelsFromUrdf(filename[, ...]) # same as above
model, collision_model = buildModelsFromUrdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION]) # only load the model and the collision model
model, collision_model = buildModelsFromUrdf(filename[, ...], geometry_types=pin.GeometryType.COLLISION) # same as above
model, visual_model = buildModelsFromUrdf(filename[, ...], geometry_types=pin.GeometryType.VISUAL) # only load the model and the visual model
model = buildModelsFromUrdf(filename[, ...], geometry_types=[]) # equivalent to buildModelFromUrdf(filename[, root_joint])
"""
if root_joint is None:
model = pin.buildModelFromUrdf(filename)
else:
model = pin.buildModelFromUrdf(filename, root_joint)
if verbose and not pin.WITH_FCL_SUPPORT() and meshLoader is not None:
print('Info: Pinocchio was compiled without hpp-fcl. meshLoader is ignored.')
if package_dirs is None:
package_dirs = []
lst = [model]
if not hasattr(geometry_types, '__iter__'):
geometry_types = [geometry_types]
for geometry_type in geometry_types:
if meshLoader is None or not pin.WITH_FCL_SUPPORT():
geom_model = pin.buildGeomFromUrdf(model, filename, package_dirs, geometry_type)
else:
geom_model = pin.buildGeomFromUrdf(model, filename, package_dirs, geometry_type, meshLoader)
lst.append(geom_model)
return tuple(lst)
def createDatas(*models):
"""Call createData() on each Model or GeometryModel in input and return the results in a tuple.
If one of the models is None, the corresponding data object in the result is also None.
"""
return tuple([None if model is None else model.createData() for model in models])
from .base_visualizer import BaseVisualizer
from .gepetto_visualizer import GepettoVisualizer
from .meshcat_visualizer import MeshcatVisualizer
from .. import libpinocchio_pywrap as pin
from ..shortcuts import buildModelsFromUrdf, createDatas
import time
class BaseVisualizer(object):
"""Pinocchio visualizers are employed to easily display a model at a given configuration.
BaseVisualizer is not meant to be directly employed, but only to provide a uniform interface and a few common methods.
New visualizers should extend this class and override its methods as neeeded.
"""
def __init__(self, model = pin.Model(), collision_model = None, visual_model = None, copy_models=False):
"""Construct a display from the given model, collision model, and visual model.
If copy_models is True, the models are copied. Otherwise, they are simply kept as a reference."""
if copy_models:
self.model = model.copy()
self.collision_model = collision_model.copy()
self.visual_model = visual_model.copy()
else:
self.model = model
self.collision_model = collision_model
self.visual_model = visual_model
self.data, self.collision_data, self.visual_data = createDatas(model,collision_model,visual_model)
def getViewerNodeName(self, geometry_object, geometry_type):
"""Return the name of the geometry object inside the viewer."""
pass
def initViewer(self, *args, **kwargs):
"""Init the viewer by loading the gui and creating a window."""
pass
def loadViewerModel(self, *args, **kwargs):
"""Create the scene displaying the robot meshes in gepetto-viewer"""
pass
def display(self, q):
"""Display the robot at configuration q in the viewer by placing all the bodies."""
pass
def displayCollisions(self,visibility):
"""Set whether to display collision objects or not."""
pass
def displayVisuals(self,visibility):
"""Set whether to display visual objects or not."""
pass
def play(self, q_trajectory, dt):
"""Play a trajectory with given time step."""
for k in range(q_trajectory.shape[1]):
t0 = time.time()
self.display(q_trajectory[:, k])
t1 = time.time()
elapsed_time = t1 - t0
if elapsed_time < dt:
time.sleep(dt - elapsed_time)
__all__ = ['BaseVisualizer']
from .. import libpinocchio_pywrap as pin
from ..shortcuts import buildModelsFromUrdf, createDatas
from . import BaseVisualizer
class GepettoVisualizer(BaseVisualizer):
"""A Pinocchio display using Gepetto Viewer"""
def getViewerNodeName(self, geometry_object, geometry_type):
"""Return the name of the geometry object inside the viewer"""
if geometry_type is pin.GeometryType.VISUAL:
return self.viewerVisualGroupName + '/' + geometry_object.name
elif geometry_type is pin.GeometryType.COLLISION:
return self.viewerCollisionGroupName + '/' + geometry_object.name
def initViewer(self, viewer=None, windowName="python-pinocchio", sceneName="world", loadModel=False):
"""Init GepettoViewer by loading the gui and creating a window."""
import gepetto.corbaserver
try:
self.viewer = gepetto.corbaserver.Client() if viewer is None else viewer
gui = self.viewer.gui
# Create window
window_l = gui.getWindowList()
if not windowName in window_l:
self.windowID = self.viewer.gui.createWindow(windowName)
else:
self.windowID = self.viewer.gui.getWindowID(windowName)
# Create scene if needed
scene_l = gui.getSceneList()
if sceneName not in scene_l:
gui.createScene(sceneName)
self.sceneName = sceneName
gui.addSceneToWindow(sceneName, self.windowID)
if loadModel:
self.loadViewerModel()
except:
import warnings
msg = ("Error while starting the viewer client.\n"
"Check whether gepetto-viewer is properly started"
)
warnings.warn(msg, category=UserWarning, stacklevel=2)
def loadViewerGeometryObject(self, geometry_object, geometry_type):
"""Load a single geometry object"""
from ..rpy import npToTuple
gui = self.viewer.gui
meshName = self.getViewerNodeName(geometry_object,geometry_type)
meshPath = geometry_object.meshPath
meshTexturePath = geometry_object.meshTexturePath
meshScale = geometry_object.meshScale
meshColor = geometry_object.meshColor
if gui.addMesh(meshName, meshPath):
gui.setScale(meshName, npToTuple(meshScale))
if geometry_object.overrideMaterial:
gui.setColor(meshName, npToTuple(meshColor))
if meshTexturePath is not '':
gui.setTexture(meshName, meshTexturePath)
def loadViewerModel(self, rootNodeName="pinocchio"):
"""Create the scene displaying the robot meshes in gepetto-viewer"""
# Start a new "scene" in this window, named "world", with just a floor.
gui = self.viewer.gui
self.viewerRootNodeName = self.sceneName + "/" + rootNodeName
if not gui.nodeExists(self.viewerRootNodeName):
gui.createGroup(self.viewerRootNodeName)
self.viewerCollisionGroupName = self.viewerRootNodeName + "/" + "collisions"
if not gui.nodeExists(self.viewerCollisionGroupName):
gui.createGroup(self.viewerCollisionGroupName)
self.viewerVisualGroupName = self.viewerRootNodeName + "/" + "visuals"
if not gui.nodeExists(self.viewerVisualGroupName):
gui.createGroup(self.viewerVisualGroupName)
# iterate over visuals and create the meshes in the viewer
for collision in self.collision_model.geometryObjects:
self.loadViewerGeometryObject(collision,pin.GeometryType.COLLISION)
self.displayCollisions(False)
for visual in self.visual_model.geometryObjects:
self.loadViewerGeometryObject(visual,pin.GeometryType.VISUAL)
self.displayVisuals(True)
# Finally, refresh the layout to obtain your first rendering.
gui.refresh()
def display(self, q):
"""Display the robot at configuration q in the viewer by placing all the bodies."""
if 'viewer' not in self.__dict__:
return
gui = self.viewer.gui
# Update the robot kinematics and geometry.
pin.forwardKinematics(self.model,self.data,q)
if self.display_collisions:
pin.updateGeometryPlacements(self.model, self.data, self.collision_model, self.collision_data)
gui.applyConfigurations (
[ self.getViewerNodeName(collision,pin.GeometryType.COLLISION) for collision in self.collision_model.geometryObjects ],
[ pin.se3ToXYZQUATtuple(self.collision_data.oMg[self.collision_model.getGeometryId(collision.name)]) for collision in self.collision_model.geometryObjects ]
)
if self.display_visuals:
pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data)
gui.applyConfigurations (
[ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ],
[ pin.se3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ]
)
gui.refresh()
def displayCollisions(self,visibility):
"""Set whether to display collision objects or not"""
gui = self.viewer.gui
self.display_collisions = visibility
if visibility:
visibility_mode = "ON"
else:
visibility_mode = "OFF"
for collision in self.collision_model.geometryObjects:
nodeName = self.getViewerNodeName(collision,pin.GeometryType.COLLISION)
gui.setVisibility(nodeName,visibility_mode)
def displayVisuals(self,visibility):
"""Set whether to display visual objects or not"""
gui = self.viewer.gui
self.display_visuals = visibility
if visibility:
visibility_mode = "ON"
else:
visibility_mode = "OFF"
for visual in self.visual_model.geometryObjects:
nodeName = self.getViewerNodeName(visual,pin.GeometryType.VISUAL)
gui.setVisibility(nodeName,visibility_mode)
__all__ = ['GepettoVisualizer']
from .. import libpinocchio_pywrap as pin
from ..shortcuts import buildModelsFromUrdf, createDatas
from . import BaseVisualizer
import os
import numpy as np
class MeshcatVisualizer(BaseVisualizer):
"""A Pinocchio display using Meshcat"""
def getViewerNodeName(self, geometry_object, geometry_type):
"""Return the name of the geometry object inside the viewer."""
if geometry_type is pin.GeometryType.VISUAL:
return self.viewerVisualGroupName + '/' + geometry_object.name
elif geometry_type is pin.GeometryType.COLLISION:
return None # TODO: collision meshes
def initViewer(self, viewer=None, open=False, loadModel=False):
"""Start a new MeshCat server and client.
Note: the server can also be started separately using the "meshcat-server" command in a terminal:
this enables the server to remain active after the current script ends.
"""
import meshcat
self.viewer = meshcat.Visualizer() if viewer is None else viewer
if open:
self.viewer.open()
if loadModel:
self.loadViewerModel()
def loadViewerGeometryObject(self, geometry_object,geometry_type, color=None):
"""Load a single geometry object"""
import meshcat.geometry
viewer_name = self.getViewerNodeName(geometry_object, geometry_type)
if geometry_object.meshPath == "":
raise IOError("{} mesh file not found for link {}.".format(str(geometry_type).lower(),geometry_object.name))
# Get file type from filename extension.
_, file_extension = os.path.splitext(geometry_object.meshPath)
if file_extension.lower() == ".dae":
obj = meshcat.geometry.DaeMeshGeometry.from_file(geometry_object.meshPath)
elif file_extension.lower() == ".obj":
obj = meshcat.geometry.ObjMeshGeometry.from_file(geometry_object.meshPath)
elif file_extension.lower() == ".stl":
obj = meshcat.geometry.StlMeshGeometry.from_file(geometry_object.meshPath)
else:
raise ImportError("Unknown mesh file format: {}.".format(geometry_object.meshPath))
material = meshcat.geometry.MeshPhongMaterial()
# Set material color from URDF, converting for triplet of doubles to a single int.
if color is None:
meshColor = geometry_object.meshColor
else:
meshColor = color
material.color = int(meshColor[0] * 255) * 256**2 + int(meshColor[1] * 255) * 256 + int(meshColor[2] * 255)
# Add transparency, if needed.
if float(meshColor[3]) != 1.0:
material.transparent = True
material.opacity = float(meshColor[3])
self.viewer[viewer_name].set_object(obj, material)
def loadViewerModel(self, rootNodeName="pinocchio", color = None):
"""Load the robot in a MeshCat viewer.
Parameters:
rootNodeName: name to give to the robot in the viewer
color: optional, color to give to the robot. This overwrites the color present in the urdf.
Format is a list of four RGBA floats (between 0 and 1)
"""
# Set viewer to use to gepetto-gui.
self.viewerRootNodeName = rootNodeName
# Load robot meshes in MeshCat
# Collisions
# self.viewerCollisionGroupName = self.viewerRootNodeName + "/" + "collisions"
self.viewerCollisionGroupName = None # TODO: collision meshes
# Visuals
self.viewerVisualGroupName = self.viewerRootNodeName + "/" + "visuals"
for visual in self.visual_model.geometryObjects:
self.loadViewerGeometryObject(visual,pin.GeometryType.VISUAL,color)
def display(self, q):
"""Display the robot at configuration q in the viewer by placing all the bodies."""
pin.forwardKinematics(self.model,self.data,q)
pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data)
for visual in self.visual_model.geometryObjects:
# Get mesh pose.
M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]
# Manage scaling
S = np.diag(np.concatenate((visual.meshScale,np.array([[1.0]]))).flat)
T = np.array(M.homogeneous).dot(S)
# Update viewer configuration.
self.viewer[self.getViewerNodeName(visual,pin.GeometryType.VISUAL)].set_transform(T)
def displayCollisions(self,visibility):
"""Set whether to display collision objects or not.
WARNING: Plotting collision meshes is not yet available for MeshcatVisualizer."""
# TODO
import warnings
warnings.warn("Plotting collision meshes is not available for MeshcatVisualizer", category=UserWarning, stacklevel=2)
pass
def displayVisuals(self,visibility):
"""Set whether to display visual objects or not
WARNING: Visual meshes are always plotted for MeshcatVisualizer"""
# TODO
import warnings
warnings.warn("Visual meshes are always plotted for MeshcatVisualizer", category=UserWarning, stacklevel=2)
pass
__all__ = ['MeshcatVisualizer']
......@@ -14,7 +14,7 @@ namespace pinocchio
void exposeSE3()
{
SE3PythonVisitor<SE3>::expose();
StdAlignedVectorPythonVisitor<SE3>::expose("StdVect_SE3");
StdAlignedVectorPythonVisitor<SE3>::expose("StdVec_SE3");
}
} // namespace python
......
......@@ -14,7 +14,7 @@ namespace pinocchio
void exposeForce()
{
ForcePythonVisitor<Force>::expose();
StdAlignedVectorPythonVisitor<Force>::expose("StdVect_Force");
StdAlignedVectorPythonVisitor<Force>::expose("StdVec_Force");
}
} // namespace python
......
......@@ -14,7 +14,7 @@ namespace pinocchio
void exposeMotion()
{
MotionPythonVisitor<Motion>::expose();
StdAlignedVectorPythonVisitor<Motion>::expose("StdVect_Motion");
StdAlignedVectorPythonVisitor<Motion>::expose("StdVec_Motion");
}
} // namespace python
......
......@@ -91,6 +91,19 @@ namespace pinocchio
.def("Random",&Inertia::Random,"Returns a random Inertia.")
.staticmethod("Random")
.def("toDynamicParameters",&InertiaPythonVisitor::toDynamicParameters_proxy,
"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)"),
"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"
)
.staticmethod("FromDynamicParameters")
.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}.")
......@@ -128,6 +141,8 @@ namespace pinocchio
symmetric_inertia(2,2);
}
static Eigen::VectorXd toDynamicParameters_proxy( const Inertia & self ) { return self.toDynamicParameters(); }
static Inertia* makeFromMCI(const double & mass,
const Vector3 & lever,
const Matrix3 & inertia)
......
......@@ -34,19 +34,22 @@ namespace pinocchio
typedef typename SE3::Scalar Scalar;
typedef typename SE3::Matrix3 Matrix3;
typedef typename SE3::Vector3 Vector3;
typedef typename SE3::Matrix4 Matrix4;
public:
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<>("Default constructor."))
.def(bp::init<Matrix3,Vector3>
((bp::arg("Rotation"),bp::arg("translation")),
"Initialize from rotation and translation."))
.def(bp::init<int>((bp::arg("trivial arg (should be 1)")),"Init to identity."))
.def(bp::init<SE3>((bp::arg("other")), "Copy constructor."))
.def(bp::init<Matrix4>
((bp::arg("Homogeneous matrix")),
"Initialize from a homogeneous matrix."))
.add_property("rotation",
&getRotation,
......
......@@ -31,40 +31,44 @@ namespace pinocchio
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def("__getitem__", &PyWraperForAlignedStdVector::getItem)
.def("__setitem__", &PyWraperForAlignedStdVector::setItem)
.def("__len__", &PyWraperForAlignedStdVector::length)