| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// Copyright (c) 2014, LAAS-CNRS |
||
2 |
// Authors: Steve Tonneau (steve.tonneau@laas.fr) |
||
3 |
// |
||
4 |
// This file is part of hpp-rbprm. |
||
5 |
// hpp-rbprm 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 |
// hpp-rbprm 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 |
// hpp-rbprm. If not, see <http://www.gnu.org/licenses/>. |
||
16 |
|||
17 |
#include <hpp/pinocchio/joint.hh> |
||
18 |
#include <hpp/rbprm/contact_generation/kinematics_constraints.hh> |
||
19 |
#include <hpp/rbprm/rbprm-limb.hh> |
||
20 |
#include <hpp/rbprm/sampling/sample-db.hh> |
||
21 |
#include <hpp/rbprm/tools.hh> |
||
22 |
#include <pinocchio/spatial/se3.hpp> |
||
23 |
namespace hpp { |
||
24 |
namespace rbprm { |
||
25 |
|||
26 |
10 |
RbPrmLimbPtr_t RbPrmLimb::create( |
|
27 |
const pinocchio::JointPtr_t limb, const std::string& effectorName, |
||
28 |
const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset, |
||
29 |
const fcl::Vec3f& normal, const double x, const double y, |
||
30 |
const std::size_t nbSamples, const hpp::rbprm::sampling::heuristic evaluate, |
||
31 |
const double resolution, hpp::rbprm::ContactType contactType, |
||
32 |
const bool disableEffectorCollision, const bool grasp, |
||
33 |
const std::string& kinematicsConstraintsPath, |
||
34 |
const double kinematicConstraintsMinDistance) { |
||
35 |
RbPrmLimb* rbprmDevice = new RbPrmLimb( |
||
36 |
limb, effectorName, offset, limbOffset, normal, x, y, nbSamples, evaluate, |
||
37 |
resolution, contactType, disableEffectorCollision, grasp, |
||
38 |
✓✗ | 10 |
kinematicsConstraintsPath, kinematicConstraintsMinDistance); |
39 |
10 |
RbPrmLimbPtr_t res(rbprmDevice); |
|
40 |
✓✗ | 10 |
res->init(res); |
41 |
10 |
return res; |
|
42 |
} |
||
43 |
|||
44 |
RbPrmLimbPtr_t RbPrmLimb::create(const pinocchio::DevicePtr_t device, |
||
45 |
std::ifstream& fileStream, |
||
46 |
const bool loadValues, |
||
47 |
const hpp::rbprm::sampling::heuristic evaluate, |
||
48 |
const bool disableEffectorCollision, |
||
49 |
const bool grasp) { |
||
50 |
RbPrmLimb* rbprmDevice = |
||
51 |
new RbPrmLimb(device, fileStream, loadValues, evaluate, |
||
52 |
disableEffectorCollision, grasp); |
||
53 |
RbPrmLimbPtr_t res(rbprmDevice); |
||
54 |
res->init(res); |
||
55 |
return res; |
||
56 |
} |
||
57 |
|||
58 |
10 |
RbPrmLimb::~RbPrmLimb() { |
|
59 |
// NOTHING |
||
60 |
10 |
} |
|
61 |
|||
62 |
// ======================================================================== |
||
63 |
|||
64 |
10 |
void RbPrmLimb::init(const RbPrmLimbWkPtr_t& weakPtr) { weakPtr_ = weakPtr; } |
|
65 |
|||
66 |
20 |
pinocchio::Frame GetEffector(const pinocchio::JointPtr_t limb, |
|
67 |
const std::string name = "") { |
||
68 |
✓✗✓✗ ✓✗ |
40 |
if (name != "") return limb->robot()->getFrameByName(name); |
69 |
pinocchio::JointPtr_t current = limb; |
||
70 |
while (current->numberChildJoints() != 0) { |
||
71 |
// assert(current->numberChildJoints() ==1); |
||
72 |
current = current->childJoint(0); |
||
73 |
} |
||
74 |
return limb->robot()->getFrameByName(current->name()); |
||
75 |
} |
||
76 |
|||
77 |
10 |
fcl::Matrix3f GetEffectorTransform(const pinocchio::Frame& effector) { |
|
78 |
✓✗ | 20 |
pinocchio::Frame ef(effector); |
79 |
✓✗✓✗ ✓✗ |
10 |
pinocchio::Configuration_t save = ef.robot()->currentConfiguration(); |
80 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
ef.robot()->currentConfiguration(ef.robot()->neutralConfiguration()); |
81 |
✓✗✓✗ |
10 |
const pinocchio::matrix3_t& rot(ef.currentTransformation().rotation()); |
82 |
✓✗✓✗ ✓✗ |
10 |
ef.robot()->currentConfiguration(save); |
83 |
✓✗✓✗ |
30 |
return rot.transpose(); |
84 |
} |
||
85 |
|||
86 |
10 |
fcl::Vec3f computeEffectorReferencePosition(const pinocchio::JointPtr_t& limb, |
|
87 |
const std::string& effectorName) { |
||
88 |
20 |
core::DevicePtr_t device = limb->robot(); |
|
89 |
✓✗ | 20 |
core::Configuration_t referenceConfig = device->neutralConfiguration(); |
90 |
✓✗ | 10 |
pinocchio::Transform3f tRoot(1); |
91 |
✓✗✓✗ |
10 |
pinocchio::Transform3f tJoint_world(1), tJoint_robot(1); |
92 |
✓✗✓✗ ✓✗ |
10 |
tRoot.translation(fcl::Vec3f(referenceConfig.head<3>())); |
93 |
// fcl::Quaternion3f |
||
94 |
// quatRoot(referenceConfig[6],referenceConfig[3],referenceConfig[4],referenceConfig[5]); |
||
95 |
✓✗✓✗ ✓✗✓✗ |
10 |
tRoot.rotation(Eigen::Quaterniond(referenceConfig.segment<4>(3)).matrix()); |
96 |
hppDout(notice, "Create limb, reference root transform : " << tRoot); |
||
97 |
// retrieve transform of each effector joint |
||
98 |
✓✗✓✗ |
10 |
device->currentConfiguration(referenceConfig); |
99 |
✓✗ | 10 |
device->computeForwardKinematics(); |
100 |
✓✗✓✗ ✓✗ |
10 |
tJoint_world = GetEffector(limb, effectorName).currentTransformation(); |
101 |
hppDout(notice, "tJoint of " << limb->name() << " : " << tJoint_world); |
||
102 |
// tJoint_robot = tRoot.inverseTimes(tJoint_world); |
||
103 |
✓✗✓✗ |
10 |
tJoint_robot = tRoot.inverse() * tJoint_world; |
104 |
hppDout(notice, "tJoint relative : " << tJoint_robot); |
||
105 |
✓✗✓✗ |
20 |
return tJoint_robot.translation(); |
106 |
} |
||
107 |
|||
108 |
10 |
RbPrmLimb::RbPrmLimb(const pinocchio::JointPtr_t& limb, |
|
109 |
const std::string& effectorName, const fcl::Vec3f& offset, |
||
110 |
const fcl::Vec3f& limbOffset, const fcl::Vec3f& normal, |
||
111 |
const double x, const double y, |
||
112 |
const std::size_t nbSamples, |
||
113 |
const hpp::rbprm::sampling::heuristic evaluate, |
||
114 |
const double resolution, ContactType contactType, |
||
115 |
bool disableEndEffectorCollision, bool grasps, |
||
116 |
const std::string& kinematicsConstraintsPath, |
||
117 |
10 |
const double kinematicConstraintsMinDistance) |
|
118 |
: limb_(limb), |
||
119 |
effector_(GetEffector(limb, effectorName)), |
||
120 |
10 |
effectorDefaultRotation_(GetEffectorTransform(effector_)), |
|
121 |
offset_(effectorDefaultRotation_ * offset), |
||
122 |
limbOffset_(limbOffset), |
||
123 |
normal_(effectorDefaultRotation_ * normal) |
||
124 |
//, normal_(normal) |
||
125 |
, |
||
126 |
x_(x), |
||
127 |
y_(y), |
||
128 |
contactType_(contactType), |
||
129 |
evaluate_(evaluate), |
||
130 |
sampleContainer_(limb, effector_.name(), nbSamples, offset, limbOffset, |
||
131 |
resolution), |
||
132 |
disableEndEffectorCollision_(disableEndEffectorCollision), |
||
133 |
grasps_(grasps), |
||
134 |
effectorReferencePosition_( |
||
135 |
computeEffectorReferencePosition(limb, effectorName)), |
||
136 |
✓✗ | 20 |
kinematicConstraints_(reachability::loadConstraintsFromObj( |
137 |
10 |
kinematicsConstraintsPath.empty() |
|
138 |
✓✗✓✗ ✓✗✓✗ ✗✗✓✗ ✓✗✗✗ ✗✗✗✗ |
50 |
? ("package://" + limb_->robot()->name() + |
139 |
✓✗✓✗ ✓✗✓✗ ✗✗ |
30 |
"-rbprm/com_inequalities/" + limb_->name() + |
140 |
"_com_constraints.obj") |
||
141 |
: kinematicsConstraintsPath, |
||
142 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
30 |
kinematicConstraintsMinDistance)) { |
143 |
// NOTHING |
||
144 |
hppDout(notice, "Create limb, normal = " << normal); |
||
145 |
hppDout(notice, "effector default rotation = " << effectorDefaultRotation_); |
||
146 |
hppDout(notice, "nex normal : " << normal_); |
||
147 |
10 |
} |
|
148 |
|||
149 |
pinocchio::Transform3f RbPrmLimb::octreeRoot() const { |
||
150 |
if (limb_->parentJoint()) |
||
151 |
return limb_->parentJoint()->currentTransformation(); |
||
152 |
return limb_->currentTransformation(); |
||
153 |
} |
||
154 |
|||
155 |
bool saveLimbInfoAndDatabase(const hpp::rbprm::RbPrmLimbPtr_t limb, |
||
156 |
std::ofstream& fp) { |
||
157 |
fp << limb->limb_->name() << std::endl; |
||
158 |
fp << limb->effector_.name() << std::endl; |
||
159 |
tools::io::writeRotMatrixFCL(limb->effectorDefaultRotation_, fp); |
||
160 |
fp << std::endl; |
||
161 |
tools::io::writeVecFCL(limb->offset_, fp); |
||
162 |
fp << std::endl; |
||
163 |
tools::io::writeVecFCL(limb->normal_, fp); |
||
164 |
fp << std::endl; |
||
165 |
fp << limb->x_ << std::endl; |
||
166 |
fp << limb->y_ << std::endl; |
||
167 |
fp << (int)limb->contactType_ << std::endl; |
||
168 |
return sampling::saveLimbDatabase(limb->sampleContainer_, fp); |
||
169 |
} |
||
170 |
} // namespace rbprm |
||
171 |
|||
172 |
namespace tools { |
||
173 |
namespace io { |
||
174 |
hpp::pinocchio::JointPtr_t extractJoint( |
||
175 |
const hpp::pinocchio::DevicePtr_t device, std::ifstream& myfile) { |
||
176 |
std::string name; |
||
177 |
getline(myfile, name); |
||
178 |
return device->getJointByName(name); |
||
179 |
} |
||
180 |
|||
181 |
hpp::pinocchio::Frame extractFrame(const hpp::pinocchio::DevicePtr_t device, |
||
182 |
std::ifstream& myfile) { |
||
183 |
std::string name; |
||
184 |
getline(myfile, name); |
||
185 |
return device->getFrameByName(name); |
||
186 |
} |
||
187 |
|||
188 |
std::ostream& operator<<(std::ostream& out, hpp::rbprm::ContactType ctype) { |
||
189 |
unsigned u = ctype; |
||
190 |
out << u; |
||
191 |
return out; |
||
192 |
} |
||
193 |
} // namespace io |
||
194 |
} // namespace tools |
||
195 |
using namespace hpp::tools::io; |
||
196 |
|||
197 |
hpp::rbprm::RbPrmLimb::RbPrmLimb(const pinocchio::DevicePtr_t device, |
||
198 |
std::ifstream& fileStream, |
||
199 |
const bool loadValues, |
||
200 |
const hpp::rbprm::sampling::heuristic evaluate, |
||
201 |
bool disableEndEffectorCollision, bool grasps) |
||
202 |
: limb_(extractJoint(device, fileStream)), |
||
203 |
effector_(extractFrame(device, fileStream)), |
||
204 |
effectorDefaultRotation_(tools::io::readRotMatrixFCL(fileStream)), |
||
205 |
offset_(readVecFCL(fileStream)), |
||
206 |
normal_(readVecFCL(fileStream)), |
||
207 |
x_(StrToD(fileStream)), |
||
208 |
y_(StrToD(fileStream)), |
||
209 |
contactType_(static_cast<hpp::rbprm::ContactType>(StrToI(fileStream))), |
||
210 |
evaluate_(evaluate), |
||
211 |
sampleContainer_(fileStream, loadValues), |
||
212 |
disableEndEffectorCollision_(disableEndEffectorCollision), |
||
213 |
grasps_(grasps), |
||
214 |
effectorReferencePosition_( |
||
215 |
computeEffectorReferencePosition(limb_, effector_.name())), |
||
216 |
kinematicConstraints_(reachability::loadConstraintsFromObj( |
||
217 |
"package://" + limb_->robot()->name() + "-rbprm/com_inequalities/" + |
||
218 |
limb_->name() + "_com_constraints.obj", |
||
219 |
0.3)) { |
||
220 |
// NOTHING |
||
221 |
} |
||
222 |
} // namespace hpp |
| Generated by: GCOVR (Version 4.2) |