Commit 5527909d authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub

Merge pull request #1001 from jcarpent/devel

Improve algos on Models
parents 73c309ed e7dd710a
......@@ -3,8 +3,8 @@
//
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/bindings/python/utils/list.hpp"
#include "pinocchio/algorithm/model.hpp"
#include "pinocchio/algorithm/aba.hpp"
namespace pinocchio
{
......@@ -19,14 +19,7 @@ namespace pinocchio
const bp::list & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
{
std::vector<JointIndex> list_of_joints_to_lock_vec;
list_of_joints_to_lock_vec.reserve((size_t)bp::len(list_of_joints_to_lock));
for(int k = 0; k < bp::len(list_of_joints_to_lock); ++k)
{
JointIndex joint_id = bp::extract<JointIndex>(list_of_joints_to_lock[k]);
list_of_joints_to_lock_vec.push_back(joint_id);
}
std::vector<JointIndex> list_of_joints_to_lock_vec = extract<JointIndex>(list_of_joints_to_lock);
return buildReducedModel(model,list_of_joints_to_lock_vec,reference_configuration);
}
......@@ -42,7 +35,7 @@ namespace pinocchio
"Build a reduce model from a given input model and a list of joint to lock.");
bp::def("buildReducedModel",
&buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
(Model (*)(const Model &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))&buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model: input kinematic model",
"list_of_joints_to_lock: list of joint indexes to lock",
"reference_configuration: reference configuration to compute the placement of the lock joints"),
......
......@@ -121,7 +121,7 @@ namespace pinocchio
const GeometryType type
)
{
std::vector<std::string> package_dirs_ = extractList<std::string>(package_dirs);
std::vector<std::string> package_dirs_ = extract<std::string>(package_dirs);
return buildGeomFromUrdf(model,filename,package_dirs_,type);
}
......@@ -175,7 +175,7 @@ namespace pinocchio
const fcl::MeshLoaderPtr& meshLoader
)
{
std::vector<std::string> package_dirs_ = extractList<std::string>(package_dirs);
std::vector<std::string> package_dirs_ = extract<std::string>(package_dirs);
return buildGeomFromUrdf(model,filename,package_dirs_,type,meshLoader);
}
......
......@@ -7,27 +7,57 @@
#include "pinocchio/bindings/python/fwd.hpp"
#include <eigenpy/exception.hpp>
#include <boost/python.hpp>
#include <boost/python/type_id.hpp>
#include <vector>
#include <sstream>
#include <string>
namespace pinocchio
{
namespace python
{
namespace bp = boost::python;
template<typename T>
std::vector<T> extractList(const bp::list & input_list)
template<typename T, class Allocator>
void extract(const boost::python::list & list, std::vector<T,Allocator> & vec)
{
long size_list = bp::len(input_list);
std::vector<T> res((size_t)size_list);
for(long i = 0; i < size_list; ++i)
namespace bp = boost::python;
size_t size_list = (size_t)bp::len(list);
vec.resize(size_list);
for(size_t i = 0; i < size_list; ++i)
{
res.push_back(bp::extract<T>(input_list[i]));
bp::extract<T> input_T(list[i]);
if(input_T.check())
vec[i] = input_T();
else
{
const std::string classname
= bp::extract<std::string>(list[i].attr("__class__").attr("__name__"));
std::stringstream ss;
ss << "The conversion from " << classname << " to "
<< bp::type_id<T>().name() << " has failed." << std::endl;
throw eigenpy::Exception(ss.str());
}
}
return res;
}
template<typename T>
std::vector<T,std::allocator<T> > extract(const boost::python::list & list)
{
std::vector<T,std::allocator<T> > vec;
extract(list,vec);
return vec;
}
template<typename T, class Allocator>
std::vector<T,Allocator> extract(const boost::python::list & list)
{
std::vector<T,Allocator> vec;
extract(list,vec);
return vec;
}
} // namespace python
......
......@@ -16,8 +16,6 @@ namespace pinocchio
*
* \param[in] modelA the parent model.
* \param[in] modelB the child model.
* \param[in] geomModelA the parent geometry model.
* \param[in] geomModelB the child geometry model.
* \param[in] frameInModelA index of the frame of modelA where to append modelB.
* \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA.
* \param[out] model the resulting model.
......@@ -27,10 +25,36 @@ namespace pinocchio
void
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
FrameIndex frameInModelA,
const FrameIndex frameInModelA,
const SE3Tpl<Scalar,Options> & aMb,
ModelTpl<Scalar,Options,JointCollectionTpl> & model);
/**
* \brief Append a child model into a parent model, after a specific frame.
*
* \param[in] modelA the parent model.
* \param[in] modelB the child model.
* \param[in] frameInModelA index of the frame of modelA where to append modelB.
* \param[in] aMb pose of modelB universe joint (index 0) in frameInModelA.
*
* \return A new model containing the fusion of modelA and modelB.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
ModelTpl<Scalar,Options,JointCollectionTpl>
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
const FrameIndex frameInModelA,
const SE3Tpl<Scalar,Options> & aMb)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model model;
appendModel(modelA,modelB,frameInModelA,aMb,model);
return model;
}
/**
* \copydoc pinocchio::appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl>&, const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB, FrameIndex frameInModelA, const SE3Tpl<Scalar, Options>& aMb, ModelTpl<Scalar,Options,JointCollectionTpl>& model);
*
......@@ -44,7 +68,7 @@ namespace pinocchio
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
const GeometryModel & geomModelA,
const GeometryModel & geomModelB,
FrameIndex frameInModelA,
const FrameIndex frameInModelA,
const SE3Tpl<Scalar,Options> & aMb,
ModelTpl<Scalar,Options,JointCollectionTpl> & model,
GeometryModel & geomModel);
......@@ -55,7 +79,24 @@ namespace pinocchio
*
* \param[in] model the input model to reduce.
* \param[in] list_of_joints list of joints to lock in the input model.
* \param[in] reference_configuration reference configuration
* \param[in] reference_configuration reference configuration.
* \param[out] reduced_model the reduced model.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
void
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
std::vector<JointIndex> list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model);
/**
*
* \brief Build a reduce model from a given input model and a list of joint to lock.
*
* \param[in] model the input model to reduce.
* \param[in] list_of_joints list of joints to lock in the input model.
* \param[in] reference_configuration reference configuration.
*
* \returns A reduce model of the input model.
*
......@@ -63,8 +104,16 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
ModelTpl<Scalar,Options,JointCollectionTpl>
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
std::vector<JointIndex> list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration);
const std::vector<JointIndex> & list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
Model reduced_model;
buildReducedModel(model,list_of_joints_to_lock,reference_configuration,reduced_model);
return reduced_model;
}
} // namespace pinocchio
......
......@@ -169,7 +169,7 @@ namespace pinocchio
void
appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
FrameIndex frameInModelA,
const FrameIndex frameInModelA,
const SE3Tpl<Scalar, Options> & aMb,
ModelTpl<Scalar,Options,JointCollectionTpl> & model)
{
......@@ -185,7 +185,7 @@ namespace pinocchio
const ModelTpl<Scalar,Options,JointCollectionTpl> & modelB,
const GeometryModel& geomModelA,
const GeometryModel& geomModelB,
FrameIndex frameInModelA,
const FrameIndex frameInModelA,
const SE3Tpl<Scalar, Options>& aMb,
ModelTpl<Scalar,Options,JointCollectionTpl>& model,
GeometryModel& geomModel)
......@@ -193,6 +193,9 @@ namespace pinocchio
typedef details::AppendJointOfModelAlgoTpl<Scalar, Options, JointCollectionTpl> AppendJointOfModelAlgo;
typedef typename AppendJointOfModelAlgo::ArgsType ArgsType;
PINOCCHIO_CHECK_INPUT_ARGUMENT(frameInModelA < (FrameIndex) modelA.nframes,
"frameInModelA is an invalid Frame index, greater than the number of frames contained in modelA.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::SE3 SE3;
typedef typename Model::Frame Frame;
......@@ -274,10 +277,11 @@ namespace pinocchio
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
ModelTpl<Scalar,Options,JointCollectionTpl>
void
buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & input_model,
std::vector<JointIndex> list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration)
const Eigen::MatrixBase<ConfigVectorType> & reference_configuration,
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(reference_configuration.size() == input_model.nq,
"The configuration vector is not of right size");
......@@ -290,8 +294,6 @@ namespace pinocchio
typedef typename Model::Frame Frame;
typedef typename Model::SE3 SE3;
Model reduced_model;
// Sort indexes
std::sort(list_of_joints_to_lock.begin(),list_of_joints_to_lock.end());
......@@ -449,11 +451,8 @@ namespace pinocchio
reduced_model.addFrame(reduced_frame);
}
}
return reduced_model;
}
} // namespace pinocchio
#endif // ifndef __pinocchio_algorithm_model_hxx__
......@@ -3,8 +3,8 @@
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_data_hpp__
#define __pinocchio_data_hpp__
#ifndef __pinocchio_multibody_data_hpp__
#define __pinocchio_multibody_data_hpp__
#include "pinocchio/math/tensor.hpp"
......@@ -384,5 +384,5 @@ namespace pinocchio
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/data.hxx"
#endif // ifndef __pinocchio_data_hpp__
#endif // ifndef __pinocchio_multibody_data_hpp__
......@@ -3,8 +3,8 @@
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_data_hxx__
#define __pinocchio_data_hxx__
#ifndef __pinocchio_multibody_data_hxx__
#define __pinocchio_multibody_data_hxx__
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/multibody/model.hpp"
......@@ -317,4 +317,4 @@ namespace pinocchio
/// @endcond
#endif // ifndef __pinocchio_data_hxx__
#endif // ifndef __pinocchio_multibody_data_hxx__
......@@ -3,8 +3,8 @@
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_model_hpp__
#define __pinocchio_model_hpp__
#ifndef __pinocchio_multibody_model_hpp__
#define __pinocchio_multibody_model_hpp__
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3.hpp"
......@@ -546,4 +546,4 @@ namespace pinocchio
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/model.hxx"
#endif // ifndef __pinocchio_model_hpp__
#endif // ifndef __pinocchio_multibody_model_hpp__
......@@ -3,8 +3,8 @@
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
#ifndef __pinocchio_model_hxx__
#define __pinocchio_model_hxx__
#ifndef __pinocchio_multibody_model_hxx__
#define __pinocchio_multibody_model_hxx__
#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/utils/string-generator.hpp"
......@@ -278,4 +278,4 @@ namespace pinocchio
/// @endcond
#endif // ifndef __pinocchio_model_hxx__
#endif // ifndef __pinocchio_multibody_model_hxx__
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