Skip to content

Commit

Permalink
[mc_rbdyn] Fix an issue with robot copy
Browse files Browse the repository at this point in the history 
Previous implementation could fail to copy the robot properly as the
robots_ vector was resized while copying a robot. This commit fixes this
by reforming a valid reference to the copied robot if needed
  • Loading branch information
@gergondet
gergondet committed Aug 18, 2021
1 parent 78fae7e commit be294db
Show file tree
Hide file tree
Showing 4 changed files with 102 additions and 71 deletions.
12 changes: 2 additions & 10 deletions include/mc_rbdyn/Robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <copyName> 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 <copyName> 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();
Expand Down
35 changes: 12 additions & 23 deletions src/mc_rbdyn/Robot.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand Down Expand Up @@ -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();
Expand All @@ -1381,23 +1378,15 @@ 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)
{
robot.forceSensors_[i].copyCalibrator(forceSensors_[i]);
}
}

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))
Expand Down
45 changes: 29 additions & 16 deletions src/mc_rbdyn/Robots.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}

Expand All @@ -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;
}
Expand Down Expand Up @@ -182,26 +184,32 @@ 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<std::runtime_error>(
"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<unsigned int>(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,
Robot & robot,
const Base & base,
const Eigen::Vector3d & baseAxis)
{
if(hasRobot(name))
{
mc_rtc::log::error_and_throw<std::runtime_error>(
"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<unsigned int>(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)
Expand Down Expand Up @@ -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<unsigned int>(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;
}

Expand Down
81 changes: 59 additions & 22 deletions tests/test_mc_rbdyn.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@
#include <chrono>
#include <random>

#include <sch/S_Object/S_Sphere.h>

mc_rbdyn::Robots & get_robots()
{
static std::shared_ptr<mc_rbdyn::Robots> robots_ptr = nullptr;
Expand All @@ -21,49 +23,84 @@ 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<mc_rbdyn::Robots> 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"));
BOOST_REQUIRE(robots_ptr->hasRobot("renamed"));
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<sch::S_Sphere>(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();
Expand Down

0 comments on commit be294db

Please sign in to comment.