| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
|||
2 |
// Copyright (C) 2018 LAAS-CNRS |
||
3 |
// Author: Pierre Fernbach |
||
4 |
// |
||
5 |
// This file is part of the hpp-rbprm. |
||
6 |
// |
||
7 |
// hpp-rbprm is free software: you can redistribute it and/or modify |
||
8 |
// it under the terms of the GNU Lesser General Public License as published by |
||
9 |
// the Free Software Foundation, either version 3 of the License, or |
||
10 |
// (at your option) any later version. |
||
11 |
// |
||
12 |
// test-hpp is distributed in the hope that it will be useful, |
||
13 |
// but WITHOUT ANY WARRANTY; without even the implied warranty of |
||
14 |
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
15 |
// GNU Lesser General Public License for more details. |
||
16 |
// |
||
17 |
// You should have received a copy of the GNU Lesser General Public License |
||
18 |
// along with hpp-core. If not, see <http://www.gnu.org/licenses/>. |
||
19 |
|||
20 |
#ifndef TOOLSFULLBODY_HH |
||
21 |
#define TOOLSFULLBODY_HH |
||
22 |
#include <hpp/core/fwd.hh> |
||
23 |
#include <hpp/pinocchio/configuration.hh> |
||
24 |
#include <hpp/pinocchio/device.hh> |
||
25 |
#include <hpp/pinocchio/simple-device.hh> |
||
26 |
#include <hpp/pinocchio/urdf/util.hh> |
||
27 |
#include <hpp/rbprm/rbprm-device.hh> |
||
28 |
#include <hpp/rbprm/rbprm-fullbody.hh> |
||
29 |
#include <hpp/rbprm/rbprm-state.hh> |
||
30 |
#include <pinocchio/parsers/urdf.hpp> |
||
31 |
|||
32 |
using namespace hpp; |
||
33 |
using namespace rbprm; |
||
34 |
namespace pin_test = hpp::pinocchio::unittest; |
||
35 |
|||
36 |
/* |
||
37 |
|||
38 |
#~ AFTER loading obstacles |
||
39 |
|||
40 |
rLeg = 'RLEG_JOINT0' |
||
41 |
rLegOffset = [0,0,-0.105] |
||
42 |
rLegLimbOffset=[0,0,-0.035]#0.035 |
||
43 |
rLegNormal = [0,0,1] |
||
44 |
rLegx = 0.09; rLegy = 0.05 |
||
45 |
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward") |
||
46 |
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, |
||
47 |
"fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset) |
||
48 |
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True) |
||
49 |
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db") |
||
50 |
|||
51 |
lLeg = 'LLEG_JOINT0' |
||
52 |
lLegOffset = [0,0,-0.105] |
||
53 |
lLegLimbOffset=[0,0,0.035] |
||
54 |
lLegNormal = [0,0,1] |
||
55 |
lLegx = 0.09; lLegy = 0.05 |
||
56 |
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward") |
||
57 |
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, |
||
58 |
"fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset) |
||
59 |
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True) |
||
60 |
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db") |
||
61 |
fullBody.setReferenceConfig (q_ref) |
||
62 |
|||
63 |
*/ |
||
64 |
|||
65 |
RbPrmFullBodyPtr_t loadSimpleHumanoid() { |
||
66 |
hpp::pinocchio::DevicePtr_t device = |
||
67 |
pin_test::makeDevice(pin_test::HumanoidSimple); |
||
68 |
device->rootJoint()->lowerBound(0, -2); |
||
69 |
device->rootJoint()->lowerBound(1, -2); |
||
70 |
device->rootJoint()->lowerBound(2, 0.4); |
||
71 |
device->rootJoint()->upperBound(0, 2); |
||
72 |
device->rootJoint()->upperBound(1, 2); |
||
73 |
device->rootJoint()->upperBound(2, 1.2); |
||
74 |
|||
75 |
device->setDimensionExtraConfigSpace(6); // used by kinodynamic methods |
||
76 |
for (size_type i = 0; i < 6; ++i) { |
||
77 |
device->extraConfigSpace().lower(i) = -5; |
||
78 |
device->extraConfigSpace().upper(i) = 5; |
||
79 |
} |
||
80 |
|||
81 |
RbPrmFullBodyPtr_t fullBody = RbPrmFullBody::create(device); |
||
82 |
|||
83 |
// add limbs : |
||
84 |
const std::string rLegId("rleg"); |
||
85 |
const std::string rLeg("rleg1_joint"); |
||
86 |
const std::string rfeet("rleg6_joint"); |
||
87 |
fcl::Vec3f rLegOffset(0, 0, -0.1); |
||
88 |
fcl::Vec3f rLegLimbOffset(0, 0, 0); |
||
89 |
fcl::Vec3f rLegNormal(0, 0, 1); |
||
90 |
double legX = 0.1; |
||
91 |
double legY = 0.05; |
||
92 |
fullBody->AddLimb(rLegId, rLeg, rfeet, rLegOffset, rLegLimbOffset, rLegNormal, |
||
93 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, |
||
94 |
"fixedStep1", 0.01, hpp::rbprm::_6_DOF, false, false, |
||
95 |
std::string(), 0.3); |
||
96 |
|||
97 |
const std::string lLegId("lleg"); |
||
98 |
const std::string lLeg("lleg1_joint"); |
||
99 |
const std::string lfeet("lleg6_joint"); |
||
100 |
fcl::Vec3f lLegOffset(0, 0, -0.1); |
||
101 |
fcl::Vec3f lLegLimbOffset(0, 0, 0); |
||
102 |
fcl::Vec3f lLegNormal(0, 0, 1); |
||
103 |
fullBody->AddLimb(lLegId, lLeg, lfeet, lLegOffset, lLegLimbOffset, lLegNormal, |
||
104 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, |
||
105 |
"fixedStep1", 0.01, hpp::rbprm::_6_DOF, false, false, |
||
106 |
std::string(), 0.3); |
||
107 |
|||
108 |
return fullBody; |
||
109 |
} |
||
110 |
|||
111 |
RbPrmFullBodyPtr_t loadHRP2() { |
||
112 |
const std::string robotName("hrp2"); |
||
113 |
const std::string rootJointType("freeflyer"); |
||
114 |
const std::string packageName("hrp2_14_description"); |
||
115 |
const std::string modelName("hrp2_14"); |
||
116 |
const std::string urdfSuffix("_reduced"); |
||
117 |
const std::string srdfSuffix(""); |
||
118 |
|||
119 |
hpp::pinocchio::DevicePtr_t device = |
||
120 |
hpp::pinocchio::Device::create(robotName); |
||
121 |
hpp::pinocchio::urdf::loadRobotModel(device, rootJointType, packageName, |
||
122 |
modelName, urdfSuffix, srdfSuffix); |
||
123 |
device->rootJoint()->lowerBound(0, -2); |
||
124 |
device->rootJoint()->lowerBound(1, -2); |
||
125 |
device->rootJoint()->lowerBound(2, 0.4); |
||
126 |
device->rootJoint()->upperBound(0, 2); |
||
127 |
device->rootJoint()->upperBound(1, 2); |
||
128 |
device->rootJoint()->upperBound(2, 1.2); |
||
129 |
|||
130 |
device->setDimensionExtraConfigSpace(6); // used by kinodynamic methods |
||
131 |
for (size_type i = 0; i < 6; ++i) { |
||
132 |
device->extraConfigSpace().lower(i) = -5; |
||
133 |
device->extraConfigSpace().upper(i) = 5; |
||
134 |
} |
||
135 |
|||
136 |
RbPrmFullBodyPtr_t fullBody = RbPrmFullBody::create(device); |
||
137 |
|||
138 |
core::Configuration_t q_ref(device->configSize()); |
||
139 |
q_ref << 0.1, -0.82, 0.648702, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, |
||
140 |
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, 0.261799388, |
||
141 |
-0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, 0.0, 0.0, -0.453785606, |
||
142 |
0.872664626, -0.41887902, 0.0, 0.0, 0.0, -0.453785606, 0.872664626, |
||
143 |
-0.41887902, 0.0, 0, 0, 0, 0, 0, 0; |
||
144 |
fullBody->referenceConfig(q_ref); |
||
145 |
|||
146 |
// add limbs : |
||
147 |
const std::string rLegId("hrp2_rleg_rom"); |
||
148 |
const std::string rLeg("RLEG_JOINT0"); |
||
149 |
const std::string rfeet("RLEG_JOINT5"); |
||
150 |
fcl::Vec3f rLegOffset(0, 0, -0.105); |
||
151 |
fcl::Vec3f rLegLimbOffset(0, 0, -0.035); |
||
152 |
fcl::Vec3f rLegNormal(0, 0, 1); |
||
153 |
double legX = 0.09; |
||
154 |
double legY = 0.05; |
||
155 |
fullBody->AddLimb(rLegId, rLeg, rfeet, rLegOffset, rLegLimbOffset, rLegNormal, |
||
156 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, |
||
157 |
"fixedStep1", 0.01, hpp::rbprm::_6_DOF, false, false, |
||
158 |
std::string(), 0.3); |
||
159 |
|||
160 |
const std::string lLegId("hrp2_lleg_rom"); |
||
161 |
const std::string lLeg("LLEG_JOINT0"); |
||
162 |
const std::string lfeet("LLEG_JOINT5"); |
||
163 |
fcl::Vec3f lLegOffset(0, 0, -0.105); |
||
164 |
fcl::Vec3f lLegLimbOffset(0, 0, 0.035); |
||
165 |
fcl::Vec3f lLegNormal(0, 0, 1); |
||
166 |
fullBody->AddLimb(lLegId, lLeg, lfeet, lLegOffset, lLegLimbOffset, lLegNormal, |
||
167 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, |
||
168 |
"fixedStep1", 0.01, hpp::rbprm::_6_DOF, false, false, |
||
169 |
std::string(), 0.3); |
||
170 |
|||
171 |
return fullBody; |
||
172 |
} |
||
173 |
|||
174 |
3 |
RbPrmFullBodyPtr_t loadTalos() { |
|
175 |
✓✗ | 6 |
const std::string robotName("talos"); |
176 |
✓✗ | 6 |
const std::string rootJointType("freeflyer"); |
177 |
✓✗ | 6 |
const std::string prefix(""); |
178 |
const std::string urdfPath( |
||
179 |
"package://example-robot-data/robots/talos_data" |
||
180 |
✓✗ | 6 |
"/robots/talos_reduced.urdf"); |
181 |
const std::string srdfPath( |
||
182 |
"package://example-robot-data/robots/talos_data" |
||
183 |
✓✗ | 6 |
"/srdf/talos.srdf"); |
184 |
|||
185 |
hpp::pinocchio::DevicePtr_t device = |
||
186 |
✓✗ | 6 |
hpp::pinocchio::Device::create(robotName); |
187 |
✓✗✓✗ |
3 |
hpp::pinocchio::urdf::loadModel(device, 0, prefix, rootJointType, urdfPath, |
188 |
srdfPath); |
||
189 |
✓✗✓✗ |
3 |
device->rootJoint()->lowerBound(0, -2); |
190 |
✓✗✓✗ |
3 |
device->rootJoint()->lowerBound(1, -2); |
191 |
✓✗✓✗ |
3 |
device->rootJoint()->lowerBound(2, 0.6); |
192 |
✓✗✓✗ |
3 |
device->rootJoint()->upperBound(0, 2); |
193 |
✓✗✓✗ |
3 |
device->rootJoint()->upperBound(1, 2); |
194 |
✓✗✓✗ |
3 |
device->rootJoint()->upperBound(2, 1.3); |
195 |
|||
196 |
✓✗ | 3 |
device->setDimensionExtraConfigSpace(6); // used by kinodynamic methods |
197 |
✓✓ | 21 |
for (size_type i = 0; i < 6; ++i) { |
198 |
✓✗ | 18 |
device->extraConfigSpace().lower(i) = -5; |
199 |
✓✗ | 18 |
device->extraConfigSpace().upper(i) = 5; |
200 |
} |
||
201 |
|||
202 |
✓✗ | 3 |
RbPrmFullBodyPtr_t fullBody = RbPrmFullBody::create(device); |
203 |
|||
204 |
✓✗✓✗ |
6 |
core::Configuration_t q_ref(device->configSize()); |
205 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
q_ref << 0.0, 0.0, 1.02127, 0.0, 0.0, 0.0, 1., 0.0, 0.0, -0.411354, 0.859395, |
206 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3 |
-0.448041, -0.001708, 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, |
207 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
0.0, 0.006761, 0.25847, 0.173046, -0.0002, -0.525366, 0.0, -0.0, 0.1, |
208 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
3 |
-0.005, -0.25847, -0.173046, 0.0002, -0.525366, 0.0, 0.0, 0.1, -0.005, 0., |
209 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
3 |
0., 0., 0., 0., 0., 0., 0.; |
210 |
|||
211 |
✓✗✓✗ |
3 |
fullBody->referenceConfig(q_ref); |
212 |
|||
213 |
// add limbs : |
||
214 |
✓✗ | 6 |
const std::string rLegId("talos_rleg_rom"); |
215 |
✓✗ | 6 |
const std::string rLeg("leg_right_1_joint"); |
216 |
✓✗ | 6 |
const std::string rfeet("leg_right_sole_fix_joint"); |
217 |
✓✗ | 3 |
fcl::Vec3f rLegOffset(0., 0., 0); |
218 |
✓✗ | 3 |
fcl::Vec3f rLegLimbOffset(0., 0., 0); |
219 |
✓✗ | 3 |
fcl::Vec3f rLegNormal(0, 0, 1); |
220 |
3 |
double legX = 0.09; |
|
221 |
3 |
double legY = 0.05; |
|
222 |
✓✗✓✗ |
12 |
fullBody->AddLimb(rLegId, rLeg, rfeet, rLegOffset, rLegLimbOffset, rLegNormal, |
223 |
6 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, |
|
224 |
"fixedStep1", 0.01, hpp::rbprm::_6_DOF, false, false, |
||
225 |
6 |
std::string(), 0.3); |
|
226 |
|||
227 |
✓✗ | 6 |
const std::string lLegId("talos_lleg_rom"); |
228 |
✓✗ | 6 |
const std::string lLeg("leg_left_1_joint"); |
229 |
✓✗ | 6 |
const std::string lfeet("leg_left_sole_fix_joint"); |
230 |
✓✗ | 3 |
fcl::Vec3f lLegOffset(0., 0., 0.0); |
231 |
✓✗ | 3 |
fcl::Vec3f lLegLimbOffset(0., 0., 0); |
232 |
✓✗ | 3 |
fcl::Vec3f lLegNormal(0, 0, 1); |
233 |
✓✗✓✗ |
12 |
fullBody->AddLimb(lLegId, lLeg, lfeet, lLegOffset, lLegLimbOffset, lLegNormal, |
234 |
6 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, |
|
235 |
"fixedStep1", 0.01, hpp::rbprm::_6_DOF, false, false, |
||
236 |
6 |
std::string(), 0.3); |
|
237 |
|||
238 |
6 |
return fullBody; |
|
239 |
} |
||
240 |
|||
241 |
36 |
void loadRom(hpp::pinocchio::T_Rom& romDevices, const std::string romName, |
|
242 |
const std::string packageName) { |
||
243 |
✓✗✓✗ ✓✗ |
144 |
std::string rootJointType("freeflyer"), urdfSuffix(""), srdfSuffix(""); |
244 |
hpp::pinocchio::DevicePtr_t romDevice = |
||
245 |
✓✗ | 72 |
hpp::pinocchio::Device::create(romName); |
246 |
✓✗✓✗ |
36 |
romDevices.insert(std::make_pair(romName, romDevice)); |
247 |
✓✗ | 36 |
hpp::pinocchio::urdf::loadRobotModel(romDevice, rootJointType, packageName, |
248 |
romName, urdfSuffix, srdfSuffix); |
||
249 |
36 |
} |
|
250 |
|||
251 |
15 |
hpp::pinocchio::RbPrmDevicePtr_t loadAbstractRobot( |
|
252 |
hpp::pinocchio::T_Rom& romDevices, const std::string robotName, |
||
253 |
const std::string packageName) { |
||
254 |
✓✗✓✗ ✓✗ |
60 |
std::string rootJointType("freeflyer"), urdfSuffix(""), srdfSuffix(""); |
255 |
hpp::pinocchio::RbPrmDevicePtr_t device = |
||
256 |
✓✗ | 15 |
hpp::pinocchio::RbPrmDevice::create(robotName, romDevices); |
257 |
✓✗ | 15 |
hpp::pinocchio::urdf::loadRobotModel(device, rootJointType, packageName, |
258 |
robotName, urdfSuffix, srdfSuffix); |
||
259 |
15 |
const hpp::pinocchio::Computation_t flag = |
|
260 |
static_cast<hpp::pinocchio::Computation_t>( |
||
261 |
hpp::pinocchio::JOINT_POSITION | hpp::pinocchio::JACOBIAN | |
||
262 |
hpp::pinocchio::COM); |
||
263 |
✓✗ | 15 |
device->controlComputation(flag); |
264 |
30 |
return device; |
|
265 |
} |
||
266 |
|||
267 |
3 |
hpp::pinocchio::RbPrmDevicePtr_t loadHyQAbsract() { |
|
268 |
6 |
hpp::pinocchio::T_Rom romDevices_; |
|
269 |
✓✗ | 6 |
const std::string packageName("hyq-rbprm"); |
270 |
✓✗✓✗ ✓✗ |
3 |
loadRom(romDevices_, std::string("hyq_lhleg_rom"), packageName); |
271 |
✓✗✓✗ ✓✗ |
3 |
loadRom(romDevices_, std::string("hyq_lfleg_rom"), packageName); |
272 |
✓✗✓✗ ✓✗ |
3 |
loadRom(romDevices_, std::string("hyq_rfleg_rom"), packageName); |
273 |
✓✗✓✗ ✓✗ |
3 |
loadRom(romDevices_, std::string("hyq_rhleg_rom"), packageName); |
274 |
hpp::pinocchio::RbPrmDevicePtr_t device = loadAbstractRobot( |
||
275 |
✓✗✓✗ ✓✗ |
6 |
romDevices_, std::string("hyq_trunk_large"), packageName); |
276 |
✓✗✓✗ |
3 |
device->rootJoint()->lowerBound(0, -4); |
277 |
✓✗✓✗ |
3 |
device->rootJoint()->lowerBound(1, -4); |
278 |
✓✗✓✗ |
3 |
device->rootJoint()->lowerBound(2, 0.3); |
279 |
✓✗✓✗ |
3 |
device->rootJoint()->upperBound(0, 5); |
280 |
✓✗✓✗ |
3 |
device->rootJoint()->upperBound(1, 4); |
281 |
✓✗✓✗ |
3 |
device->rootJoint()->upperBound(2, 1); |
282 |
✓✗ | 3 |
hpp::core::vector3_t p_lFLeg(0.3735, 0.207, -0.57697); |
283 |
✓✗ | 3 |
hpp::core::vector3_t p_rFLeg(0.3735, -0.207, -0.57697); |
284 |
✓✗ | 3 |
hpp::core::vector3_t p_lHLeg(-0.3735, 0.207, -0.57697); |
285 |
✓✗ | 3 |
hpp::core::vector3_t p_rHLeg(-0.3735, -0.207, -0.57697); |
286 |
✓✗✓✗ ✓✗ |
3 |
device->setEffectorReference("hyq_lfleg_rom", p_lFLeg); |
287 |
✓✗✓✗ ✓✗ |
3 |
device->setEffectorReference("hyq_rfleg_rom", p_rFLeg); |
288 |
✓✗✓✗ ✓✗ |
3 |
device->setEffectorReference("hyq_lhleg_rom", p_lHLeg); |
289 |
✓✗✓✗ ✓✗ |
3 |
device->setEffectorReference("hyq_rhleg_rom", p_rHLeg); |
290 |
6 |
return device; |
|
291 |
} |
||
292 |
|||
293 |
12 |
hpp::pinocchio::RbPrmDevicePtr_t loadSimpleHumanoidAbsract() { |
|
294 |
24 |
hpp::pinocchio::T_Rom romDevices_; |
|
295 |
✓✗ | 24 |
const std::string packageName("simple-humanoid-rbprm"); |
296 |
✓✗✓✗ ✓✗ |
12 |
loadRom(romDevices_, std::string("simple_humanoid_lleg_rom"), packageName); |
297 |
✓✗✓✗ ✓✗ |
12 |
loadRom(romDevices_, std::string("simple_humanoid_rleg_rom"), packageName); |
298 |
hpp::pinocchio::RbPrmDevicePtr_t device = loadAbstractRobot( |
||
299 |
✓✗✓✗ ✓✗ |
24 |
romDevices_, std::string("simple_humanoid_trunk"), packageName); |
300 |
✓✗✓✗ |
12 |
device->rootJoint()->lowerBound(0, -5); |
301 |
✓✗✓✗ |
12 |
device->rootJoint()->lowerBound(1, -5); |
302 |
✓✗✓✗ |
12 |
device->rootJoint()->lowerBound(2, 0.3); |
303 |
✓✗✓✗ |
12 |
device->rootJoint()->upperBound(0, 5); |
304 |
✓✗✓✗ |
12 |
device->rootJoint()->upperBound(1, 5); |
305 |
✓✗✓✗ |
12 |
device->rootJoint()->upperBound(2, 1); |
306 |
✓✗ | 12 |
hpp::core::vector3_t p_lLeg(0., 0.1, -1.); |
307 |
✓✗ | 12 |
hpp::core::vector3_t p_rLeg(0., -0.1, -1.); |
308 |
✓✗✓✗ ✓✗ |
12 |
device->setEffectorReference("simple_humanoid_lleg_rom", p_lLeg); |
309 |
✓✗✓✗ ✓✗ |
12 |
device->setEffectorReference("simple_humanoid_rleg_rom", p_rLeg); |
310 |
24 |
return device; |
|
311 |
} |
||
312 |
|||
313 |
hpp::pinocchio::RbPrmDevicePtr_t loadTalosLEGAbsract() { |
||
314 |
hpp::pinocchio::T_Rom romDevices_; |
||
315 |
const std::string packageName("talos-rbprm"); |
||
316 |
loadRom(romDevices_, std::string("talos_lleg_rom"), packageName); |
||
317 |
loadRom(romDevices_, std::string("talos_rleg_rom"), packageName); |
||
318 |
// loadRom(romDevices_, std::string("talos_larm_rom"),packageName); |
||
319 |
// loadRom(romDevices_, std::string("talos_rarm_rom"),packageName); |
||
320 |
hpp::pinocchio::RbPrmDevicePtr_t device = |
||
321 |
loadAbstractRobot(romDevices_, std::string("talos_trunk"), packageName); |
||
322 |
device->rootJoint()->lowerBound(0, -5); |
||
323 |
device->rootJoint()->lowerBound(1, -5); |
||
324 |
device->rootJoint()->lowerBound(2, 0.9); |
||
325 |
device->rootJoint()->upperBound(0, 5); |
||
326 |
device->rootJoint()->upperBound(1, 5); |
||
327 |
device->rootJoint()->upperBound(2, 1.1); |
||
328 |
return device; |
||
329 |
} |
||
330 |
|||
331 |
1 |
RbPrmFullBodyPtr_t loadHyQ() { |
|
332 |
✓✗ | 2 |
const std::string robotName("hyq"); |
333 |
✓✗ | 2 |
const std::string rootJointType("freeflyer"); |
334 |
|||
335 |
hpp::pinocchio::DevicePtr_t device = |
||
336 |
✓✗ | 2 |
hpp::pinocchio::Device::create(robotName); |
337 |
|||
338 |
✓✗✓✗ ✓✗✓✗ |
2 |
hpp::pinocchio::urdf::loadModel( |
339 |
✓✗ | 1 |
device, 0, "", rootJointType, |
340 |
"package://example-robot-data/robots/hyq_description/robots/" |
||
341 |
"hyq_no_sensors.urdf", |
||
342 |
"package://example-robot-data/robots/hyq_description/srdf/hyq.srdf"); |
||
343 |
// [-2,5, -1, 1, 0.3, 4] |
||
344 |
|||
345 |
/*DevicePtr_t robot = |
||
346 |
hpp::pinocchio::humanoidSimple("test", true, |
||
347 |
(Device::Computation_t) (Device::JOINT_POSITION | Device::JACOBIAN));*/ |
||
348 |
✓✗✓✗ |
1 |
device->rootJoint()->lowerBound(0, -2); |
349 |
✓✗✓✗ |
1 |
device->rootJoint()->lowerBound(1, -1); |
350 |
✓✗✓✗ |
1 |
device->rootJoint()->lowerBound(2, 0.3); |
351 |
✓✗✓✗ |
1 |
device->rootJoint()->upperBound(0, 5); |
352 |
✓✗✓✗ |
1 |
device->rootJoint()->upperBound(1, 1); |
353 |
✓✗✓✗ |
1 |
device->rootJoint()->upperBound(2, 4); |
354 |
|||
355 |
// device->setDimensionExtraConfigSpace(6); |
||
356 |
|||
357 |
✓✗ | 1 |
RbPrmFullBodyPtr_t fullBody = RbPrmFullBody::create(device); |
358 |
|||
359 |
✓✗✓✗ |
2 |
core::Configuration_t q_ref(device->configSize()); |
360 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
1 |
q_ref << -2.0, 0.0, 0.6838277139631803, 0.0, 0.0, 0.0, 1.0, |
361 |
✓✗✓✗ ✓✗ |
1 |
0.14279812395541294, 0.934392553166556, -0.9968239786882757, |
362 |
✓✗✓✗ ✓✗ |
1 |
-0.06521258938340457, -0.8831796268418511, 1.150049183494211, |
363 |
✓✗✓✗ ✓✗ |
1 |
-0.06927610020154493, 0.9507443168724581, -0.8739975339028809, |
364 |
✓✗✓✗ ✓✗ |
1 |
0.03995660287873871, -0.9577096766517215, 0.93846028213260710; |
365 |
✓✗✓✗ |
1 |
fullBody->referenceConfig(q_ref); |
366 |
|||
367 |
✓✗ | 1 |
const fcl::Vec3f limbOffset(0, 0, 0); |
368 |
✓✗ | 1 |
fcl::Vec3f legOffset(0, 0, -0.021); |
369 |
✓✗ | 1 |
fcl::Vec3f legNormal(0, 0, 1); |
370 |
1 |
double legX = 0.02; |
|
371 |
1 |
double legY = 0.02; |
|
372 |
|||
373 |
// add limbs : |
||
374 |
✓✗ | 2 |
const std::string rLegId("rfleg"); |
375 |
✓✗ | 2 |
const std::string rLeg("rf_haa_joint"); |
376 |
✓✗ | 2 |
const std::string rfeet("rf_foot_joint"); |
377 |
✓✗✓✗ |
4 |
fullBody->AddLimb(rLegId, rLeg, rfeet, legOffset, limbOffset, legNormal, legX, |
378 |
2 |
legY, hpp::core::ObjectStdVector_t(), 1000, "random", 0.1, |
|
379 |
2 |
hpp::rbprm::_3_DOF, false, false, std::string(), 0.3); |
|
380 |
|||
381 |
✓✗ | 2 |
const std::string lLegId("lfleg"); |
382 |
✓✗ | 2 |
const std::string lLeg("lf_haa_joint"); |
383 |
✓✗ | 2 |
const std::string lfeet("lf_foot_joint"); |
384 |
✓✗✓✗ |
4 |
fullBody->AddLimb(lLegId, lLeg, lfeet, legOffset, limbOffset, legNormal, legX, |
385 |
2 |
legY, hpp::core::ObjectStdVector_t(), 1000, "random", 0.1, |
|
386 |
2 |
hpp::rbprm::_3_DOF, false, false, std::string(), 0.3); |
|
387 |
|||
388 |
✓✗ | 2 |
const std::string rhLegId("rhleg"); |
389 |
✓✗ | 2 |
const std::string rhLeg("rh_haa_joint"); |
390 |
✓✗ | 2 |
const std::string rhfeet("rh_foot_joint"); |
391 |
✓✗✓✗ |
4 |
fullBody->AddLimb(rhLegId, rhLeg, rhfeet, legOffset, limbOffset, legNormal, |
392 |
2 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, "random", |
|
393 |
2 |
0.1, hpp::rbprm::_3_DOF, false, false, std::string(), 0.3); |
|
394 |
|||
395 |
✓✗ | 2 |
const std::string lhLegId("lhleg"); |
396 |
✓✗ | 2 |
const std::string lhLeg("lh_haa_joint"); |
397 |
✓✗ | 2 |
const std::string lhfeet("lh_foot_joint"); |
398 |
✓✗✓✗ |
4 |
fullBody->AddLimb(lhLegId, lhLeg, lhfeet, legOffset, limbOffset, legNormal, |
399 |
2 |
legX, legY, hpp::core::ObjectStdVector_t(), 1000, "random", |
|
400 |
2 |
0.1, hpp::rbprm::_3_DOF, false, false, std::string(), 0.3); |
|
401 |
|||
402 |
✓✗✓✗ |
1 |
fullBody->device_->currentConfiguration(q_ref); |
403 |
|||
404 |
2 |
return fullBody; |
|
405 |
} |
||
406 |
|||
407 |
19 |
State createState(const RbPrmFullBodyPtr_t& fullBody, |
|
408 |
core::Configuration_t config, |
||
409 |
const std::vector<std::string>& limbsInContact) { |
||
410 |
✓✗✓✗ |
19 |
fullBody->device_->currentConfiguration(config); |
411 |
19 |
hpp::pinocchio::Computation_t newflag = |
|
412 |
static_cast<hpp::pinocchio::Computation_t>( |
||
413 |
hpp::pinocchio::JOINT_POSITION | hpp::pinocchio::JACOBIAN | |
||
414 |
hpp::pinocchio::COM); |
||
415 |
✓✗ | 19 |
fullBody->device_->controlComputation(newflag); |
416 |
✓✗ | 19 |
fullBody->device_->computeForwardKinematics(); |
417 |
✓✗ | 19 |
State state; |
418 |
✓✗ | 19 |
state.configuration_ = config; |
419 |
59 |
for (std::vector<std::string>::const_iterator cit = limbsInContact.begin(); |
|
420 |
✓✓ | 99 |
cit != limbsInContact.end(); ++cit) { |
421 |
✓✗ | 80 |
rbprm::RbPrmLimbPtr_t limb = fullBody->GetLimbs().at(*cit); |
422 |
40 |
const std::string& limbName = *cit; |
|
423 |
✓✗ | 40 |
state.contacts_[limbName] = true; |
424 |
const fcl::Vec3f position = |
||
425 |
✓✗✓✗ ✓✗ |
40 |
limb->effector_.currentTransformation().translation(); |
426 |
✓✗✓✗ |
40 |
state.contactPositions_[limbName] = position; |
427 |
✓✗ | 40 |
state.contactNormals_[limbName] = |
428 |
✓✗✓✗ ✓✗✓✗ |
80 |
limb->effector_.currentTransformation().rotation() * limb->normal_; |
429 |
80 |
state.contactRotation_[limbName] = |
|
430 |
✓✗✓✗ ✓✗✓✗ |
40 |
limb->effector_.currentTransformation().rotation(); |
431 |
✓✗ | 40 |
state.contactOrder_.push(limbName); |
432 |
} |
||
433 |
19 |
state.nbContacts = state.contactNormals_.size(); |
|
434 |
|||
435 |
38 |
return state; |
|
436 |
} |
||
437 |
|||
438 |
// assume all limbs are in contact |
||
439 |
18 |
State createState(const RbPrmFullBodyPtr_t& fullBody, |
|
440 |
core::Configuration_t config) { |
||
441 |
18 |
std::vector<std::string> limbsInContact; |
|
442 |
54 |
for (rbprm::CIT_Limb lit = fullBody->GetLimbs().begin(); |
|
443 |
✓✓ | 90 |
lit != fullBody->GetLimbs().end(); ++lit) { |
444 |
✓✗ | 36 |
limbsInContact.push_back(lit->first); |
445 |
} |
||
446 |
✓✗✓✗ |
36 |
return createState(fullBody, config, limbsInContact); |
447 |
} |
||
448 |
|||
449 |
#endif // TOOLSFULLBODY_HH |
| Generated by: GCOVR (Version 4.2) |