diff --git a/include/mc_rbdyn/Robot.h b/include/mc_rbdyn/Robot.h index 5813d579f5..818235b1bd 100644 --- a/include/mc_rbdyn/Robot.h +++ b/include/mc_rbdyn/Robot.h @@ -949,16 +949,8 @@ struct MC_RBDYN_DLLAPI Robot const sva::PTransformd * base = nullptr, const std::string & baseName = ""); - /** Copy existing Robot with a new base - * - * \throws std::runtime_error if a robot named already exists - **/ - void copy(Robots & robots, const std::string & copyName, unsigned int robots_idx, const Base & base) const; - /** Copy existing Robot - * - * \throws std::runtime_error if a robot named already exists - **/ - void copy(Robots & robots, const std::string & copyName, unsigned int robots_idx) const; + /** Copy loaded data from this robot to a new robot **/ + void copyLoadedData(Robot & destination) const; /** Used to set the surfaces' X_b_s correctly */ void fixSurfaces(); diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp index 54634f553c..177885e8b5 100644 --- a/src/mc_rbdyn/Robot.cpp +++ b/src/mc_rbdyn/Robot.cpp @@ -385,18 +385,17 @@ Robot::Robot(const std::string & name, collisionTransforms_[o.first] = sva::PTransformd::Identity(); } } + for(const auto & b : mb().bodies()) + { + collisionTransforms_[b.name()] = sva::PTransformd::Identity(); + } + for(const auto & p : module_.collisionTransforms()) + { + collisionTransforms_[p.first] = p.second; + } + fixCollisionTransforms(); } - for(const auto & b : mb().bodies()) - { - collisionTransforms_[b.name()] = sva::PTransformd::Identity(); - } - for(const auto & p : module_.collisionTransforms()) - { - collisionTransforms_[p.first] = p.second; - } - fixCollisionTransforms(); - if(loadFiles) { if(bfs::exists(module_.rsdf_dir)) @@ -1368,10 +1367,8 @@ const sva::MotionVecd Robot::accW() const return sva::PTransformd{rot} * mbc().bodyAccB[0]; } -void Robot::copy(Robots & robots, const std::string & copyName, unsigned int robots_idx, const Base & base) const +void Robot::copyLoadedData(Robot & robot) const { - robots.robots_.emplace_back(Robot(copyName, robots, robots_idx, false, &base.X_0_s, base.baseName)); - auto & robot = robots.robots_.back(); for(const auto & s : surfaces_) { robot.surfaces_[s.first] = s.second->copy(); @@ -1381,6 +1378,8 @@ void Robot::copy(Robots & robots, const std::string & copyName, unsigned int rob { robot.convexes_[cH.first] = {cH.second.first, S_ObjectPtr(cH.second.second->clone())}; } + robot.collisionTransforms_ = collisionTransforms_; + robot.fixCollisionTransforms(); fixSCH(robot, robot.convexes_, robot.collisionTransforms_); for(size_t i = 0; i < forceSensors_.size(); ++i) { @@ -1388,16 +1387,6 @@ void Robot::copy(Robots & robots, const std::string & copyName, unsigned int rob } } -void Robot::copy(Robots & robots, const std::string & copyName, unsigned int robots_idx) const -{ - robots.robots_.emplace_back(Robot(copyName, robots, robots_idx, false)); - auto & robot = robots.robots_.back(); - for(const auto & s : surfaces_) - { - robot.surfaces_[s.first] = s.second->copy(); - } -} - mc_rbdyn::Surface & Robot::copySurface(const std::string & sName, const std::string & name) { if(hasSurface(name)) diff --git a/src/mc_rbdyn/Robots.cpp b/src/mc_rbdyn/Robots.cpp index f2b7155413..94cda97019 100644 --- a/src/mc_rbdyn/Robots.cpp +++ b/src/mc_rbdyn/Robots.cpp @@ -54,7 +54,8 @@ Robots::Robots(const Robots & rhs) for(unsigned int i = 0; i < rhs.robots_.size(); ++i) { const Robot & robot = rhs.robots_[i]; - robot.copy(*this, robot.name(), i); + robots_.emplace_back(Robot(robot.name(), *this, i, false)); + robot.copyLoadedData(robots_.back()); } } @@ -74,7 +75,8 @@ Robots & Robots::operator=(const Robots & rhs) for(unsigned int i = 0; i < rhs.robots_.size(); ++i) { const Robot & robot = rhs.robots_[i]; - robot.copy(*this, robot.name(), i); + robots_.emplace_back(Robot(robot.name(), *this, i, false)); + robot.copyLoadedData(robots_.back()); } return *this; } @@ -182,7 +184,24 @@ void Robots::createRobotWithBase(const std::string & name, const Base & base, const Eigen::Vector3d & baseAxis) { - createRobotWithBase(name, robots.robot(robots_idx), base, baseAxis); + { + const auto & robot = robots.robots_[robots_idx]; + if(hasRobot(name)) + { + mc_rtc::log::error_and_throw( + "Cannot copy robot {} with a new base as a robot named {} already exists", robot.name(), name); + } + this->robot_modules_.push_back(robot.module()); + this->mbs_.push_back(robot.mbg().makeMultiBody(base.baseName, base.baseType, baseAxis, base.X_0_s, base.X_b0_s)); + this->mbcs_.emplace_back(this->mbs_.back()); + this->mbgs_.push_back(robot.mbg()); + } + auto robotIndex = static_cast(this->mbs_.size()) - 1; + robots_.emplace_back(Robot(name, *this, robotIndex, false)); + // emplace_back might have invalidated the reference we formed before + const auto & robot = robots.robots_[robots_idx]; + robot.copyLoadedData(robots_.back()); + robotNameToIndex_[name] = robotIndex; } void Robots::createRobotWithBase(const std::string & name, @@ -190,18 +209,7 @@ void Robots::createRobotWithBase(const std::string & name, const Base & base, const Eigen::Vector3d & baseAxis) { - if(hasRobot(name)) - { - mc_rtc::log::error_and_throw( - "Cannot copy robot {} with a new base as a robot named {} already exists", robot.name(), name); - } - this->robot_modules_.push_back(robot.module()); - this->mbs_.push_back(robot.mbg().makeMultiBody(base.baseName, base.baseType, baseAxis, base.X_0_s, base.X_b0_s)); - this->mbcs_.emplace_back(this->mbs_.back()); - this->mbgs_.push_back(robot.mbg()); - auto robotIndex = static_cast(this->mbs_.size()) - 1; - robot.copy(*this, name, robotIndex, base); - robotNameToIndex_[name] = robotIndex; + createRobotWithBase(name, *robot.robots_, robot.robots_idx_, base, baseAxis); } void Robots::removeRobot(const std::string & name) @@ -246,8 +254,13 @@ void Robots::robotCopy(const Robot & robot, const std::string & copyName) this->mbs_.push_back(robot.mb()); this->mbcs_.push_back(robot.mbc()); this->mbgs_.push_back(robot.mbg()); + auto referenceRobots = robot.robots_; + auto referenceIndex = robot.robots_idx_; auto copyRobotIndex = static_cast(this->mbs_.size()) - 1; - robot.copy(*this, copyName, copyRobotIndex); + robots_.emplace_back(Robot(copyName, *this, copyRobotIndex, false)); + // emplace_back might have invalidated the reference we were given + const auto & refRobot = referenceRobots->robots_[referenceIndex]; + refRobot.copyLoadedData(robots_.back()); robotNameToIndex_[copyName] = copyRobotIndex; } diff --git a/tests/test_mc_rbdyn.cpp b/tests/test_mc_rbdyn.cpp index d742c67bf7..83ff30076e 100644 --- a/tests/test_mc_rbdyn.cpp +++ b/tests/test_mc_rbdyn.cpp @@ -6,6 +6,8 @@ #include #include +#include + mc_rbdyn::Robots & get_robots() { static std::shared_ptr robots_ptr = nullptr; @@ -21,42 +23,57 @@ mc_rbdyn::Robots & get_robots() return *robots_ptr; } -BOOST_AUTO_TEST_CASE(TestRobotLoading) +void TestRobotLoadingCommon(mc_rbdyn::RobotModulePtr rm, mc_rbdyn::RobotModulePtr envrm) { - configureRobotLoader(); - auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1"); - auto envrm = mc_rbdyn::RobotLoader::get_robot_module("env", std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH), - std::string("ground")); // Non-unique names BOOST_REQUIRE_THROW(mc_rbdyn::loadRobots({rm, rm}), std::runtime_error); std::shared_ptr robots_ptr = nullptr; BOOST_REQUIRE_NO_THROW(robots_ptr = mc_rbdyn::loadRobots({rm, envrm})); BOOST_REQUIRE(robots_ptr->hasRobot(rm->name)); BOOST_REQUIRE(robots_ptr->hasRobot(envrm->name)); - auto & robot = robots_ptr->robot(rm->name); - auto & env = robots_ptr->robot(envrm->name); - BOOST_REQUIRE_EQUAL(robot.name(), rm->name); - BOOST_REQUIRE_EQUAL(robot.robotIndex(), 0); - BOOST_REQUIRE_EQUAL(env.name(), envrm->name); - BOOST_REQUIRE_EQUAL(env.robotIndex(), 1); - robots_ptr->rename(robot.name(), "renamed"); - BOOST_REQUIRE(robots_ptr->hasRobot("renamed")); - auto & renamed = robots_ptr->robot("renamed"); - BOOST_REQUIRE_EQUAL(renamed.name(), "renamed"); - BOOST_REQUIRE_EQUAL(robot.name(), "renamed"); - BOOST_REQUIRE_EQUAL(renamed.robotIndex(), 0); - BOOST_REQUIRE_EQUAL(robot.robotIndex(), 0); - BOOST_REQUIRE(robots_ptr->hasRobot(envrm->name)); - BOOST_REQUIRE_EQUAL(robots_ptr->robot(envrm->name).name(), envrm->name); - BOOST_REQUIRE_EQUAL(robots_ptr->robot(envrm->name).robotIndex(), 1); + { + auto & robot = robots_ptr->robot(rm->name); + auto & env = robots_ptr->robot(envrm->name); + BOOST_REQUIRE_EQUAL(robot.name(), rm->name); + BOOST_REQUIRE_EQUAL(robot.robotIndex(), 0); + BOOST_REQUIRE_EQUAL(env.name(), envrm->name); + BOOST_REQUIRE_EQUAL(env.robotIndex(), 1); + robots_ptr->rename(robot.name(), "renamed"); + BOOST_REQUIRE(robots_ptr->hasRobot("renamed")); + auto & renamed = robots_ptr->robot("renamed"); + BOOST_REQUIRE_EQUAL(renamed.name(), "renamed"); + BOOST_REQUIRE_EQUAL(robot.name(), "renamed"); + BOOST_REQUIRE_EQUAL(renamed.robotIndex(), 0); + BOOST_REQUIRE_EQUAL(robot.robotIndex(), 0); + BOOST_REQUIRE(robots_ptr->hasRobot(envrm->name)); + BOOST_REQUIRE_EQUAL(robots_ptr->robot(envrm->name).name(), envrm->name); + BOOST_REQUIRE_EQUAL(robots_ptr->robot(envrm->name).robotIndex(), 1); + BOOST_REQUIRE_NO_THROW(robots_ptr->robotCopy(robot, "robotCopy")); + } - BOOST_REQUIRE_NO_THROW(robots_ptr->robotCopy(robot, "robotCopy")); BOOST_REQUIRE(robots_ptr->hasRobot("robotCopy")); auto & robotCopy = robots_ptr->robots().back(); BOOST_REQUIRE(robotCopy.name() != rm->name); BOOST_REQUIRE_EQUAL(robots_ptr->robot("robotCopy").name(), "robotCopy"); BOOST_REQUIRE_EQUAL(robotCopy.robotIndex(), 2); BOOST_REQUIRE_EQUAL(robots_ptr->robots().back().name(), "robotCopy"); + auto & robot = robots_ptr->robot("renamed"); + for(const auto & c : robot.convexes()) + { + BOOST_REQUIRE(robotCopy.hasConvex(c.first)); + } + for(const auto & s : robot.surfaces()) + { + BOOST_REQUIRE(robotCopy.hasSurface(s.first)); + } + for(const auto & fs : robot.forceSensors()) + { + BOOST_REQUIRE(robotCopy.hasForceSensor(fs.name())); + } + for(const auto & bs : robot.bodySensors()) + { + BOOST_REQUIRE(robotCopy.hasBodySensor(bs.name())); + } robots_ptr->removeRobot("robotCopy"); BOOST_REQUIRE(!robots_ptr->hasRobot("robotCopy")); @@ -64,6 +81,26 @@ BOOST_AUTO_TEST_CASE(TestRobotLoading) BOOST_REQUIRE(robots_ptr->hasRobot(envrm->name)); } +BOOST_AUTO_TEST_CASE(TestRobotLoading) +{ + configureRobotLoader(); + auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1"); + auto envrm = mc_rbdyn::RobotLoader::get_robot_module("env", std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH), + std::string("ground")); + TestRobotLoadingCommon(rm, envrm); +} + +BOOST_AUTO_TEST_CASE(TestRobotLoadingWithCollisionObjects) +{ + configureRobotLoader(); + auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1"); + rm->_collisionObjects["L_HAND_SPHERE"] = {"L_WRIST_Y_S", std::make_shared(0.09)}; + rm->_collisionTransforms["L_HAND_SPHERE"] = sva::PTransformd::Identity(); + auto envrm = mc_rbdyn::RobotLoader::get_robot_module("env", std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH), + std::string("ground")); + TestRobotLoadingCommon(rm, envrm); +} + BOOST_AUTO_TEST_CASE(TestRobotPosWVelWAccW) { auto robots = get_robots();