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