model-loader.cc 1.81 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.

#include <hpp/pinocchio/hpp-model/model-loader.hh>
18
#include <pinocchio/multibody/geometry.hpp>
19
20
21

/* Default path of the urdf file describing the robot to parse. */
const std::string urdfDefaultFilename =
22
       ROMEO_MODEL_DIR "/romeo_description/urdf/romeo_small.urdf";
23
24
25
26
27
28

/* Build a hpp::pinocchio::Device from urdf path. */
hpp::pinocchio::DevicePtr_t hppPinocchio( bool withGeoms, const std::string urdfFilename)
{
  hpp::pinocchio::DevicePtr_t pinocchio = hpp::pinocchio::Device::create(urdfFilename);
  hpp::pinocchio::ModelPtr_t model( new se3::Model() );
29
  se3::urdf::buildModel(urdfFilename,se3::JointModelFreeFlyer(),*model);
30
31
32
33
34
  pinocchio->model(model);
  pinocchio->createData();

  if( withGeoms )
    {
Nicolas Mansard's avatar
Nicolas Mansard committed
35
      std::vector<std::string> baseDirs; baseDirs.push_back(ROMEO_MODEL_DIR);
36
      hpp::pinocchio::GeomModelPtr_t geom( new se3::GeometryModel() );
37
      se3::urdf::buildGeom(pinocchio->model(),pinocchio->name(),se3::COLLISION,*geom,baseDirs);
38
39
40
41
42
43
44
45

      pinocchio->geomModel(geom);
      pinocchio->createGeomData();
    }

  return pinocchio;
}