pinocchio  UNKNOWN
fcl.hxx
1 //
2 // Copyright (c) 2015-2016 CNRS
3 //
4 // This file is part of Pinocchio
5 // Pinocchio is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 //
10 // Pinocchio is distributed in the hope that it will be
11 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 // General Lesser Public License for more details. You should have
14 // received a copy of the GNU Lesser General Public License along with
15 // Pinocchio If not, see
16 // <http://www.gnu.org/licenses/>.
17 
18 #ifndef __se3_fcl_hxx__
19 #define __se3_fcl_hxx__
20 
21 
22 #include <iostream>
23 
24 
25 namespace se3
26 {
27 
35  inline CollisionPair::CollisionPair(const GeomIndex co1, const GeomIndex co2)
36  : Base(co1,co2)
37  {
38  assert(co1 != co2 && "The index of collision objects must not be equal.");
39  }
40 
41  inline bool CollisionPair::operator== (const CollisionPair& rhs) const
42  {
43  return (first == rhs.first && second == rhs.second)
44  || (first == rhs.second && second == rhs.first );
45  }
46 
47  inline void CollisionPair::disp(std::ostream & os) const
48  { os << "collision pair (" << first << "," << second << ")\n"; }
49 
50  inline std::ostream & operator << (std::ostream & os, const CollisionPair & X)
51  {
52  X.disp(os); return os;
53  }
54 
55 
56 #ifdef WITH_HPP_FCL
57 
58  inline bool operator == (const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
59  {
60  return lhs.collisionGeometry() == rhs.collisionGeometry()
61  && lhs.getAABB().min_ == rhs.getAABB().min_
62  && lhs.getAABB().max_ == rhs.getAABB().max_;
63  }
64 
65 #endif // WITH_HPP_FCL
66 
67  inline bool operator==(const GeometryObject & lhs, const GeometryObject & rhs)
68  {
69  return ( lhs.name == rhs.name
70  && lhs.parentFrame == rhs.parentFrame
71  && lhs.parentJoint == rhs.parentJoint
72  && lhs.fcl == rhs.fcl
73  && lhs.placement == rhs.placement
74  && lhs.meshPath == rhs.meshPath
75  && lhs.meshScale == rhs.meshScale
76  );
77  }
78 
79  inline std::ostream & operator<< (std::ostream & os, const GeometryObject & geom_object)
80  {
81  os << "Name: \t \n" << geom_object.name << "\n"
82  << "Parent frame ID: \t \n" << geom_object.parentFrame << "\n"
83  << "Parent joint ID: \t \n" << geom_object.parentJoint << "\n"
84  << "Position in parent frame: \t \n" << geom_object.placement << "\n"
85  << "Absolute path to mesh file: \t \n" << geom_object.meshPath << "\n"
86  << "Scale for transformation of the mesh: \t \n" << geom_object.meshScale.transpose() << "\n"
87  << std::endl;
88  return os;
89  }
90 
91 
92 } // namespace se3
93 
94 
95 #endif // ifndef __se3_fcl_hxx__
JointIndex parentJoint
Index of the parent joint.
Definition: fcl.hpp:102
Eigen::Vector3d meshScale
Scaling vector applied to the fcl object.
Definition: fcl.hpp:114
boost::shared_ptr< fcl::CollisionGeometry > fcl
The actual cloud of points representing the collision mesh of the object after scaling.
Definition: fcl.hpp:105
CollisionPair(const GeomIndex co1, const GeomIndex co2)
Default constructor of a collision pair from two collision object indexes. The indexes must be ordere...
Definition: fcl.hxx:35
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string name
Name of the geometry object.
Definition: fcl.hpp:92
std::string meshPath
Absolute path to the mesh file.
Definition: fcl.hpp:111
SE3 placement
Position of geometry object in parent joint frame.
Definition: fcl.hpp:108
FrameIndex parentFrame
Index of the parent frame.
Definition: fcl.hpp:99