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

Merge pull request #1427 from ntorresalberto/RobotWrapper.reduceModel

add Robot wrapper.reduceModel function and extend buildReducedModel for multiple GeometricModel
parents f3e0aecc ae242e2d
Pipeline #13975 passed with stage
in 125 minutes and 24 seconds
......@@ -4,6 +4,7 @@
#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
#include "pinocchio/bindings/python/utils/list.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
#include "pinocchio/algorithm/model.hpp"
namespace pinocchio
......@@ -45,11 +46,31 @@ namespace pinocchio
return bp::make_tuple(reduced_model,reduced_geom_model);
}
template <typename Scalar, int Options,
template <typename, int> class JointCollectionTpl,
typename ConfigVectorType>
bp::tuple buildReducedModel(const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &list_of_geom_models,
const std::vector<JointIndex> &list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration)
{
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > reduced_geom_models;
Model reduced_model;
buildReducedModel(model, list_of_geom_models, list_of_joints_to_lock,
reference_configuration, reduced_model,
reduced_geom_models);
return bp::make_tuple(reduced_model, reduced_geom_models);
}
void exposeModelAlgo()
{
using namespace Eigen;
typedef std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > GeometryModelVector;
StdVectorPythonVisitor<GeometryModel,GeometryModelVector::allocator_type>::expose("StdVec_GeometryModel");
bp::def("appendModel",
(Model (*)(const Model &, const Model &, const FrameIndex, const SE3 &))&appendModel<double,0,JointCollectionDefaultTpl>,
bp::args("modelA","modelB","frame_in_modelA","aMb"),
......@@ -85,20 +106,40 @@ namespace pinocchio
"\treference_configuration: reference configuration to compute the placement of the lock joints\n");
bp::def("buildReducedModel",
(bp::tuple (*)(const Model &, const GeometryModel &, const std::vector<JointIndex> &, const Eigen::MatrixBase<VectorXd> &))
(bp::tuple (*)(const Model &,
const GeometryModel &,
const std::vector<JointIndex> &,
const Eigen::MatrixBase<VectorXd> &))
&buildReducedModel<double,0,JointCollectionDefaultTpl,VectorXd>,
bp::args("model",
"geom_model",
"list_of_joints_to_lock",
"reference_configuration"),
"Build a reduced model and a rededuced geometry model from a given input model,"
"a given input geometry model and a list of joint to lock.\n\n"
"Build a reduced model and a reduced geometry model from a given input model,"
"an input geometry model and a list of joints to lock.\n\n"
"Parameters:\n"
"\tmodel: input kinematic modell to reduce\n"
"\tmodel: input kinematic model to reduce\n"
"\tgeom_model: input geometry model to reduce\n"
"\tlist_of_joints_to_lock: list of joint indexes to lock\n"
"\treference_configuration: reference configuration to compute the placement of the lock joints\n");
"\treference_configuration: reference configuration to compute the placement of the locked joints\n");
bp::def("buildReducedModel",
(bp::tuple(*)(const Model &,
const std::vector<GeometryModel,Eigen::aligned_allocator<GeometryModel> > &,
const std::vector<JointIndex> &,
const Eigen::MatrixBase<VectorXd> &))
buildReducedModel<double, 0, JointCollectionDefaultTpl, VectorXd>,
bp::args("model", "list_of_geom_models", "list_of_joints_to_lock",
"reference_configuration"),
"Build a reduced model and the related reduced geometry models from a given "
"input model,"
"a list of input geometry models and a list of joints to lock.\n\n"
"Parameters:\n"
"\tmodel: input kinematic model to reduce\n"
"\tlist_of_geom_models: input geometry models to reduce\n"
"\tlist_of_joints_to_lock: list of joint indexes to lock\n"
"\treference_configuration: reference configuration to compute the "
"placement of the locked joints\n");
}
} // namespace python
......
......@@ -175,6 +175,40 @@ class RobotWrapper(object):
def framesForwardKinematics(self, q):
pin.framesForwardKinematics(self.model, self.data, q)
def buildReducedRobot(self,
list_of_joints_to_lock,
reference_configuration=None):
"""
Build a reduced robot model given a list of joints to lock.
Parameters:
\tlist_of_joints_to_lock: list of joint indexes/names to lock.
\treference_configuration: reference configuration to compute the
placement of the lock joints. If not provided, reference_configuration
defaults to the robot's neutral configuration.
Returns: a new robot model.
"""
# if joint to lock is a string, try to find its index
lockjoints_idx = []
for jnt in list_of_joints_to_lock:
idx = jnt
if isinstance(jnt, str):
idx = self.model.getJointId(jnt)
lockjoints_idx.append(idx)
if reference_configuration is None:
reference_configuration = pin.neutral(self.model)
model, geom_models = pin.buildReducedModel(
model=self.model,
list_of_geom_models=[self.visual_model, self.collision_model],
list_of_joints_to_lock=lockjoints_idx,
reference_configuration=reference_configuration)
return RobotWrapper(model=model, visual_model=geom_models[0],
collision_model=geom_models[1])
def getFrameJacobian(self, frame_id, rf_frame=pin.ReferenceFrame.LOCAL):
"""
It computes the Jacobian of frame given by its id (frame_id) either expressed in the
......
......@@ -18,6 +18,7 @@ model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, me
print('standard model: dim=' + str(len(model.joints)))
for jn in model.joints:
print(jn)
print('-' * 30)
# Create a list of joints to lock
jointsToLock = ['wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
......@@ -34,13 +35,40 @@ for jn in jointsToLock:
initialJointConfig = np.array([0,0,0, # shoulder and elbow
1,1,1]) # gripper)
# Option 1: Build the reduced model including the geometric model for proper displaying of the robot
model_reduced, visual_model_reduced = pin.buildReducedModel(model, visual_model, jointsToLockIDs, initialJointConfig)
# Option 1: Only build the reduced model in case no display needed:
model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)
# Option 2: Only build the reduced model in case no display needed:
# model_reduced = pin.buildReducedModel(model, jointsToLockIDs, initialJointConfig)
# Option 2: Build the reduced model including the geometric model for proper displaying of the robot
model_reduced, visual_model_reduced = pin.buildReducedModel(
model, visual_model, jointsToLockIDs, initialJointConfig)
# Option 3: Build the reduced model including multiple geometric models (for example: visuals, collision)
geom_models = [visual_model, collision_model]
model_reduced, geometric_models_reduced = pin.buildReducedModel(
model,
list_of_geom_models=geom_models,
list_of_joints_to_lock=jointsToLockIDs,
reference_configuration=initialJointConfig)
# geometric_models_reduced is a list, ordered as the passed variable "geom_models" so:
visual_model_reduced, collision_model_reduced = geometric_models_reduced[
0], geometric_models_reduced[1]
# Check dimensions of the reduced model
# options 1-3 only take joint ids
print('joints to lock (only ids):', jointsToLockIDs)
print('reduced model: dim=' + str(len(model_reduced.joints)))
for jn in model_reduced.joints:
print('-' * 30)
# Option 4: Build a reduced model of a robot using RobotWrapper
# reference_configuration is optional: if not provided, neutral configuration used
# you can even mix joint names and joint ids
mixed_jointsToLockIDs = [jointsToLockIDs[0], 'wrist_2_joint', 'wrist_3_joint']
robot = pin.RobotWrapper.BuildFromURDF(urdf_filename, mesh_dir)
reduced_robot = robot.buildReducedRobot(list_of_joints_to_lock=mixed_jointsToLockIDs,
reference_configuration=initialJointConfig)
# Check dimensions of the reduced model and joint info
print('mixed joints to lock (names and ids):', mixed_jointsToLockIDs)
print('RobotWrapper reduced model: dim=' + str(len(reduced_robot.model.joints)))
for jn in robot.model.joints:
print(jn)
......@@ -144,6 +144,32 @@ namespace pinocchio
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
GeometryModel & reduced_geom_model);
/**
*
* \brief Build a reduced model and a rededuced geometry model from a given input model, a given input geometry model and a list of joint to lock.
*
* \param[in] model the input model to reduce.
* \param[in] list_of_geom_models the input geometry model to reduce (example: visual_model, collision_model).
* \param[in] list_of_joints_to_lock list of joints to lock in the input model.
* \param[in] reference_configuration reference configuration.
* \param[out] reduced_model the reduced model.
* \param[out] list_of_reduced_geom_model the list of reduced geometry models.
*
* \remarks All the joints that have been set to be fixed in the new reduced_model now appear in the kinematic tree as a Frame as FIXED_JOINT.
*
*/
template <typename Scalar, int Options,
template <typename, int> class JointCollectionTpl,
typename GeometryModelAllocator,
typename ConfigVectorType>
void buildReducedModel(
const ModelTpl<Scalar, Options, JointCollectionTpl> &model,
const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models,
const std::vector<JointIndex> &list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models);
} // namespace pinocchio
#include "pinocchio/algorithm/model.hxx"
......
......@@ -477,56 +477,85 @@ namespace pinocchio
ModelTpl<Scalar,Options,JointCollectionTpl> & reduced_model,
GeometryModel & reduced_geom_model)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
buildReducedModel(input_model,list_of_joints_to_lock,reference_configuration,reduced_model);
// Add all the geometries
typedef GeometryModel::GeometryObject GeometryObject;
typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
for(GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
it != input_geom_model.geometryObjects.end(); ++it)
{
const GeometryModel::GeometryObject & geom = *it;
const JointIndex joint_id_in_input_model = geom.parentJoint;
_PINOCCHIO_CHECK_INPUT_ARGUMENT_2((joint_id_in_input_model < (JointIndex)input_model.njoints),
"Invalid joint parent index for the geometry with name " + geom.name);
const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
JointIndex reduced_joint_id = (JointIndex)-1;
typedef typename Model::SE3 SE3;
SE3 relative_placement = SE3::Identity();
if(reduced_model.existJointName(parent_joint_name))
const std::vector<GeometryModel> temp_input_geoms { input_geom_model };
std::vector<GeometryModel> temp_reduced_geom_models;
buildReducedModel(input_model, temp_input_geoms, list_of_joints_to_lock,
reference_configuration, reduced_model,
temp_reduced_geom_models);
reduced_geom_model = temp_reduced_geom_models.front();
}
template <typename Scalar, int Options,
template <typename, int> class JointCollectionTpl,
typename GeometryModelAllocator,
typename ConfigVectorType>
void buildReducedModel(
const ModelTpl<Scalar, Options, JointCollectionTpl> &input_model,
const std::vector<GeometryModel,GeometryModelAllocator> &list_of_geom_models,
const std::vector<JointIndex> &list_of_joints_to_lock,
const Eigen::MatrixBase<ConfigVectorType> &reference_configuration,
ModelTpl<Scalar, Options, JointCollectionTpl> &reduced_model,
std::vector<GeometryModel,GeometryModelAllocator> &list_of_reduced_geom_models) {
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);
// for all GeometryModels
for (size_t gmi = 0; gmi < list_of_geom_models.size(); ++gmi) {
const GeometryModel &input_geom_model = list_of_geom_models[gmi];
GeometryModel reduced_geom_model;
// Add all the geometries
typedef GeometryModel::GeometryObject GeometryObject;
typedef GeometryModel::GeometryObjectVector GeometryObjectVector;
for(GeometryObjectVector::const_iterator it = input_geom_model.geometryObjects.begin();
it != input_geom_model.geometryObjects.end(); ++it)
{
reduced_joint_id = reduced_model.getJointId(parent_joint_name);
const GeometryModel::GeometryObject & geom = *it;
const JointIndex joint_id_in_input_model = geom.parentJoint;
_PINOCCHIO_CHECK_INPUT_ARGUMENT_2((joint_id_in_input_model < (JointIndex)input_model.njoints),
"Invalid joint parent index for the geometry with name " + geom.name);
const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];
JointIndex reduced_joint_id = (JointIndex)-1;
typedef typename Model::SE3 SE3;
SE3 relative_placement = SE3::Identity();
if(reduced_model.existJointName(parent_joint_name))
{
reduced_joint_id = reduced_model.getJointId(parent_joint_name);
}
else // The joint is now a frame
{
const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
reduced_joint_id = reduced_model.frames[reduced_frame_id].parent;
relative_placement = reduced_model.frames[reduced_frame_id].placement;
}
GeometryObject reduced_geom(geom);
reduced_geom.parentJoint = reduced_joint_id;
reduced_geom.parentFrame = reduced_model.getBodyId(
input_model.frames[geom.parentFrame].name);
reduced_geom.placement = relative_placement * geom.placement;
reduced_geom_model.addGeometryObject(reduced_geom);
}
else // The joint is now a frame
#ifdef PINOCCHIO_WITH_HPP_FCL
// Add all the collision pairs - the index of the geometry objects should have not changed
typedef GeometryModel::CollisionPairVector CollisionPairVector;
for(CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
it != input_geom_model.collisionPairs.end(); ++it)
{
const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name);
reduced_joint_id = reduced_model.frames[reduced_frame_id].parent;
relative_placement = reduced_model.frames[reduced_frame_id].placement;
const CollisionPair & cp = *it;
reduced_geom_model.addCollisionPair(cp);
}
GeometryObject reduced_geom(geom);
reduced_geom.parentJoint = reduced_joint_id;
reduced_geom.parentFrame = reduced_model.getBodyId(
input_model.frames[geom.parentFrame].name);
reduced_geom.placement = relative_placement * geom.placement;
reduced_geom_model.addGeometryObject(reduced_geom);
}
#ifdef PINOCCHIO_WITH_HPP_FCL
// Add all the collision pairs - the index of the geometry objects should have not changed
typedef GeometryModel::CollisionPairVector CollisionPairVector;
for(CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();
it != input_geom_model.collisionPairs.end(); ++it)
{
const CollisionPair & cp = *it;
reduced_geom_model.addCollisionPair(cp);
#endif
list_of_reduced_geom_models.push_back(reduced_geom_model);
}
#endif
}
} // namespace pinocchio
......
......@@ -494,6 +494,22 @@ BOOST_AUTO_TEST_CASE(test_buildReducedModel_with_geom)
{
BOOST_CHECK(geom_data.oMg[i].isApprox(reduded_geom_data.oMg[i]));
}
// Test other signature
std::vector<GeometryModel> full_geometry_models;
full_geometry_models.push_back(humanoid_geometry);
full_geometry_models.push_back(humanoid_geometry);
full_geometry_models.push_back(humanoid_geometry);
std::vector<GeometryModel> reduced_geometry_models;
Model reduced_humanoid_model_other_sig;
buildReducedModel(humanoid_model,full_geometry_models,joints_to_lock,
reference_config_humanoid,reduced_humanoid_model_other_sig,reduced_geometry_models);
BOOST_CHECK(reduced_geometry_models[0] == reduced_humanoid_geometry);
BOOST_CHECK(reduced_geometry_models[1] == reduced_humanoid_geometry);
BOOST_CHECK(reduced_geometry_models[2] == reduced_humanoid_geometry);
}
#endif // PINOCCHIO_WITH_HPP_FCL
......
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