Commit 92776029 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add Device::removeJoints

parent 66278135
Pipeline #13874 failed with stage
in 1 minute and 1 second
......@@ -115,6 +115,10 @@ namespace hpp {
/// Create Pinocchio geomData from model.
void createGeomData();
/// Remove some joints from the configuration space.
void removeJoints(const std::vector<std::string>& jointNames,
Configuration_t referenceConfig);
/// \}
// -----------------------------------------------------------------------
/// \name Joints
......
......@@ -30,6 +30,7 @@
#include <hpp/fcl/BVH/BVH_model.h>
#include <pinocchio/algorithm/geometry.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp> // ::pinocchio::details::Dispatch
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/multibody/model.hpp>
......@@ -157,6 +158,34 @@ namespace hpp {
numberDeviceData(numberDeviceData());
}
void Device::removeJoints(const std::vector<std::string>& jointNames,
Configuration_t referenceConfig)
{
std::vector<JointIndex> joints (jointNames.size());
std::transform(jointNames.begin(), jointNames.end(), joints.begin(),
[this](const std::string& n) {
auto id = model_->getJointId(n);
if (id >= model_->joints.size())
throw std::invalid_argument ("Device " + name_ + " does not have "
"any joint named " + n);
return id;
});
ModelPtr_t nModel (new Model);
GeomModelPtr_t nGeomModel (new GeomModel);
::pinocchio::buildReducedModel(*model_, *geomModel_, joints,
referenceConfig.head(model_->nq), *nModel, *nGeomModel);
model_ = nModel;
geomModel_ = nGeomModel;
invalidate();
createData();
createGeomData();
numberDeviceData(numberDeviceData());
}
void Device::controlComputation (const Computation_t& flag)
{
AbstractDevice::controlComputation (flag);
......
......@@ -36,15 +36,15 @@
#include <hpp/pinocchio/liegroup-space.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/serialization.hh>
static bool verbose = true;
using namespace hpp::pinocchio;
void displayAABB(const hpp::fcl::AABB& aabb)
{
std::cout << "Bounding box is\n"
BOOST_TEST_MESSAGE(
"Bounding box is\n"
<< aabb.min_.transpose() << '\n'
<< aabb.max_.transpose() << std::endl;
<< aabb.max_.transpose());
}
BOOST_AUTO_TEST_CASE (computeAABB)
......@@ -55,17 +55,17 @@ BOOST_AUTO_TEST_CASE (computeAABB)
robot->rootJoint()->lowerBounds(vector3_t::Constant(-0));
robot->rootJoint()->upperBounds(vector3_t::Constant( 0));
hpp::fcl::AABB aabb0 = robot->computeAABB();
if (verbose) displayAABB(aabb0);
displayAABB(aabb0);
robot->rootJoint()->lowerBounds(vector3_t(-1, -1, 0));
robot->rootJoint()->upperBounds(vector3_t( 1, 1, 0));
hpp::fcl::AABB aabb1 = robot->computeAABB();
if (verbose) displayAABB(aabb1);
displayAABB(aabb1);
robot->rootJoint()->lowerBounds(vector3_t(-2, -2, 0));
robot->rootJoint()->upperBounds(vector3_t(-1, -1, 0));
hpp::fcl::AABB aabb2 = robot->computeAABB();
if (verbose) displayAABB(aabb2);
displayAABB(aabb2);
}
/* -------------------------------------------------------------------------- */
BOOST_AUTO_TEST_CASE (unit_test_device)
......@@ -93,6 +93,14 @@ BOOST_AUTO_TEST_CASE (unit_test_device)
space->mergeVectorSpaces();
BOOST_CHECK_EQUAL (space->name(), "SE(3)*R^29");
BOOST_TEST_MESSAGE(*robot);
robot->removeJoints({ "rleg2_joint", "rarm1_joint" },
robot->neutralConfiguration());
BOOST_TEST_MESSAGE(*robot);
BOOST_CHECK_THROW(robot->getJointByName("rleg2_joint"), std::runtime_error);
BOOST_CHECK_THROW(robot->getJointByName("rarm1_joint"), std::runtime_error);
robot = unittest::makeDevice (unittest::CarLike);
space = LiegroupSpace::createCopy(robot->configSpace());
space->mergeVectorSpaces();
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment