Line data Source code
1 : /*
2 : * Copyright 2020 <copyright holder> <email>
3 : *
4 : * Licensed under the Apache License, Version 2.0 (the "License");
5 : * you may not use this file except in compliance with the License.
6 : * You may obtain a copy of the License at
7 : *
8 : * http://www.apache.org/licenses/LICENSE-2.0
9 : *
10 : * Unless required by applicable law or agreed to in writing, software
11 : * distributed under the License is distributed on an "AS IS" BASIS,
12 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 : * See the License for the specific language governing permissions and
14 : * limitations under the License.
15 : */
16 :
17 : #include <ros_end_effector/ParserMoveIt.h>
18 :
19 144 : ROSEE::ParserMoveIt::ParserMoveIt() {
20 :
21 144 : }
22 :
23 144 : ROSEE::ParserMoveIt::~ParserMoveIt() {
24 :
25 144 : }
26 :
27 144 : bool ROSEE::ParserMoveIt::init ( std::string robot_description, bool verbose ) {
28 :
29 144 : if (robot_model != nullptr ) {
30 0 : std::cerr << "[PARSER::" << __func__ << "]: init() already called by someone " << std::endl;;
31 0 : return false;
32 : }
33 : // it is a ros param in the launch, take care that also sdrf is read by robot_mo
34 : // (param for srd is robot_description_semantic)
35 144 : this->robot_description = robot_description;
36 :
37 : //false: we dont need kinematic solvers now
38 288 : robot_model_loader::RobotModelLoader robot_model_loader(robot_description, false) ;
39 144 : robot_model = robot_model_loader.getModel() ;
40 144 : if (robot_model == nullptr) {
41 : std::cerr << " [PARSER::" << __func__ <<
42 : "]: Fail To load robot model " << robot_description <<
43 : " are you sure to have put both urdf and srdf files in the parameter server " <<
44 0 : "with names robot_description and robot_description_semantic, respectively?" << std::endl;
45 0 : return false;
46 : }
47 144 : std::cout << "[PARSER::" << __func__ << "]: Parsed Model: " << robot_model->getName() << std::endl; ;
48 :
49 144 : handName = robot_model->getName();
50 :
51 144 : lookForFingertips(verbose);
52 144 : lookForActiveJoints();
53 144 : lookForPassiveJoints();
54 144 : lookForDescendants();
55 144 : lookJointsTipsCorrelation();
56 :
57 144 : return true;
58 :
59 : }
60 :
61 92 : std::string ROSEE::ParserMoveIt::getHandName() const {
62 92 : return handName;
63 : }
64 :
65 216 : std::vector<std::string> ROSEE::ParserMoveIt::getFingertipNames() const {
66 216 : return fingertipNames;
67 : }
68 :
69 0 : std::vector<std::string> ROSEE::ParserMoveIt::getActiveJointNames() const {
70 0 : return activeJointNames;
71 : }
72 :
73 99696 : std::vector<const moveit::core::JointModel*> ROSEE::ParserMoveIt::getActiveJointModels() const {
74 99696 : return activeJointModels;
75 : }
76 :
77 777000 : std::vector<std::string> ROSEE::ParserMoveIt::getPassiveJointNames() const {
78 777000 : return passiveJointNames;
79 : }
80 :
81 0 : std::map <std::string, std::vector < const moveit::core::LinkModel* > > ROSEE::ParserMoveIt::getDescendantLinksOfJoint() const {
82 0 : return descendantLinksOfJoint;
83 : }
84 :
85 0 : std::map <std::string, std::vector < const moveit::core::JointModel* > > ROSEE::ParserMoveIt::getDescendantJointsOfJoint() const {
86 0 : return descendantJointsOfJoint;
87 : }
88 :
89 6 : unsigned int ROSEE::ParserMoveIt::getNFingers () const {
90 6 : return nFingers;
91 : }
92 :
93 305 : const robot_model::RobotModelPtr ROSEE::ParserMoveIt::getRobotModel () const {
94 305 : return robot_model;
95 : }
96 :
97 186850 : std::map<std::string, std::vector<std::string> > ROSEE::ParserMoveIt::getFingertipsOfJointMap() const {
98 186850 : return fingertipsOfJointMap;
99 : }
100 :
101 136 : std::map<std::string, std::vector<std::string> > ROSEE::ParserMoveIt::getJointsOfFingertipMap() const {
102 136 : return jointsOfFingertipMap;
103 : }
104 :
105 0 : std::map < std::string, std::string> ROSEE::ParserMoveIt::getFingerOfFingertipMap() const {
106 0 : return fingerOfFingertipMap;
107 : }
108 :
109 52451 : std::string ROSEE::ParserMoveIt::getFingerOfFingertip (std::string tipName) const {
110 :
111 52451 : auto it = fingerOfFingertipMap.find(tipName);
112 :
113 52451 : if (it != fingerOfFingertipMap.end() ) {
114 52451 : return (it->second);
115 :
116 : } else {
117 0 : return "";
118 : }
119 : }
120 :
121 238 : std::map < std::string, std::string> ROSEE::ParserMoveIt::getFingertipOfFingerMap() const {
122 238 : return fingertipOfFingerMap;
123 : }
124 :
125 170272 : std::string ROSEE::ParserMoveIt::getFingertipOfFinger (std::string fingerName) const {
126 :
127 170272 : auto it = fingertipOfFingerMap.find(fingerName);
128 :
129 170272 : if (it != fingertipOfFingerMap.end() ) {
130 170272 : return (it->second);
131 :
132 : } else {
133 0 : return "";
134 : }
135 : }
136 :
137 0 : std::pair<std::string, std::string> ROSEE::ParserMoveIt::getMimicNLFatherOfJoint(std::string mimicNLJointName) const {
138 :
139 0 : std::pair<std::string, std::string> retPair;
140 :
141 0 : auto it = mimicNLFatherOfJointMap.find(mimicNLJointName);
142 :
143 0 : if (it != mimicNLFatherOfJointMap.end()) {
144 0 : retPair = it->second;
145 : }
146 0 : return retPair;
147 : }
148 :
149 144 : std::map<std::string, std::pair<std::string, std::string>> ROSEE::ParserMoveIt::getMimicNLFatherOfJointMap() const {
150 :
151 144 : return mimicNLFatherOfJointMap;
152 :
153 : }
154 :
155 0 : std::string ROSEE::ParserMoveIt::getMimicNLJointOfFather(std::string mimicNLFatherName, std::string mimicNLJointName) const {
156 :
157 0 : auto map = getMimicNLJointsOfFather(mimicNLFatherName);
158 :
159 0 : auto it = map.find(mimicNLJointName);
160 :
161 0 : if (it != map.end()) {
162 0 : return it->second;
163 : }
164 :
165 0 : return "";
166 : }
167 :
168 0 : std::map<std::string, std::string> ROSEE::ParserMoveIt::getMimicNLJointsOfFather(std::string mimicNLFatherName) const {
169 :
170 0 : std::map<std::string, std::string> map;
171 :
172 0 : auto it = mimicNLJointsOfFatherMap.find(mimicNLFatherName);
173 :
174 0 : if (it != mimicNLJointsOfFatherMap.end()) {
175 0 : map = it->second;
176 : }
177 0 : return map;
178 : }
179 :
180 0 : std::map<std::string, std::map<std::string, std::string>> ROSEE::ParserMoveIt::getMimicNLJointsOfFatherMap() const {
181 :
182 0 : return mimicNLJointsOfFatherMap;
183 :
184 : }
185 :
186 :
187 68 : robot_model::RobotModelPtr ROSEE::ParserMoveIt::getCopyModel() const {
188 136 : robot_model_loader::RobotModelLoader robot_model_loader(robot_description);
189 136 : return robot_model_loader.getModel();
190 : }
191 :
192 0 : std::vector < std::string > ROSEE::ParserMoveIt::getGroupOfLink ( std::string linkName ) {
193 :
194 0 : std::vector < std::string > groupsReturn;
195 :
196 0 : if (robot_model == nullptr) {
197 : std::cerr << " [PARSER::" << __func__ <<
198 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
199 0 : return groupsReturn;
200 : }
201 :
202 0 : for (auto group : robot_model->getJointModelGroups() ) {
203 :
204 0 : if ( group->hasLinkModel(linkName) ) {
205 :
206 0 : groupsReturn.push_back ( group->getName() ) ;
207 : }
208 : }
209 0 : return groupsReturn;
210 : }
211 :
212 0 : bool ROSEE::ParserMoveIt::groupIsChain(const std::string groupName) const {
213 :
214 0 : if (robot_model == nullptr) {
215 : std::cerr << " [PARSER::" << __func__ <<
216 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
217 0 : return false;
218 : }
219 :
220 0 : if (! robot_model->hasJointModelGroup(groupName) ) {
221 : std::cerr << " [PARSER::" << __func__ <<
222 0 : "]: " << groupName << " is not a group " << std::endl;
223 0 : return false;
224 : }
225 0 : return groupIsChain(robot_model->getJointModelGroup(groupName));
226 : }
227 :
228 360 : bool ROSEE::ParserMoveIt::groupIsChain(const moveit::core::JointModelGroup* group) const {
229 :
230 720 : std::stringstream log;
231 360 : log << "Checking if " << group->getName() << " is a chain ..." << std::endl;
232 1188 : for (auto link : group->getLinkModels() ){
233 828 : if (link->getChildJointModels().size() > 1 ) {
234 0 : log << "... no because " << link->getName() << " has " << link->getChildJointModels().size() << " children " << std::endl;
235 : // std::cout << log.str() << std::endl;
236 0 : return false;
237 : }
238 : }
239 360 : return true;
240 : }
241 :
242 :
243 :
244 663 : bool ROSEE::ParserMoveIt::checkIfContinuosJoint ( std::string jointName) const {
245 663 : if (robot_model == nullptr) {
246 : std::cerr << " [PARSER::" << __func__ <<
247 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
248 0 : return false;
249 : }
250 663 : return (ROSEE::ParserMoveIt::checkIfContinuosJoint(robot_model->getJointModel(jointName)));
251 : }
252 :
253 906 : bool ROSEE::ParserMoveIt::checkIfContinuosJoint ( const moveit::core::JointModel* joint ) const {
254 906 : if (robot_model == nullptr) {
255 : std::cerr << " [PARSER::" << __func__ <<
256 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
257 0 : return false;
258 : }
259 :
260 : // for moveit, a continuos joint is a revolute joint
261 906 : if (joint->getType() != moveit::core::JointModel::REVOLUTE ) {
262 0 : return false;
263 : }
264 :
265 : //if revolute, only one limit (at.(0))
266 1812 : moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
267 906 : if ( limits.at(0).max_position_ - limits.at(0).min_position_ >= (2*EIGEN_PI) ) {
268 0 : return true;
269 : }
270 :
271 906 : return false;
272 : }
273 :
274 443 : std::vector <double> ROSEE::ParserMoveIt::getBiggerBoundFromZero ( std::string jointName ) const {
275 443 : if (robot_model == nullptr) {
276 : std::cerr << " [PARSER::" << __func__ <<
277 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
278 0 : return std::vector<double>();
279 : }
280 443 : return ( ROSEE::ParserMoveIt::getBiggerBoundFromZero (robot_model->getJointModel(jointName) ) );
281 :
282 : }
283 :
284 443 : std::vector <double> ROSEE::ParserMoveIt::getBiggerBoundFromZero ( const moveit::core::JointModel* joint ) const {
285 443 : if (robot_model == nullptr) {
286 : std::cerr << " [PARSER::" << __func__ <<
287 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
288 0 : return std::vector<double>();
289 : }
290 :
291 886 : moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
292 :
293 886 : std::vector <double> maxPos;
294 886 : for ( auto limit : limits ) {
295 443 : if ( std::abs(limit.max_position_) > std::abs(limit.min_position_)) {
296 144 : maxPos.push_back ( limit.max_position_ ) ;
297 : } else {
298 299 : maxPos.push_back ( limit.min_position_ ) ;
299 : }
300 : }
301 443 : return maxPos;
302 : }
303 :
304 1 : std::vector <double> ROSEE::ParserMoveIt::getSmallerBoundFromZero ( std::string jointName ) const {
305 1 : if (robot_model == nullptr) {
306 : std::cerr << " [PARSER::" << __func__ <<
307 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
308 0 : return std::vector<double>();
309 : }
310 1 : return ( ROSEE::ParserMoveIt::getSmallerBoundFromZero (robot_model->getJointModel(jointName) ) );
311 :
312 : }
313 :
314 1 : std::vector <double> ROSEE::ParserMoveIt::getSmallerBoundFromZero ( const moveit::core::JointModel* joint ) const {
315 1 : if (robot_model == nullptr) {
316 : std::cerr << " [PARSER::" << __func__ <<
317 0 : "]: robot_model is null. Have you called init() before?" << std::endl;
318 0 : return std::vector<double>();
319 : }
320 :
321 2 : moveit::core::JointModel::Bounds limits = joint->getVariableBounds();
322 :
323 2 : std::vector <double> minPos;
324 2 : for ( auto limit : limits ) {
325 1 : if ( std::abs(limit.max_position_) < std::abs(limit.min_position_)) {
326 0 : minPos.push_back ( limit.max_position_ ) ;
327 : } else {
328 1 : minPos.push_back ( limit.min_position_ ) ;
329 : }
330 : }
331 1 : return minPos;
332 : }
333 :
334 180 : unsigned int ROSEE::ParserMoveIt::getNExclusiveJointsOfTip ( std::string tipName, bool continuosIncluded ) const {
335 :
336 180 : if (jointsOfFingertipMap.size() == 0) {
337 : std::cerr << " [PARSER::" << __func__ <<
338 0 : "]: jointsOfFingertipMap empty. Have you called init() before? Also check your URDF and SRDF files"
339 0 : << std::endl;
340 0 : return 0;
341 : }
342 :
343 180 : if (fingertipsOfJointMap.size() == 0) {
344 : std::cerr << " [PARSER::" << __func__ <<
345 0 : "]: jointsOfFingertipMap empty. Have you called init() before? Also check your URDF and SRDF files"
346 0 : << std::endl;
347 0 : return 0;
348 : }
349 :
350 180 : unsigned int nExclusiveJoints = 0;
351 :
352 558 : for (auto jointOfTip : jointsOfFingertipMap.find(tipName)->second ) {
353 :
354 378 : if ( !continuosIncluded && checkIfContinuosJoint(jointOfTip) == true ) {
355 0 : continue; //we dont want to count a continuos joint
356 : }
357 :
358 : //check if the joints of the tip move only that tip
359 378 : if ( fingertipsOfJointMap.find(jointOfTip)->second.size() == 1 ) {
360 :
361 342 : if (fingertipsOfJointMap.find(jointOfTip)->second.at(0).compare (tipName) != 0) {
362 : // this condition is false if jointsOfFingertipMap and fingertipsOfJointMap are built well
363 : std::cerr << " [PARSER::" << __func__ <<
364 : "]: Strange error in fingertipsOfJointMap and jointsOfFingertipMap: they are not consistent: "
365 : << "The unique tip present in the map for the key " << jointOfTip
366 0 : << " is " << fingertipsOfJointMap.find(jointOfTip)->second.at(0)
367 0 : << " and not " << tipName << " as it should be"
368 0 : << std::endl;
369 0 : return 0;
370 : }
371 :
372 342 : nExclusiveJoints++;
373 : }
374 : }
375 180 : return nExclusiveJoints;
376 : }
377 :
378 :
379 72 : std::string ROSEE::ParserMoveIt::getFirstActuatedParentJoint ( std::string linkName, bool includeContinuos ) const {
380 :
381 72 : const moveit::core::LinkModel* linkModel = robot_model->getLinkModel ( linkName ) ;
382 :
383 144 : while ( linkModel->getParentJointModel()->getMimic() != NULL ||
384 144 : linkModel->parentJointIsFixed() ||
385 216 : linkModel->getParentJointModel()->isPassive() ||
386 144 : (!includeContinuos && checkIfContinuosJoint(linkModel->getParentJointModel())) ) {
387 :
388 : //an active joint is not any of these condition.
389 : //passive is an attribute of the joint in the srdf, so it may be not setted
390 : // (default is not passive), so we need also the getMimic == NULL
391 : // (ie: an actuated joint dont mimic anything)
392 : //WARNING these 4 conditions should be enough I think
393 :
394 0 : linkModel = linkModel->getParentLinkModel();
395 :
396 0 : if (linkModel == NULL ) {
397 :
398 : std::cerr << " [PARSER::" << __func__ <<
399 : "]: Strange error: fingertipsOfJointMap, jointsOfFingertipMap, and/or other structures " <<
400 0 : "may have been built badly" << std::endl ;
401 0 : return "";
402 : }
403 : }
404 :
405 72 : return (linkModel->getParentJointModel()->getName());
406 : }
407 :
408 :
409 72 : std::string ROSEE::ParserMoveIt::getFirstActuatedJointInFinger (std::string linkName) const {
410 72 : const moveit::core::LinkModel* linkModel = robot_model->getLinkModel(linkName);
411 : const moveit::core::JointModel* joint;
412 :
413 : // we stop when the link has more than 1 joint: so linkModel will be the parent of the first
414 : // link of the finger group and in joint we have stored the actuated (not continuos)
415 : // child joint most near to linkModel
416 414 : while ( (linkModel != NULL) && (linkModel->getChildJointModels().size() < 2) ) {
417 :
418 : //if the parent joint is an actuated (not cont) joint, store it (or overwrite the previous stored)
419 513 : if ( linkModel->getParentJointModel()->getMimic() == NULL &&
420 342 : (!linkModel->parentJointIsFixed()) &&
421 513 : (!linkModel->getParentJointModel()->isPassive()) &&
422 171 : (!checkIfContinuosJoint(linkModel->getParentJointModel() )) ) {
423 :
424 171 : joint = linkModel->getParentJointModel();
425 : }
426 :
427 171 : linkModel = linkModel->getParentLinkModel();
428 : }
429 :
430 72 : return joint->getName();
431 : }
432 :
433 :
434 :
435 : /*********************************** PRIVATE FUNCTIONS **********************************************************/
436 144 : void ROSEE::ParserMoveIt::lookForFingertips(bool verbose) {
437 648 : for (auto it: robot_model->getJointModelGroups()) {
438 :
439 1008 : std::string logGroupInfo;
440 504 : logGroupInfo = "[PARSER::" + std::string(__func__) + "] Found Group '" + it->getName() + "', " ;
441 :
442 504 : if (it->getSubgroupNames().size() != 0 ) {
443 144 : logGroupInfo.append("but it has some subgroups \n");
444 :
445 360 : } else if (! groupIsChain ( it ) ) {
446 0 : logGroupInfo.append("but it is not a chain \n");
447 :
448 360 : } else if (it->getLinkModels().size() == 0) {
449 0 : logGroupInfo.append("but it has 0 links \n");
450 :
451 : } else {
452 :
453 360 : logGroupInfo.append("with links: \n");
454 :
455 720 : std::string theTip = ""; //the last link with a visual geometry
456 1188 : for (auto itt : it->getLinkModels()) {
457 :
458 828 : logGroupInfo.append("\t'" + itt->getName() + "' ");
459 :
460 828 : if (itt->getChildJointModels().size() != 0) {
461 468 : logGroupInfo.append("(not a leaf link) ");
462 : } else {
463 360 : logGroupInfo.append("(a leaf link) ");
464 : }
465 :
466 828 : if (itt->getShapes().size() == 0 ) {
467 108 : logGroupInfo.append("(no visual geometry) ");
468 :
469 : } else {
470 720 : theTip = itt->getName();
471 : }
472 828 : logGroupInfo.append("\n");
473 :
474 : }
475 :
476 360 : if (theTip.compare("") == 0) {
477 0 : logGroupInfo.append("Warning: No link has a mesh in this group\n");
478 :
479 : } else {
480 360 : fingertipNames.push_back(theTip);
481 360 : fingerOfFingertipMap.insert( std::make_pair(theTip, it->getName()));
482 360 : fingertipOfFingerMap.insert( std::make_pair(it->getName(), theTip));
483 : }
484 : }
485 :
486 504 : if (verbose) {
487 0 : std::cout << logGroupInfo << std::endl;
488 : }
489 : }
490 144 : nFingers = fingertipNames.size();
491 144 : }
492 :
493 144 : void ROSEE::ParserMoveIt::lookForActiveJoints() {
494 :
495 864 : for (auto joint : robot_model->getActiveJointModels() ) {
496 : // robot_model->getActiveJointModels() returns not fixed not mimic but CAN return PASSIVE joints
497 720 : if (! joint->isPassive() ) {
498 720 : activeJointNames.push_back(joint->getName());
499 720 : activeJointModels.push_back(joint);
500 : }
501 : }
502 144 : }
503 :
504 144 : void ROSEE::ParserMoveIt::lookForPassiveJoints() {
505 :
506 1404 : for (auto joint : robot_model->getJointModels() ) {
507 1260 : if ( joint->isPassive() ) {
508 0 : passiveJointNames.push_back(joint->getName());
509 : }
510 : }
511 144 : }
512 :
513 :
514 144 : void ROSEE::ParserMoveIt::lookJointsTipsCorrelation() {
515 :
516 : //initialize the map with all tips and with empty vectors of its joints
517 504 : for (const auto &it: fingertipNames) {
518 360 : jointsOfFingertipMap.insert ( std::make_pair (it, std::vector<std::string>() ) );
519 : }
520 :
521 : //initialize the map with all the actuated joints and an empty vector for the tips that the joint move
522 864 : for (const auto &it: activeJointNames) {
523 720 : fingertipsOfJointMap.insert ( std::make_pair (it, std::vector<std::string>() ) );
524 : }
525 :
526 864 : for ( const auto &jointLink: descendantLinksOfJoint){ //for each actuated joint
527 :
528 2052 : for (const auto &link : jointLink.second) { //for each descendant link
529 :
530 : //if link is a tip...
531 1332 : if (std::find(fingertipNames.begin(), fingertipNames.end(), link->getName()) != fingertipNames.end()){
532 756 : jointsOfFingertipMap.at ( link->getName() ) .push_back( jointLink.first);
533 756 : fingertipsOfJointMap.at ( jointLink.first ) .push_back( link->getName() );
534 : }
535 : }
536 : }
537 :
538 144 : }
539 :
540 144 : void ROSEE::ParserMoveIt::lookForDescendants () {
541 :
542 864 : for (auto actJoint : activeJointModels) {
543 :
544 1440 : std::vector < const moveit::core::LinkModel* > linksVector;
545 1440 : std::vector < const moveit::core::JointModel* > jointsVector;
546 :
547 720 : getRealDescendantLinkModelsRecursive ( actJoint->getChildLinkModel(), linksVector, actJoint, jointsVector );
548 :
549 : //now we have to look among the mimic joints, but the mimic of only joint passed as argument, not also the mimic of children joints
550 828 : for (auto mimicJ : actJoint->getMimicRequests()) {
551 : // but we do not look on mimic joints that are children of the this joint in the tree,
552 : // because we have already explored them with recursion. Here we look only for mimic that are "cousins" of this joint
553 108 : if (std::find (jointsVector.begin(), jointsVector.end(), mimicJ) == jointsVector.end() ) {
554 36 : getRealDescendantLinkModelsRecursive (mimicJ->getChildLinkModel(), linksVector, mimicJ, jointsVector );
555 : }
556 : }
557 :
558 720 : descendantLinksOfJoint.insert (std::make_pair ( actJoint->getName(), linksVector ) );
559 720 : descendantJointsOfJoint.insert (std::make_pair ( actJoint->getName(), jointsVector ) );
560 : }
561 144 : }
562 :
563 1332 : void ROSEE::ParserMoveIt::getRealDescendantLinkModelsRecursive (
564 : const moveit::core::LinkModel* link, std::vector< const moveit::core::LinkModel* > & linksVect,
565 : const moveit::core::JointModel* joint, std::vector< const moveit::core::JointModel* > & jointsVect ) const {
566 :
567 1332 : linksVect.push_back (link) ;
568 1332 : jointsVect.push_back (joint);
569 1908 : auto childJoints = link->getChildJointModels();
570 1332 : if ( childJoints.size() == 0 ) {
571 756 : return; //recursive base
572 : }
573 :
574 1152 : for (auto cj : childJoints) {
575 : //recursion
576 576 : getRealDescendantLinkModelsRecursive( cj->getChildLinkModel(), linksVect, cj, jointsVect );
577 : }
578 :
579 : }
580 :
581 :
582 0 : void ROSEE::ParserMoveIt::parseNonLinearMimicRelations (std::string xml) {
583 :
584 :
585 : //we do not make urdf verification here, it is already done before by robot model loader of moveit,
586 : //and also by Parser with parseUrdf
587 0 : TiXmlDocument tiDoc;
588 0 : tiDoc.Parse(xml.c_str());
589 : //std::cout << robot_description << std::endl;
590 0 : TiXmlElement* jointEl = tiDoc.FirstChildElement("robot")->FirstChildElement("joint") ;
591 :
592 0 : while (jointEl) {
593 :
594 0 : std::string jointName = jointEl->Attribute("name");
595 0 : auto mimicEl = jointEl->FirstChildElement("mimic");
596 0 : if (mimicEl) {
597 0 : auto nlAttr = mimicEl->Attribute("nlFunPos");
598 0 : if (nlAttr) {
599 : //std::cout << jointName << std::endl;
600 : //std::cout << nlAttr << std::endl;
601 : //std::cout << mimicEl->Attribute("joint") << std::endl;
602 0 : std::string fatherName = mimicEl->Attribute("joint");
603 0 : mimicNLFatherOfJointMap.insert ( std::make_pair( jointName,
604 0 : std::make_pair(fatherName, nlAttr)) );
605 :
606 0 : mimicNLJointsOfFatherMap[fatherName].insert(std::make_pair(jointName, nlAttr)) ;
607 : }
608 : }
609 :
610 0 : jointEl = jointEl->NextSiblingElement("joint");
611 : }
612 :
613 60 : }
|
