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

Bug fix and unit-tests

parent 8abeee0a
......@@ -130,7 +130,8 @@ namespace hpp {
const value_type& u,
ConfigurationOut_t result)
{
result = se3::interpolate<LieGroup>(robot->model(), q0, q1, u);
const Model& model = robot->model();
result.head(model.nq) = se3::interpolate<LieGroup>(model, q0, q1, u);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = u * q1.tail (dim) + (1-u) * q0.tail (dim);
}
......@@ -160,7 +161,8 @@ namespace hpp {
void difference (const DevicePtr_t& robot, ConfigurationIn_t q1,
ConfigurationIn_t q2, vectorOut_t result)
{
result = se3::difference<LieGroup> (robot->model(), q2, q1);
const Model& model = robot->model();
result.head(model.nv) = se3::difference<LieGroup> (model, q2, q1);
const size_type& dim = robot->extraConfigSpace().dimension();
result.tail (dim) = q1.tail (dim) - q2.tail (dim);
}
......
......@@ -137,8 +137,6 @@ namespace hpp {
d_.data_ = DataPtr_t( new Data(model()) );
// We assume that model is now complete and state can be resized.
resizeState();
d_.invalidate();
numberDeviceData(datas_.size());
}
void Device::
......@@ -263,6 +261,7 @@ namespace hpp {
void Device::
resizeState()
{
d_.invalidate();
d_.currentConfiguration_ = neutralConfiguration();
d_.currentVelocity_ = vector_t::Zero(numberDof());
d_.currentAcceleration_ = vector_t::Zero(numberDof());
......@@ -274,6 +273,8 @@ namespace hpp {
*configSpace_ *= Joint(weakPtr_, i).configurationSpace();
if (extraConfigSpace_.dimension() > 0)
*configSpace_ *= LiegroupSpace::create (extraConfigSpace_.dimension());
numberDeviceData(datas_.size());
}
LiegroupSpacePtr_t Device::
......
......@@ -53,6 +53,16 @@ BOOST_AUTO_TEST_CASE (single_thread)
BOOST_CHECK_EQUAL (data2, d1->dataPtr());
delete d1;
delete d2;
size_type nCfg = robot->configSize();
robot->setDimensionExtraConfigSpace(3);
BOOST_CHECK_EQUAL(robot->configSize(), nCfg+3);
d1 = new DeviceSync (robot);
d2 = new DeviceSync (robot);
BOOST_CHECK_EQUAL(d1->d().currentConfiguration_.size(), nCfg+3);
BOOST_CHECK_EQUAL(d2->d().currentConfiguration_.size(), nCfg+3);
delete d1;
delete d2;
}
typedef se3::container::aligned_vector<se3::SE3> SE3Vector_t;
......
......@@ -81,6 +81,14 @@ BOOST_AUTO_TEST_CASE (unit_test_device)
space->mergeVectorSpaces();
BOOST_CHECK_EQUAL (space->name(), "R^3*SO(3)*R^26");
robot->setDimensionExtraConfigSpace(3);
BOOST_CHECK_EQUAL(robot->numberDof(), 32+3);
BOOST_CHECK_EQUAL(robot->configSize(), 33+3);
space = LiegroupSpace::createCopy(robot->configSpace());
space->mergeVectorSpaces();
BOOST_CHECK_EQUAL (space->name(), "SE(3)*R^29");
robot = makeDeviceSafe (unittest::CarLike);
space = LiegroupSpace::createCopy(robot->configSpace());
space->mergeVectorSpaces();
......
Supports Markdown
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