@@ -331,7 +331,10 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
331
331
return baseLink_->node ();
332
332
}
333
333
auto linkIter = links_.find (linkId);
334
- CORRADE_INTERNAL_ASSERT (linkIter != links_.end ());
334
+ ESP_CHECK (
335
+ linkIter != links_.end (),
336
+ " ArticulatedObject::getLinkSceneNode - no link found with linkId ="
337
+ << linkId);
335
338
return linkIter->second ->node ();
336
339
}
337
340
@@ -347,7 +350,10 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
347
350
return baseLink_->visualNodes_ ;
348
351
}
349
352
auto linkIter = links_.find (linkId);
350
- CORRADE_INTERNAL_ASSERT (linkIter != links_.end ());
353
+ ESP_CHECK (linkIter != links_.end (),
354
+ " ArticulatedObject::getLinkVisualSceneNodes - no link found with "
355
+ " linkId ="
356
+ << linkId);
351
357
return linkIter->second ->visualNodes_ ;
352
358
}
353
359
@@ -403,7 +409,8 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
403
409
}
404
410
405
411
auto linkIter = links_.find (id);
406
- CORRADE_INTERNAL_ASSERT (linkIter != links_.end ());
412
+ ESP_CHECK (linkIter != links_.end (),
413
+ " ArticulatedObject::getLink - no link found with linkId =" << id);
407
414
return *linkIter->second ;
408
415
}
409
416
@@ -448,7 +455,9 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
448
455
*/
449
456
int getLinkIdFromName (const std::string& _name) const {
450
457
auto linkIdIter = linkNamesToIDs_.find (_name);
451
- CORRADE_INTERNAL_ASSERT (linkIdIter != linkNamesToIDs_.end ());
458
+ ESP_CHECK (linkIdIter != linkNamesToIDs_.end (),
459
+ " ArticulatedObject::getLinkIdFromName - no link found with name ="
460
+ << _name);
452
461
return linkIdIter->second ;
453
462
}
454
463
@@ -465,34 +474,36 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
465
474
* @brief Given the list of passed points in this object's local space, return
466
475
* those points transformed to world space.
467
476
* @param points vector of points in object local space
468
- * @param linkID Unused for rigids .
477
+ * @param linkId Internal link index .
469
478
* @return vector of points transformed into world space
470
479
*/
471
480
std::vector<Mn::Vector3> transformLocalPointsToWorld (
472
481
const std::vector<Mn::Vector3>& points,
473
- int linkID) const override {
474
- auto linkIter = links_.find (linkID);
475
- if (linkIter != links_.end ()) {
476
- return linkIter->second ->transformLocalPointsToWorld (points, linkID);
477
- }
478
- return points;
482
+ int linkId) const override {
483
+ auto linkIter = links_.find (linkId);
484
+ ESP_CHECK (linkIter != links_.end (),
485
+ " ArticulatedObject::getLinkVisualSceneNodes - no link found with "
486
+ " linkId ="
487
+ << linkId);
488
+ return linkIter->second ->transformLocalPointsToWorld (points, linkId);
479
489
}
480
490
481
491
/* *
482
492
* @brief Given the list of passed points in world space, return
483
493
* those points transformed to this object's local space.
484
494
* @param points vector of points in world space
485
- * @param linkID Unused for rigids .
495
+ * @param linkId Internal link index .
486
496
* @return vector of points transformed to be in local space
487
497
*/
488
498
std::vector<Mn::Vector3> transformWorldPointsToLocal (
489
499
const std::vector<Mn::Vector3>& points,
490
- int linkID) const override {
491
- auto linkIter = links_.find (linkID);
492
- if (linkIter != links_.end ()) {
493
- return linkIter->second ->transformWorldPointsToLocal (points, linkID);
494
- }
495
- return points;
500
+ int linkId) const override {
501
+ auto linkIter = links_.find (linkId);
502
+ ESP_CHECK (linkIter != links_.end (),
503
+ " ArticulatedObject::getLinkVisualSceneNodes - no link found with "
504
+ " linkId ="
505
+ << linkId);
506
+ return linkIter->second ->transformWorldPointsToLocal (points, linkId);
496
507
}
497
508
498
509
/* *
@@ -661,11 +672,12 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
661
672
* @param linkId The link's index.
662
673
* @return The link's parent joint's name.
663
674
*/
664
- virtual std::string getLinkJointName (CORRADE_UNUSED int linkId) const {
675
+ virtual std::string getLinkJointName (int linkId) const {
665
676
auto linkIter = links_.find (linkId);
666
- ESP_CHECK (linkIter != links_.end (),
667
- " ArticulatedObject::getLinkJointName - no link with linkId ="
668
- << linkId);
677
+ ESP_CHECK (
678
+ linkIter != links_.end (),
679
+ " ArticulatedObject::getLinkJointName - no link found with linkId ="
680
+ << linkId);
669
681
return linkIter->second ->linkJointName ;
670
682
}
671
683
@@ -675,15 +687,15 @@ class ArticulatedObject : public esp::physics::PhysicsObjectBase {
675
687
* @param linkId The link's index. -1 for base link.
676
688
* @return The link's name.
677
689
*/
678
- virtual std::string getLinkName (CORRADE_UNUSED int linkId) const {
690
+ virtual std::string getLinkName (int linkId) const {
679
691
if (linkId == -1 ) {
680
692
return baseLink_->linkName ;
681
693
}
682
694
683
695
auto linkIter = links_.find (linkId);
684
- ESP_CHECK (
685
- linkIter != links_. end (),
686
- " ArticulatedObject::getLinkName - no link with linkId = " << linkId);
696
+ ESP_CHECK (linkIter != links_. end (),
697
+ " ArticulatedObject::getLinkName - no link found with linkId = "
698
+ << linkId);
687
699
return linkIter->second ->linkName ;
688
700
}
689
701
0 commit comments