JointIndex getJointId(const ModelTpl<Scalar,Options,JointCollectionTpl> & model_in,

      const JointIndex joint_id = model_in.getJointId(joint_name_in_model_in);

      assert(joint_id < model_in.joints.size());

      if(joint_id == 0 && model_in.parents[0] == 0) // This is the universe, maybe renamed.

        return model_out.getJointId(joint_name_in_model_in);

    FrameIndex getFrameId(const ModelTpl<Scalar,Options,JointCollectionTpl> & model_in,

      const FrameIndex frame_id = model_in.getFrameId(frame_name_in_model_in);

      assert(frame_id < model_in.frames.size());

      if(frame_id == 0 && model_in.frames[0].previousFrame == 0 && model_in.frames[0].parent == 0) // This is the universe, maybe renamed.

        return model_out.getFrameId(model_out.frames[0].name,type);

        return model_out.getFrameId(frame_name_in_model_in,type);

    void appendUniverseToModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelAB,

      PINOCCHIO_THROW(parentFrame < model.frames.size(),

      const Frame & pframe = model.frames[parentFrame];

      JointIndex jid = pframe.parent;

      assert(jid < model.joints.size());

      if (modelAB.inertias[0] == modelAB.inertias[0])

        model.appendBodyToJoint (jid, modelAB.inertias[0], pframe.placement * pfMAB);

      for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)

        Frame frame = modelAB.frames[fid];

        if (frame.parent == 0)

          PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existFrame(frame.name, frame.type),

          frame.parent = jid;

          if (frame.previousFrame != 0)

            frame.previousFrame = getFrameId(modelAB,model,modelAB.frames[frame.previousFrame].name,

                                             modelAB.frames[frame.previousFrame].type);

            frame.previousFrame = parentFrame;

          frame.placement = pframe.placement * pfMAB * frame.placement;

          model.addFrame (frame);

      for (GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)

        GeometryObject go = geomModelAB.geometryObjects[gid];

        if (go.parentJoint == 0)

          go.parentJoint = jid;

          if (go.parentFrame != 0)

            go.parentFrame = getFrameId(modelAB,model,modelAB.frames[go.parentFrame].name,

                                        modelAB.frames[go.parentFrame].type);

          go.placement = pframe.placement * pfMAB * go.placement;

          geomModel.addGeometryObject (go);

    }

      static void algo (const JointModelBase<JointModel> & jmodel_in,

        const JointIndex joint_id_in = jmodel_in.id();

        if (modelAB.parents[joint_id_in] > 0)

          parent_id = getJointId(modelAB,model,modelAB.names[modelAB.parents[joint_id_in]]);

        PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existJointName(modelAB.names[joint_id_in]),

        JointIndex joint_id_out = model.addJoint(parent_id,

                                                 pMi * modelAB.jointPlacements[joint_id_in],

                                                 modelAB.names[joint_id_in],

                                                 jmodel_in.jointVelocitySelector(modelAB.effortLimit),

                                                 jmodel_in.jointVelocitySelector(modelAB.velocityLimit),

                                                 jmodel_in.jointConfigSelector(modelAB.lowerPositionLimit),

                                                 jmodel_in.jointConfigSelector(modelAB.upperPositionLimit),

                                                 jmodel_in.jointVelocitySelector(modelAB.friction),

                                                 jmodel_in.jointVelocitySelector(modelAB.damping));

        assert(joint_id_out < model.joints.size());

        model.appendBodyToJoint(joint_id_out, modelAB.inertias[joint_id_in]);

        const typename Model::JointModel & jmodel_out = model.joints[joint_id_out];

        jmodel_out.jointVelocitySelector(model.rotorInertia) = jmodel_in.jointVelocitySelector(modelAB.rotorInertia);

        jmodel_out.jointVelocitySelector(model.rotorGearRatio) = jmodel_in.jointVelocitySelector(modelAB.rotorGearRatio);

        for (FrameIndex fid = 1; fid < modelAB.frames.size(); ++fid)

          Frame frame = modelAB.frames[fid];

          if (frame.parent == jmodel_in.id())

            PINOCCHIO_CHECK_INPUT_ARGUMENT(!model.existFrame(frame.name, frame.type),

            frame.parent = joint_id_out;

            assert (frame.previousFrame > 0 || frame.type == JOINT);

            if (frame.previousFrame != 0)

              frame.previousFrame = getFrameId(modelAB,model,modelAB.frames[frame.previousFrame].name,modelAB.frames[frame.previousFrame].type);

            model.addFrame(frame);

        for(GeomIndex gid = 0; gid < geomModelAB.geometryObjects.size(); ++gid)

          GeometryObject go = geomModelAB.geometryObjects[gid];

          if(go.parentJoint == joint_id_in)

            go.parentJoint = joint_id_out;

            assert(go.parentFrame > 0);

            if(go.parentFrame != 0 && go.parentFrame < modelAB.frames.size())

              go.parentFrame = getFrameId(modelAB,model,modelAB.frames[go.parentFrame].name,modelAB.frames[go.parentFrame].type);

            geomModel.addGeometryObject(go);

  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,

    GeometryModel geomModelA, geomModelB, geomModel;

    appendModel(modelA, modelB, geomModelA, geomModelB, frameInModelA, aMb,

  }

  appendModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & modelA,

    PINOCCHIO_CHECK_INPUT_ARGUMENT((bool)(frameInModelA < (FrameIndex) modelA.nframes),

    const Frame & frame = modelA.frames[frameInModelA];

    static const SE3 id = SE3::Identity();

    int njoints = modelA.njoints + modelB.njoints - 1;

    model.names                 .reserve ((size_t)njoints);

    model.joints                .reserve ((size_t)njoints);

    model.jointPlacements       .reserve ((size_t)njoints);

    model.parents               .reserve ((size_t)njoints);

    model.inertias              .reserve ((size_t)njoints);

    int nframes = modelA.nframes + modelB.nframes - 1;

    model.frames                .reserve ((size_t)nframes);

    geomModel.geometryObjects.reserve (geomModelA.ngeoms + geomModelB.ngeoms);

    details::appendUniverseToModel (modelA, geomModelA, 0, id, model, geomModel);

    for (JointIndex jid = 1; jid <= frame.parent; ++jid)

      ArgsType args (modelA, geomModelA, 0, id, model, geomModel);

      AppendJointOfModelAlgo::run (modelA.joints[jid], args);

    details::appendUniverseToModel (modelB, geomModelB,

                                    details::getFrameId(modelA,model,frame.name,frame.type), aMb, model, geomModel);

    for (JointIndex jid = 1; jid < modelB.joints.size(); ++jid)

      SE3 pMi = (jid == 1 ? frame.placement * aMb : id);

      ArgsType args (modelB, geomModelB, frame.parent, pMi, model, geomModel);

      AppendJointOfModelAlgo::run (modelB.joints[jid], args);

    for (JointIndex jid = frame.parent+1; jid < modelA.joints.size(); ++jid)

      ArgsType args (modelA, geomModelA, 0, id, model, geomModel);

      AppendJointOfModelAlgo::run (modelA.joints[jid], args);

    geomModel.collisionPairs.reserve(geomModelA.collisionPairs.size()

       + geomModelB.collisionPairs.size()

       + geomModelA.geometryObjects.size() * geomModelB.geometryObjects.size());

    for (std::size_t icp = 0; icp < geomModelA.collisionPairs.size(); ++icp)

      const CollisionPair& cp = geomModelA.collisionPairs[icp];

      GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.first ].name);

      GeomIndex go2 = geomModel.getGeometryId(geomModelA.geometryObjects[cp.second].name);

      geomModel.addCollisionPair (CollisionPair (go1, go2));

    for (std::size_t icp = 0; icp < geomModelB.collisionPairs.size(); ++icp)

      const CollisionPair & cp = geomModelB.collisionPairs[icp];

      GeomIndex go1 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.first ].name);

      GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[cp.second].name);

      geomModel.addCollisionPair (CollisionPair (go1, go2));

    for (Index i = 0; i < geomModelA.geometryObjects.size(); ++i)

      GeomIndex go1 = geomModel.getGeometryId(geomModelA.geometryObjects[i].name);

      for (Index j = 0; j < geomModelB.geometryObjects.size(); ++j)

        GeomIndex go2 = geomModel.getGeometryId(geomModelB.geometryObjects[j].name);

        if (   geomModel.geometryObjects[go1].parentJoint

            != geomModel.geometryObjects[go2].parentJoint)

          geomModel.addCollisionPair(CollisionPair(go1, go2));

  }

  buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & input_model,

    PINOCCHIO_CHECK_ARGUMENT_SIZE(reference_configuration.size(), input_model.nq,

    PINOCCHIO_CHECK_INPUT_ARGUMENT(list_of_joints_to_lock.size() <= (size_t)input_model.njoints,

    std::sort(list_of_joints_to_lock.begin(),list_of_joints_to_lock.end());

    for(size_t id = 1; id < list_of_joints_to_lock.size(); ++id)

      PINOCCHIO_CHECK_INPUT_ARGUMENT(list_of_joints_to_lock[id-1] < list_of_joints_to_lock[id],

    const Eigen::DenseIndex njoints = input_model.njoints - (Eigen::DenseIndex)list_of_joints_to_lock.size();

    reduced_model.names                 .reserve((size_t)njoints);

    reduced_model.joints                .reserve((size_t)njoints);

    reduced_model.jointPlacements       .reserve((size_t)njoints);

    reduced_model.parents               .reserve((size_t)njoints);

    reduced_model.names[0] = input_model.names[0];

    reduced_model.joints[0] = input_model.joints[0];

    reduced_model.jointPlacements[0] = input_model.jointPlacements[0];

    reduced_model.parents[0] = input_model.parents[0];

    reduced_model.inertias[0] = input_model.inertias[0];

    reduced_model.name = input_model.name;

    reduced_model.gravity = input_model.gravity;

    size_t current_index_to_lock = 0;

    for(JointIndex joint_id = 1; joint_id < (JointIndex)input_model.njoints; ++joint_id)

      const JointIndex joint_id_to_lock = (current_index_to_lock < list_of_joints_to_lock.size()) ? list_of_joints_to_lock[current_index_to_lock] : 0;

      const JointIndex input_parent_joint_index = input_model.parents[joint_id];

      const std::string & joint_name = input_model.names[joint_id];

      const JointModel & joint_input_model = input_model.joints[joint_id];

      const std::string & parent_joint_name = input_model.names[input_parent_joint_index];

      const bool exist_parent_joint = reduced_model.existJointName(parent_joint_name);

      const JointIndex reduced_parent_joint_index

      ? reduced_model.getJointId(parent_joint_name)

      : reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].parent;

      const SE3 parent_frame_placement

      : reduced_model.frames[reduced_model.getFrameId(parent_joint_name)].placement;

      const FrameIndex reduced_previous_frame_index

      ? 0

      if(joint_id == joint_id_to_lock)

        JointData joint_data = joint_input_model.createData();

        joint_input_model.calc(joint_data,reference_configuration);

        const SE3 liMi = parent_frame_placement * input_model.jointPlacements[joint_id] * joint_data.M();

        Frame frame = Frame(joint_name,

                            input_model.inertias[joint_id]);

        FrameIndex frame_id = reduced_model.addFrame(frame);

        reduced_model.frames[frame_id].previousFrame = frame_id; // a bit weird, but this is a solution for missing parent frame

        current_index_to_lock++;

        JointIndex reduced_joint_id = reduced_model.addJoint(reduced_parent_joint_index,

                                                             parent_frame_placement * input_model.jointPlacements[joint_id],

                                                             joint_input_model.jointVelocitySelector(input_model.effortLimit),

                                                             joint_input_model.jointVelocitySelector(input_model.velocityLimit),

                                                             joint_input_model.jointConfigSelector(input_model.lowerPositionLimit),

                                                             joint_input_model.jointConfigSelector(input_model.upperPositionLimit),

                                                             joint_input_model.jointVelocitySelector(input_model.friction),

                                                             joint_input_model.jointVelocitySelector(input_model.damping));

        reduced_model.appendBodyToJoint(reduced_joint_id,

                                        input_model.inertias[joint_id],

        const typename Model::JointModel & jmodel_out = reduced_model.joints[reduced_joint_id];

        jmodel_out.jointVelocitySelector(reduced_model.rotorInertia)

        = joint_input_model.jointVelocitySelector(input_model.rotorInertia);

        jmodel_out.jointVelocitySelector(reduced_model.rotorGearRatio)

        = joint_input_model.jointVelocitySelector(input_model.rotorGearRatio);

    for(typename ConfigVectorMap::const_iterator config_it = input_model.referenceConfigurations.begin();

        config_it != input_model.referenceConfigurations.end(); ++config_it)

      const std::string & config_name = config_it->first;

      const typename Model::ConfigVectorType & input_config_vector = config_it->second;

      typename Model::ConfigVectorType reduced_config_vector(reduced_model.nq);

      for(JointIndex reduced_joint_id = 1;

          reduced_joint_id < reduced_model.joints.size();

        const JointIndex input_joint_id = input_model.getJointId(reduced_model.names[reduced_joint_id]);

        const JointModel & input_joint_model = input_model.joints[input_joint_id];

        const JointModel & reduced_joint_model = reduced_model.joints[reduced_joint_id];

        reduced_joint_model.jointConfigSelector(reduced_config_vector) = input_joint_model.jointConfigSelector(input_config_vector);

      reduced_model.referenceConfigurations.insert(std::make_pair(config_name, reduced_config_vector));

    typename Model::FrameVector::const_iterator frame_it = input_model.frames.begin();

    for(++frame_it;frame_it != input_model.frames.end(); ++frame_it)

      const Frame & input_frame = *frame_it;

      const std::string & support_joint_name = input_model.names[input_frame.parent];

      std::vector<JointIndex>::const_iterator support_joint_it = std::find(list_of_joints_to_lock.begin(),

                                                                           input_frame.parent);

      if(support_joint_it != list_of_joints_to_lock.end())

        if(   input_frame.type == JOINT

           && reduced_model.existFrame(input_frame.name)

           && support_joint_name == input_frame.name)

          continue; // this means that the Joint is now fixed and has been replaced by a Frame. No need to add a new one.

        const FrameIndex joint_frame_id = reduced_model.getFrameId(support_joint_name);

        const Frame & joint_frame = reduced_model.frames[joint_frame_id];

        Frame reduced_frame = input_frame;

        reduced_frame.placement = joint_frame.placement * input_frame.placement;

        reduced_frame.parent = joint_frame.parent;

        reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);

        reduced_model.addFrame(reduced_frame, false);

        Frame reduced_frame = input_frame;

        reduced_frame.parent = reduced_model.getJointId(input_model.names[input_frame.parent]);

        reduced_frame.previousFrame = reduced_model.getFrameId(input_model.frames[input_frame.previousFrame].name);

        reduced_model.addFrame(reduced_frame, false);

  }

  buildReducedModel(const ModelTpl<Scalar,Options,JointCollectionTpl> & input_model,

    const std::vector<GeometryModel> temp_input_geoms(1,input_geom_model);

    std::vector<GeometryModel> temp_reduced_geom_models;

    buildReducedModel(input_model, temp_input_geoms, list_of_joints_to_lock,

    reduced_geom_model = temp_reduced_geom_models.front();

  }

  void buildReducedModel(

    buildReducedModel(input_model, list_of_joints_to_lock, reference_configuration, reduced_model);

    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;

      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),

        const std::string & parent_joint_name = input_model.names[joint_id_in_input_model];

        JointIndex reduced_joint_id = (JointIndex)-1;

        SE3 relative_placement = SE3::Identity();

        if(reduced_model.existJointName(parent_joint_name))

          reduced_joint_id = reduced_model.getJointId(parent_joint_name);

          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);

      for(CollisionPairVector::const_iterator it = input_geom_model.collisionPairs.begin();

          it != input_geom_model.collisionPairs.end(); ++it)

    list_of_reduced_geom_models.push_back(reduced_geom_model);

  }