Line |
Branch |
Exec |
Source |
1 |
|
|
// Copyright (C) 2009, 2010 by Florent Lamiraux, Thomas Moulard, JRL. |
2 |
|
|
// |
3 |
|
|
|
4 |
|
|
// Redistribution and use in source and binary forms, with or without |
5 |
|
|
// modification, are permitted provided that the following conditions are |
6 |
|
|
// met: |
7 |
|
|
// |
8 |
|
|
// 1. Redistributions of source code must retain the above copyright |
9 |
|
|
// notice, this list of conditions and the following disclaimer. |
10 |
|
|
// |
11 |
|
|
// 2. Redistributions in binary form must reproduce the above copyright |
12 |
|
|
// notice, this list of conditions and the following disclaimer in the |
13 |
|
|
// documentation and/or other materials provided with the distribution. |
14 |
|
|
// |
15 |
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
16 |
|
|
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
17 |
|
|
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
18 |
|
|
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
19 |
|
|
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
20 |
|
|
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
21 |
|
|
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
22 |
|
|
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
23 |
|
|
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
24 |
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
25 |
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH |
26 |
|
|
// DAMAGE. |
27 |
|
|
// |
28 |
|
|
// This software is provided "as is" without warranty of any kind, |
29 |
|
|
// either expressed or implied, including but not limited to the |
30 |
|
|
// implied warranties of fitness for a particular purpose. |
31 |
|
|
// |
32 |
|
|
// See the COPYING file for more information. |
33 |
|
|
|
34 |
|
|
#include "obstacle.impl.hh" |
35 |
|
|
|
36 |
|
|
#include <coal/BVH/BVH_model.h> |
37 |
|
|
#include <coal/mesh_loader/loader.h> |
38 |
|
|
#include <coal/shape/geometric_shapes.h> |
39 |
|
|
|
40 |
|
|
#include <hpp/corbaserver/server-plugin.hh> |
41 |
|
|
#include <hpp/pinocchio/collision-object.hh> |
42 |
|
|
#include <hpp/pinocchio/device.hh> |
43 |
|
|
#include <hpp/pinocchio/urdf/util.hh> |
44 |
|
|
#include <hpp/util/debug.hh> |
45 |
|
|
#include <hpp/util/exception-factory.hh> |
46 |
|
|
#include <iostream> |
47 |
|
|
#include <pinocchio/algorithm/geometry.hpp> |
48 |
|
|
#include <pinocchio/multibody/geometry.hpp> |
49 |
|
|
#include <pinocchio/parsers/utils.hpp> |
50 |
|
|
|
51 |
|
|
#include "tools.hh" |
52 |
|
|
|
53 |
|
|
namespace hpp { |
54 |
|
|
namespace corbaServer { |
55 |
|
|
namespace impl { |
56 |
1/2
✓ Branch 3 taken 8 times.
✗ Branch 4 not taken.
|
16 |
Obstacle::Obstacle() : server_(NULL) {} |
57 |
|
|
|
58 |
|
✗ |
core::ProblemSolverPtr_t Obstacle::problemSolver() { |
59 |
|
✗ |
return server_->problemSolver(); |
60 |
|
|
} |
61 |
|
|
|
62 |
|
✗ |
void Obstacle::loadObstacleModel(const char* filename, const char* prefix) { |
63 |
|
|
try { |
64 |
|
✗ |
DevicePtr_t device(Device::create(prefix)); |
65 |
|
✗ |
hpp::pinocchio::urdf::loadModel(device, 0, "", "anchor", filename, ""); |
66 |
|
|
|
67 |
|
✗ |
problemSolver()->addObstacle(device, true, true); |
68 |
|
✗ |
} catch (const std::exception& exc) { |
69 |
|
✗ |
throw hpp::Error(exc.what()); |
70 |
|
|
} |
71 |
|
|
} |
72 |
|
|
|
73 |
|
✗ |
void Obstacle::loadObstacleModelFromString(const char* urdfString, |
74 |
|
|
const char* prefix) { |
75 |
|
|
try { |
76 |
|
✗ |
DevicePtr_t device(Device::create(prefix)); |
77 |
|
✗ |
hpp::pinocchio::urdf::loadModelFromString(device, 0, "", "anchor", |
78 |
|
|
urdfString, ""); |
79 |
|
✗ |
problemSolver()->addObstacle(device, true, true); |
80 |
|
✗ |
} catch (const std::exception& exc) { |
81 |
|
✗ |
throw hpp::Error(exc.what()); |
82 |
|
|
} |
83 |
|
|
} |
84 |
|
|
|
85 |
|
✗ |
void Obstacle::loadPolyhedron(const char* name, const char* resourcename) { |
86 |
|
|
try { |
87 |
|
✗ |
coal::MeshLoader loader(coal::BV_OBBRSS); |
88 |
|
|
std::string filename(::pinocchio::retrieveResourcePath( |
89 |
|
✗ |
resourcename, ::pinocchio::rosPaths())); |
90 |
|
|
coal::CollisionGeometryPtr_t coalgeom( |
91 |
|
✗ |
loader.load(filename, coal::Vec3f::Ones())); |
92 |
|
|
CollisionGeometryPtr_t geom(coalgeom.get(), |
93 |
|
✗ |
[coalgeom](...) mutable { coalgeom.reset(); }); |
94 |
|
✗ |
problemSolver()->addObstacle(name, geom, Transform3s::Identity(), true, |
95 |
|
|
true); |
96 |
|
✗ |
} catch (const std::exception& exc) { |
97 |
|
✗ |
throw hpp::Error(exc.what()); |
98 |
|
|
} |
99 |
|
|
} |
100 |
|
|
|
101 |
|
✗ |
void Obstacle::removeObstacleFromJoint(const char* objectName, |
102 |
|
|
const char* jointName, Boolean collision, |
103 |
|
|
Boolean distance) { |
104 |
|
|
using pinocchio::COLLISION; |
105 |
|
|
using pinocchio::DISTANCE; |
106 |
|
|
using pinocchio::JointPtr_t; |
107 |
|
|
using pinocchio::ObjectVector_t; |
108 |
|
✗ |
std::string objName(objectName); |
109 |
|
✗ |
std::string jName(jointName); |
110 |
|
|
|
111 |
|
|
try { |
112 |
|
✗ |
if (collision) { |
113 |
|
✗ |
problemSolver()->removeObstacleFromJoint(objectName, jointName); |
114 |
|
|
} |
115 |
|
✗ |
if (distance) { |
116 |
|
✗ |
throw std::runtime_error("Not implemented."); |
117 |
|
|
} |
118 |
|
✗ |
} catch (const std::exception& exc) { |
119 |
|
✗ |
throw hpp::Error(exc.what()); |
120 |
|
|
} |
121 |
|
|
} |
122 |
|
|
|
123 |
|
✗ |
void Obstacle::removeObstacle(const char* objectName) { |
124 |
|
|
try { |
125 |
|
✗ |
problemSolver()->removeObstacle(objectName); |
126 |
|
✗ |
} catch (std::exception& e) { |
127 |
|
✗ |
throw Error(e.what()); |
128 |
|
|
} |
129 |
|
|
} |
130 |
|
|
|
131 |
|
✗ |
void Obstacle::cutObstacle(const char* objectName, const floatSeq& aabb) { |
132 |
|
|
try { |
133 |
|
✗ |
vector_t _aabb = floatSeqToVector(aabb, 6); |
134 |
|
✗ |
coal::AABB coalaabb(_aabb.head<3>(), _aabb.tail<3>()); |
135 |
|
✗ |
problemSolver()->cutObstacle(objectName, coalaabb); |
136 |
|
✗ |
} catch (const std::exception& e) { |
137 |
|
✗ |
throw hpp::Error(e.what()); |
138 |
|
|
} |
139 |
|
|
} |
140 |
|
|
|
141 |
|
✗ |
void Obstacle::addObstacle(const char* objectName, Boolean collision, |
142 |
|
|
Boolean distance) { |
143 |
|
✗ |
DevicePtr_t robot = getRobotOrThrow(problemSolver()); |
144 |
|
|
|
145 |
|
✗ |
std::string objName(objectName); |
146 |
|
✗ |
CollisionGeometryPtr_t geometry = objectMap_.geometry(objectName); |
147 |
|
✗ |
problemSolver()->addObstacle(objName, geometry, Transform3s::Identity(), |
148 |
|
|
collision, distance); |
149 |
|
|
} |
150 |
|
|
|
151 |
|
✗ |
CollisionObjectPtr_t Obstacle::getObstacleByName(const char* name) { |
152 |
|
|
try { |
153 |
|
✗ |
return problemSolver()->obstacle(name); |
154 |
|
✗ |
} catch (const std::invalid_argument&) { |
155 |
|
✗ |
HPP_THROW(Error, "Object " << name << " not found"); |
156 |
|
|
} |
157 |
|
|
} |
158 |
|
|
|
159 |
|
✗ |
void Obstacle::moveObstacle(const char* objectName, const Transform_ cfg) { |
160 |
|
✗ |
CollisionObjectPtr_t object = getObstacleByName(objectName); |
161 |
|
✗ |
if (object) { |
162 |
|
✗ |
object->move(toTransform3s(cfg)); |
163 |
|
✗ |
return; |
164 |
|
|
} |
165 |
|
✗ |
HPP_THROW(Error, "Object " << objectName << " not found"); |
166 |
|
|
} |
167 |
|
|
|
168 |
|
✗ |
void Obstacle::getObstaclePosition(const char* objectName, Transform_ cfg) { |
169 |
|
✗ |
CollisionObjectPtr_t object = getObstacleByName(objectName); |
170 |
|
✗ |
if (object) { |
171 |
|
✗ |
Transform3s transform = object->getTransform(); |
172 |
|
✗ |
toHppTransform(transform, cfg); |
173 |
|
✗ |
return; |
174 |
|
|
} |
175 |
|
|
try { |
176 |
|
✗ |
Transform3s transform = problemSolver()->obstacleFramePosition(objectName); |
177 |
|
✗ |
toHppTransform(transform, cfg); |
178 |
|
✗ |
return; |
179 |
|
✗ |
} catch (const std::invalid_argument&) { |
180 |
|
|
// Ignore this exception. |
181 |
|
✗ |
} catch (const std::exception& e) { |
182 |
|
✗ |
throw Error(e.what()); |
183 |
|
|
} |
184 |
|
✗ |
HPP_THROW(Error, "Object " << objectName << " not found"); |
185 |
|
|
} |
186 |
|
|
|
187 |
|
✗ |
Names_t* Obstacle::getObstacleNames(bool collision, bool distance) { |
188 |
|
|
std::list<std::string> obstacles = |
189 |
|
✗ |
problemSolver()->obstacleNames(collision, distance); |
190 |
|
✗ |
return toNames_t(obstacles.begin(), obstacles.end()); |
191 |
|
|
} |
192 |
|
|
|
193 |
|
✗ |
void Obstacle::createPolyhedron(const char* polyhedronName) { |
194 |
|
✗ |
objectMap_.createPolyhedron(polyhedronName); |
195 |
|
|
} |
196 |
|
|
|
197 |
|
✗ |
void Obstacle::createBox(const char* boxName, Double x, Double y, Double z) { |
198 |
|
✗ |
objectMap_.createBox(boxName, x, y, z); |
199 |
|
|
} |
200 |
|
|
|
201 |
|
✗ |
void Obstacle::createSphere(const char* name, Double radius) { |
202 |
|
✗ |
objectMap_.createSphere(name, radius); |
203 |
|
|
} |
204 |
|
|
|
205 |
|
✗ |
void Obstacle::createCylinder(const char* name, Double radius, Double length) { |
206 |
|
✗ |
objectMap_.createCylinder(name, radius, length); |
207 |
|
|
} |
208 |
|
|
|
209 |
|
✗ |
ULong Obstacle::addPoint(const char* polyhedronName, Double x, Double y, |
210 |
|
|
Double z) { |
211 |
|
✗ |
return static_cast<ULong>(objectMap_.addPoint(polyhedronName, x, y, z)); |
212 |
|
|
} |
213 |
|
|
|
214 |
|
✗ |
ULong Obstacle::addTriangle(const char* polyhedronName, ULong pt1, ULong pt2, |
215 |
|
|
ULong pt3) { |
216 |
|
|
return static_cast<ULong>( |
217 |
|
✗ |
objectMap_.addTriangle(polyhedronName, pt1, pt2, pt3)); |
218 |
|
|
} |
219 |
|
|
} // namespace impl |
220 |
|
|
} // end of namespace corbaServer. |
221 |
|
|
} // end of namespace hpp. |
222 |
|
|
|