Unverified Commit 3421e34e authored by Rohan Budhiraja's avatar Rohan Budhiraja Committed by GitHub
Browse files

Merge pull request #1478 from proyan/pinocchio3-preview

Add signatures for loading geometry models, add support in robot_wrapper, and fix issues in sdf parsing
parents 46abd97c 404f8aba
Pipeline #15447 failed with stage
in 300 minutes and 12 seconds
This diff is collapsed.
......@@ -29,9 +29,8 @@ namespace pinocchio
}
bp::tuple buildModelFromSdf(const std::string & filename,
const bp::object & root_joint_object)
const JointModel & root_joint)
{
JointModelVariant root_joint = bp::extract<JointModelVariant>(root_joint_object)();
Model model;
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
pinocchio::sdf::buildModel(filename, root_joint, model, contact_models);
......@@ -48,13 +47,13 @@ namespace pinocchio
bp::def("buildModelFromSdf",
static_cast <bp::tuple (*) (const std::string &)> (pinocchio::python::buildModelFromSdf),
bp::args("sdf_filename"),
"Parse the SDF file given in input and return a pinocchio Model."
"Parse the SDF file given in input and return a pinocchio Model and constraint models."
);
bp::def("buildModelFromSdf",
static_cast <bp::tuple (*) (const std::string &, const bp::object &)> (pinocchio::python::buildModelFromSdf),
static_cast <bp::tuple (*) (const std::string &, const JointModel &)> (pinocchio::python::buildModelFromSdf),
bp::args("sdf_filename","root_joint"),
"Append to a given model a SDF structure given by its filename and the root joint."
"Parse the SDF file given in input and return a pinocchio Model and constraint models starting with the given root joint."
);
#endif // #ifdef PINOCCHIO_WITH_HPP_FCL
#endif
......
......@@ -5,7 +5,7 @@
from . import pinocchio_pywrap_default as pin
from . import utils
from .deprecation import deprecated
from .shortcuts import buildModelsFromUrdf, createDatas
from .shortcuts import buildModelsFromUrdf, createDatas, buildModelsFromSdf
import numpy as np
......@@ -21,6 +21,17 @@ class RobotWrapper(object):
model, collision_model, visual_model = buildModelsFromUrdf(filename, package_dirs, root_joint, verbose, meshLoader)
RobotWrapper.__init__(self,model=model,collision_model=collision_model,visual_model=visual_model)
@staticmethod
def BuildFromSDF(filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None):
robot = RobotWrapper()
robot.initFromSDF(filename, package_dirs, root_joint, verbose, meshLoader)
return robot
def initFromSDF(self,filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None):
model, constraint_models, collision_model, visual_model = buildModelsFromSdf(filename, package_dirs, root_joint, verbose, meshLoader)
RobotWrapper.__init__(self,model=model,collision_model=collision_model,visual_model=visual_model)
self.constraint_models = constraint_models
def __init__(self, model = pin.Model(), collision_model = None, visual_model = None, verbose=False):
self.model = model
......
......@@ -9,7 +9,7 @@ from . import WITH_HPP_FCL, WITH_HPP_FCL_BINDINGS
nle = pin.nonLinearEffects
def buildModelsFromUrdf(filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None, geometry_types=[pin.GeometryType.COLLISION,pin.GeometryType.VISUAL]):
def buildModelsFromUrdf(filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None, geometry_types=None):
"""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)
......@@ -22,7 +22,8 @@ def buildModelsFromUrdf(filename, package_dirs=None, root_joint=None, verbose=Fa
model = buildModelsFromUrdf(filename[, ...], geometry_types=[]) # equivalent to buildModelFromUrdf(filename[, root_joint])
"""
if geometry_types is None:
geometry_types = [pin.GeometryType.COLLISION,pin.GeometryType.VISUAL]
if root_joint is None:
model = pin.buildModelFromUrdf(filename)
else:
......@@ -54,3 +55,45 @@ def createDatas(*models):
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])
def buildModelsFromSdf(filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None, geometry_types=None):
"""Parse the SDF file given in input and return a Pinocchio Model and a list of Constraint Models, followed by corresponding GeometryModels of types specified by geometry_types, in the same order as listed.
Examples of usage:
# load model, constraint models, collision model, and visual model, in this order (default)
model, constraint_models, collision_model, visual_model = buildModelsFromSdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION,pin.GeometryType.VISUAL])
model, constraint_models, collision_model, visual_model = buildModelsFromSdf(filename[, ...]) # same as above
model, constraint_models, collision_model = buildModelsFromSdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION]) # only load the model, constraint models and the collision model
model, constraint_models, collision_model = buildModelsFromSdf(filename[, ...], geometry_types=pin.GeometryType.COLLISION) # same as above
model, constraint_models, visual_model = buildModelsFromSdf(filename[, ...], geometry_types=pin.GeometryType.VISUAL) # only load the model and the visual model
model, constraint_models = buildModelsFromSdf(filename[, ...], geometry_types=[]) # equivalent to buildModelFromSdf(filename[, root_joint])
"""
if geometry_types is None:
geometry_types = [pin.GeometryType.COLLISION,pin.GeometryType.VISUAL]
if root_joint is None:
model, constraint_models = pin.buildModelFromSdf(filename)
else:
model, constraint_models = pin.buildModelFromSdf(filename, root_joint)
if verbose and not WITH_HPP_FCL and meshLoader is not None:
print('Info: MeshLoader is ignored. Pinocchio has not been compiled with HPP-FCL.')
if verbose and not WITH_HPP_FCL_BINDINGS and meshLoader is not None:
print('Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed.')
if package_dirs is None:
package_dirs = []
lst = [model, constraint_models]
if not hasattr(geometry_types, '__iter__'):
geometry_types = [geometry_types]
for geometry_type in geometry_types:
if meshLoader is None or (not WITH_HPP_FCL and not WITH_HPP_FCL_BINDINGS):
geom_model = pin.buildGeomFromSdf(model, constraint_models, filename, geometry_type, package_dirs)
else:
geom_model = pin.buildGeomFromSdf(model, constraint_models, filename, geometry_type, package_dirs, meshLoader)
lst.append(geom_model)
return tuple(lst)
......@@ -273,7 +273,7 @@ class MeshcatVisualizer(BaseVisualizer):
T = np.array(M.homogeneous).dot(S)
else:
T = M.homogeneous
# Update viewer configuration.
self.viewer[visual_name].set_transform(T)
......
......@@ -20,7 +20,7 @@ namespace pinocchio
///
template<typename VecType>
struct PickleVector : boost::python::pickle_suite
{
{
static boost::python::tuple getinitargs(const VecType&)
{ return boost::python::make_tuple(); }
......
......@@ -291,8 +291,16 @@ namespace pinocchio
data.Ig.inertia() = Ytot.inertia();
// Compute the partial derivatives
translateForceSet(data.dHdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike0,dh_dq));
translateForceSet(data.dFdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dHdq,com,dh_dq.const_cast_derived());
Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dh_dq_.col(k).template segment<3>(Force::ANGULAR) += data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(data.dFdq,com,dhdot_dq.const_cast_derived());
Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,data.Ag);
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da) = data.Ag;
......@@ -410,8 +418,16 @@ namespace pinocchio
data.Ig.inertia() = Ytot.inertia();
// Retrieve the partial derivatives from RNEA derivatives
translateForceSet(data.dHdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike0,dh_dq));
translateForceSet(Ftmp,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dHdq,com,dh_dq.const_cast_derived());
Matrix6xLike0 & dh_dq_ = dh_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dh_dq_.col(k).template segment<3>(Force::ANGULAR) += data.hg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(Ftmp,com,dhdot_dq.const_cast_derived());
Matrix6xLike1 & dhdot_dq_ = dhdot_dq.const_cast_derived();
for(Eigen::DenseIndex k = 0; k < model.nv; ++k)
dhdot_dq_.col(k).template segment<3>(Force::ANGULAR) += data.dhg.linear().cross(data.dFda.col(k).template segment<3>(Force::LINEAR))/Ytot.mass();
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,data.Ag);
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da) = data.Ag;
......
......@@ -18,13 +18,13 @@ namespace pinocchio
#ifdef PINOCCHIO_WITH_HPP_FCL
#ifdef PINOCCHIO_WITH_SDF
/**
* @brief Build The GeometryModel from a URDF file. Search for meshes
* @brief Build The GeometryModel from a SDF file. Search for meshes
* in the directories specified by the user first and then in
* the environment variable ROS_PACKAGE_PATH
*
* @param[in] model The model of the robot, built with
* urdf::buildModel
* @param[in] filename The URDF complete (absolute) file path
* sdf::buildModel
* @param[in] filename The SDF complete (absolute) file path
* @param[in] packageDirs A vector containing the different directories
* where to search for models and meshes, typically
* obtained from calling pinocchio::rosPaths()
......@@ -39,20 +39,55 @@ namespace pinocchio
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
GeometryModel & buildGeom(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel & buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models,
const std::string & filename,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & packageDirs = std::vector<std::string> (),
::hpp::fcl::MeshLoaderPtr meshLoader = ::hpp::fcl::MeshLoaderPtr());
/**
* @brief Build The GeometryModel from a SDF file. Search for meshes
* in the directories specified by the user first and then in
* the environment variable ROS_PACKAGE_PATH
*
* @param[in] model The model of the robot, built with
* sdf::buildModel
* @param[in] filename The SDF complete (absolute) file path
* @param[in] package_path A string containing the path to the directories of the meshes,
* typically obtained from calling pinocchio::rosPaths().
*
* @param[in] type The type of objects that must be loaded (must be VISUAL or COLLISION)
* @param[in] meshLoader object used to load meshes: hpp::fcl::MeshLoader [default] or hpp::fcl::CachedMeshLoader.
* @param[out] geomModel Reference where to put the parsed information.
*
* @return Returns the reference on geom model for convenience.
*
* \warning If hpp-fcl has not been found during compilation, COLLISION objects can not be loaded
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
GeometryModel & buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models,
const std::string & filename,
const GeometryType type,
GeometryModel & geomModel,
const std::string & packagePath,
::hpp::fcl::MeshLoaderPtr meshLoader = ::hpp::fcl::MeshLoaderPtr())
{
const std::vector<std::string> dirs(1,packagePath);
return buildGeom(model,contact_models,filename,type,geomModel,dirs,meshLoader);
};
///
/// \brief Build the model from a URDF file with a particular joint as root of the model tree inside
/// \brief Build the model from a SDF file with a particular joint as root of the model tree inside
/// the model given as reference argument.
///
/// \param[in] filename The URDF complete file path.
/// \param[in] filename The SDF complete file path.
/// \param[in] rootJoint The joint at the root of the model tree.
/// \param[in] verbose Print parsing info.
/// \param[out] model Reference model where to put the parsed information.
......@@ -68,9 +103,9 @@ namespace pinocchio
///
/// \brief Build the model from a URDF file with a fixed joint as root of the model tree.
/// \brief Build the model from a SDF file with a fixed joint as root of the model tree.
///
/// \param[in] filename The URDF complete file path.
/// \param[in] filename The SDF complete file path.
/// \param[in] verbose Print parsing info.
/// \param[out] model Reference model where to put the parsed information.
/// \return Return the reference on argument model for convenience.
......
......@@ -65,10 +65,12 @@ namespace pinocchio
std::vector< ::sdf::ElementPtr> geometry_array;
::sdf::ElementPtr geomElement = link->GetElement("collision");
while (geomElement)
{
//Inserting data in std::map
geometry_array.push_back(geomElement);
geomElement = link->GetNextElement("collision");
{
//Inserting data in std::map
if (geomElement->Get<std::string>("name") != "__default__") {
geometry_array.push_back(geomElement);
}
geomElement = link->GetNextElement("collision");
}
return geometry_array;
}
......@@ -82,7 +84,9 @@ namespace pinocchio
while (geomElement)
{
//Inserting data in std::map
geometry_array.push_back(geomElement);
if (geomElement->Get<std::string>("name") != "__default__") {
geometry_array.push_back(geomElement);
}
geomElement = link->GetNextElement("visual");
}
return geometry_array;
......@@ -326,7 +330,7 @@ namespace pinocchio
} // namespace details
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
GeometryModel& buildGeom(ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel& buildGeom(const ModelTpl<Scalar,Options,JointCollectionTpl> & const_model,
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)& contact_models,
const std::string & filename,
const GeometryType type,
......@@ -335,6 +339,7 @@ namespace pinocchio
::hpp::fcl::MeshLoaderPtr meshLoader)
{
Model& model = const_cast<Model &>(const_model); //TODO: buildGeom should not need to parse model again.
::pinocchio::urdf::details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
::pinocchio::sdf::details::SdfGraph graph (visitor, contact_models);
//if (verbose) visitor.log = &std::cout;
......
......@@ -63,11 +63,11 @@ namespace pinocchio
*
*/
PINOCCHIO_DLLAPI void parseTreeForGeom(UrdfGeomVisitorBase& visitor,
const std::istream& xmlStream,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs,
::hpp::fcl::MeshLoaderPtr meshLoader);
const std::istream& xmlStream,
const GeometryType type,
GeometryModel & geomModel,
const std::vector<std::string> & package_dirs,
::hpp::fcl::MeshLoaderPtr meshLoader);
} // namespace details
......
......@@ -108,8 +108,7 @@ BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
const pinocchio::Force dhg = pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q,v,a);
const pinocchio::Force hg = data_fd.hg;
BOOST_CHECK(data.hg.isApprox(data_ref.hg));
const pinocchio::Force::Vector3 com = data_fd.com[0];
// Check dhdot_dq and dh_dq with finite differences
Eigen::VectorXd q_plus(model.nq,1);
Eigen::VectorXd v_eps(model.nv,1); v_eps.setZero();
......@@ -124,13 +123,9 @@ BOOST_AUTO_TEST_CASE(test_centroidal_derivatives)
const pinocchio::Force & dhg_plus
= pinocchio::computeCentroidalMomentumTimeVariation(model,data_fd,q_plus,v,a);
const pinocchio::Force hg_plus = data_fd.hg;
const pinocchio::Force::Vector3 com_plus = data_fd.com[0];
pinocchio::SE3 transform(pinocchio::SE3::Identity());
transform.translation() = com_plus - com;
dhdot_dq_fd.col(k) = (transform.act(dhg_plus) - dhg).toVector()/eps;
dh_dq_fd.col(k) = (transform.act(hg_plus) - hg).toVector()/eps;
dhdot_dq_fd.col(k) = (dhg_plus - dhg).toVector()/eps;
dh_dq_fd.col(k) = (hg_plus - hg).toVector()/eps;
v_eps[k] = 0.;
}
......
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