1 |
|
|
// Copyright (c) 2014, LAAS-CNRS |
2 |
|
|
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
// Redistribution and use in source and binary forms, with or without |
6 |
|
|
// modification, are permitted provided that the following conditions are |
7 |
|
|
// met: |
8 |
|
|
// |
9 |
|
|
// 1. Redistributions of source code must retain the above copyright |
10 |
|
|
// notice, this list of conditions and the following disclaimer. |
11 |
|
|
// |
12 |
|
|
// 2. Redistributions in binary form must reproduce the above copyright |
13 |
|
|
// notice, this list of conditions and the following disclaimer in the |
14 |
|
|
// documentation and/or other materials provided with the distribution. |
15 |
|
|
// |
16 |
|
|
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
17 |
|
|
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
18 |
|
|
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
19 |
|
|
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
20 |
|
|
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
21 |
|
|
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
22 |
|
|
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
23 |
|
|
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
24 |
|
|
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
25 |
|
|
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
26 |
|
|
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH |
27 |
|
|
// DAMAGE. |
28 |
|
|
|
29 |
|
|
#include "hpp/manipulation/srdf/factories/contact.hh" |
30 |
|
|
|
31 |
|
|
#include <hpp/manipulation/device.hh> |
32 |
|
|
#include <hpp/pinocchio/body.hh> |
33 |
|
|
#include <hpp/pinocchio/collision-object.hh> |
34 |
|
|
#include <hpp/pinocchio/joint.hh> |
35 |
|
|
#include <hpp/util/debug.hh> |
36 |
|
|
#include <hpp/util/pointer.hh> |
37 |
|
|
#include <pinocchio/multibody/geometry.hpp> |
38 |
|
|
#include <pinocchio/multibody/model.hpp> |
39 |
|
|
|
40 |
|
|
namespace hpp { |
41 |
|
|
namespace manipulation { |
42 |
|
|
namespace srdf { |
43 |
|
|
void ContactFactory::finishTags() { |
44 |
|
|
DevicePtr_t device = HPP_DYNAMIC_PTR_CAST(Device, root()->device()); |
45 |
|
|
if (!device) { |
46 |
|
|
hppDout(error, "Failed to create contacts"); |
47 |
|
|
return; |
48 |
|
|
} |
49 |
|
|
const pinocchio::Model& model = device->model(); |
50 |
|
|
const pinocchio::GeomModel& geomModel = device->geomModel(); |
51 |
|
|
|
52 |
|
|
/// Get the link |
53 |
|
|
ObjectFactory* o = NULL; |
54 |
|
|
getChildOfType("link", o); |
55 |
|
|
linkName_ = root()->prependPrefix(o->name()); |
56 |
|
|
|
57 |
|
|
/// Build joint |
58 |
|
|
if (!model.existBodyName(linkName_)) |
59 |
|
|
throw std::invalid_argument("Link " + linkName_ + |
60 |
|
|
" not found. Cannot create contact"); |
61 |
|
|
const ::pinocchio::Frame& linkFrame = |
62 |
|
|
model.frames[model.getFrameId(linkName_)]; |
63 |
|
|
assert(linkFrame.type == ::pinocchio::BODY); |
64 |
|
|
JointPtr_t joint(Joint::create(device, linkFrame.parent)); |
65 |
|
|
|
66 |
|
|
Transform3f M; |
67 |
|
|
M.setIdentity(); |
68 |
|
|
if (o->hasAttribute("index")) { |
69 |
|
|
throw std::invalid_argument("attribute index is unsupported yet"); |
70 |
|
|
// If there is an index, we consider the position are given relatively |
71 |
|
|
// to the "index"th collision object, |
72 |
|
|
objectName_ = linkName_ + "_" + o->getAttribute("index"); |
73 |
|
|
/// In this case, coordinates are expressed in the body frame. |
74 |
|
|
pinocchio::GeomIndex id = geomModel.getGeometryId(objectName_); |
75 |
|
|
if (id < geomModel.geometryObjects.size()) { |
76 |
|
|
hppDout(error, |
77 |
|
|
"Geometry " << objectName_ << " not found in link " << linkName_); |
78 |
|
|
} else { |
79 |
|
|
M = geomModel.geometryObjects[id].placement; |
80 |
|
|
} |
81 |
|
|
} else { |
82 |
|
|
// If there is no index, the position are relative to the link. |
83 |
|
|
M = linkFrame.placement; |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
getChildOfType("point", o); |
87 |
|
|
PointFactory* pts = o->as<PointFactory>(); |
88 |
|
|
|
89 |
|
|
/// First build the sequence of points |
90 |
|
|
const PointFactory::OutType& v = pts->values(); |
91 |
|
|
if (v.size() % 3 != 0) |
92 |
|
|
throw std::length_error("Point sequence size should be a multiple of 3."); |
93 |
|
|
std::vector<vector3_t> points; |
94 |
|
|
for (size_t i = 0; i < v.size(); i += 3) |
95 |
|
|
points.push_back(M.act(vector3_t(v[i], v[i + 1], v[i + 2]))); |
96 |
|
|
|
97 |
|
|
try { |
98 |
|
|
/// Construct shapes |
99 |
|
|
getChildOfType("shape", o); |
100 |
|
|
ShapeFactory* shapes = o->as<ShapeFactory>(); |
101 |
|
|
|
102 |
|
|
const ShapeFactory::OutType& shapeIdxs = shapes->values(); |
103 |
|
|
std::size_t i_shape = 0; |
104 |
|
|
while (i_shape < shapeIdxs.size()) { |
105 |
|
|
if (i_shape + shapeIdxs[i_shape] > shapeIdxs.size()) |
106 |
|
|
throw std::out_of_range( |
107 |
|
|
"shape should be a sequence of unsigned " |
108 |
|
|
"integer: N iPoints_1 ... iPoints_N M iPoints_1 ... iPoints_M"); |
109 |
|
|
Shape_t shape(shapeIdxs[i_shape]); |
110 |
|
|
for (size_t i = 1; i < shapeIdxs[i_shape] + 1; ++i) |
111 |
|
|
shape[i - 1] = points[shapeIdxs[i_shape + i]]; |
112 |
|
|
i_shape += shapeIdxs[i_shape] + 1; |
113 |
|
|
shapes_.push_back(JointAndShape_t(joint, shape)); |
114 |
|
|
} |
115 |
|
|
} catch (const std::invalid_argument& e) { |
116 |
|
|
/// Backward compatibility |
117 |
|
|
try { |
118 |
|
|
getChildOfType("triangle", o); |
119 |
|
|
} catch (const std::invalid_argument& eTri) { |
120 |
|
|
throw std::invalid_argument(e); |
121 |
|
|
} |
122 |
|
|
TriangleFactory* tri = o->as<TriangleFactory>(); |
123 |
|
|
/// Group points by 3 to form triangle |
124 |
|
|
const TriangleFactory::OutType& indexes = tri->values(); |
125 |
|
|
if (indexes.size() % 3 != 0) |
126 |
|
|
throw std::length_error( |
127 |
|
|
"Triangle sequence size should be a multiple of 3."); |
128 |
|
|
if (*std::max_element(indexes.begin(), indexes.end()) >= points.size()) |
129 |
|
|
throw std::out_of_range( |
130 |
|
|
"triangle should be a sequence of unsigned integer lower than the " |
131 |
|
|
"number of points."); |
132 |
|
|
for (size_t i_tri = 0; i_tri < indexes.size(); i_tri += 3) { |
133 |
|
|
/// For each of the point indexes |
134 |
|
|
shapes_.push_back(JointAndShape_t( |
135 |
|
|
joint, {points[indexes[i_tri]], points[indexes[i_tri + 1]], |
136 |
|
|
points[indexes[i_tri + 2]]})); |
137 |
|
|
} |
138 |
|
|
hppDout(info, |
139 |
|
|
"Triangles are deprecated and will be removed." |
140 |
|
|
" Use tag shape instead."); |
141 |
|
|
} |
142 |
|
|
|
143 |
|
|
device->jointAndShapes.add(root()->prependPrefix(name()), shapes_); |
144 |
|
|
} |
145 |
|
|
} // namespace srdf |
146 |
|
|
} // namespace manipulation |
147 |
|
|
} // namespace hpp |