Unverified Commit cd09e14e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #111 from jmirabel/devel

Add LiegroupSpace::interpolate + misc.
parents f3a1c7c7 d32ff0e1
......@@ -178,7 +178,7 @@ namespace hpp {
const LiegroupSpacePtr_t& configSpace () const { return configSpace_; }
/// See Joint::RnxSOnConfigurationSpace
LiegroupSpacePtr_t RnxSOnConfigSpace () const;
const LiegroupSpacePtr_t& RnxSOnConfigSpace () const { return configSpaceRnxSOn_; }
/// Get the neutral configuration
Configuration_t neutralConfiguration () const;
......@@ -330,7 +330,7 @@ namespace hpp {
std::string name_;
// Grippers
Grippers_t grippers_;
LiegroupSpacePtr_t configSpace_;
LiegroupSpacePtr_t configSpace_, configSpaceRnxSOn_;
// Extra configuration space
ExtraConfigSpace extraConfigSpace_;
// Joint linear constraints
......
......@@ -258,6 +258,17 @@ namespace hpp {
template <DerivativeProduct side>
void dDifference_dq1 (vectorIn_t q0, vectorIn_t q1, matrixOut_t J1) const;
/// Interpolate between two elements of the Lie group.
///
/// This is equivalent to \f$ q_0 \oplus u*(q_1 \ominus q_0) \f$.
///
/// \param q0, q1 two elements
/// \param u in [0,1] position along the interpolation: q0 for u=0,
/// q1 for u=1
/// \retval result interpolated configuration
void interpolate (vectorIn_t q0, vectorIn_t q1, value_type u,
vectorOut_t result) const;
/// Return name of Lie group
std::string name () const;
......
......@@ -123,7 +123,6 @@ namespace hpp {
{
weakPtr_ = weakPtr;
d_.devicePtr_ = weakPtr;
DevicePtr_t self (weakPtr_.lock());
}
void Device::initCopy(const DeviceWkPtr_t& weakPtr, const Device& other)
......@@ -279,27 +278,21 @@ namespace hpp {
d_.jointJacobians_.resize ((std::size_t)model().njoints);
configSpace_ = LiegroupSpace::empty();
configSpaceRnxSOn_ = LiegroupSpace::empty();
const Model& m (model());
for (JointIndex i = 1; i < m.joints.size(); ++i)
*configSpace_ *= Joint(weakPtr_, i).configurationSpace();
if (extraConfigSpace_.dimension() > 0)
*configSpace_ *= LiegroupSpace::create (extraConfigSpace_.dimension());
for (JointIndex i = 1; i < m.joints.size(); ++i) {
*configSpace_ *= Joint(weakPtr_, i).configurationSpace();
*configSpaceRnxSOn_ *= Joint(weakPtr_, i).RnxSOnConfigurationSpace();
}
if (extraConfigSpace_.dimension() > 0) {
LiegroupSpacePtr_t extra = LiegroupSpace::create (extraConfigSpace_.dimension());
*configSpace_ *= extra;
*configSpaceRnxSOn_ *= extra;
}
numberDeviceData(numberDeviceData());
}
LiegroupSpacePtr_t Device::
RnxSOnConfigSpace () const
{
const Model& m (model());
LiegroupSpacePtr_t space (LiegroupSpace::empty());
for (JointIndex i = 1; i < m.joints.size(); ++i)
*space *= Joint(weakPtr_, i).RnxSOnConfigurationSpace();
if (extraConfigSpace_.dimension() > 0)
*space *= LiegroupSpace::create (extraConfigSpace_.dimension());
return space;
}
Configuration_t Device::
neutralConfiguration () const
{
......
......@@ -20,6 +20,7 @@
#include "../src/size-visitor.hh"
#include "../src/dintegrate-visitor.hh"
#include "../src/jdifference-visitor.hh"
#include "../src/visitor/interpolate.hh"
namespace hpp {
namespace pinocchio {
......@@ -253,6 +254,19 @@ namespace hpp {
template void LiegroupSpace::dDifference_dq1<DerivativeTimesInput> (vectorIn_t, vectorIn_t, matrixOut_t) const;
template void LiegroupSpace::dDifference_dq1<InputTimesDerivative> (vectorIn_t, vectorIn_t, matrixOut_t) const;
void LiegroupSpace::interpolate (vectorIn_t q0, vectorIn_t q1, value_type u,
vectorOut_t r) const
{
assert (q0.size() == nq());
assert (q1.size() == nq());
assert (r .size() == nq());
liegroupType::visitor::Interpolate iv (q0, q1, u, r);
for (LiegroupTypes::const_iterator it = liegroupTypes ().begin ();
it != liegroupTypes ().end (); ++it)
boost::apply_visitor (iv, *it);
}
struct NameVisitor : public boost::static_visitor <>
{
template <typename LgT1> void operator () (const LgT1& lg1)
......
// Copyright (c) 2020, CNRS
// Authors: Joseph Mirabel
//
// This file is part of hpp-pinocchio.
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_PINOCCHIO_SRC_VISITOR_INTERPOLATE_HH
# define HPP_PINOCCHIO_SRC_VISITOR_INTERPOLATE_HH
namespace hpp {
namespace pinocchio {
namespace liegroupType {
namespace visitor {
/// Interpolate
struct Interpolate : public boost::static_visitor <>
{
Interpolate (const vectorIn_t& e1, const vectorIn_t& e2,
value_type u,
vectorOut_t& er) :
e1_ (e1), e2_ (e2), u_ (u), er_ (er),
iq_(0), iv_(0)
{}
template <typename LiegroupType> void operator () (LiegroupType& op)
{
op.interpolate (
e1_.segment<LiegroupType::NQ>(iq_, op.nq()),
e2_.segment<LiegroupType::NQ>(iq_, op.nq()),
u_,
er_.segment<LiegroupType::NQ>(iq_, op.nq()));
iq_ += op.nq();
iv_ += op.nv();
}
const vectorIn_t& e1_;
const vectorIn_t& e2_;
value_type u_;
vectorOut_t& er_;
size_type iq_, iv_;
}; // struct SubstractionVisitor
} // namespace visitor
} // namespace liegroupType
} // namespace pinocchio
} // namespace hpp
#endif // HPP_PINOCCHIO_SRC_VISITOR_INTERPOLATE_HH
......@@ -18,7 +18,11 @@
#include <boost/test/unit_test.hpp>
#include <pinocchio/fwd.hpp>
#include <hpp/pinocchio/fwd.hh>
#include <hpp/pinocchio/joint-collection.hh>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/check.hpp>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/device.hh>
......@@ -64,6 +68,9 @@ BOOST_AUTO_TEST_CASE (unit_test_device)
DevicePtr_t robot;
LiegroupSpacePtr_t space;
robot = Device::create ("robot");
BOOST_CHECK(pinocchio::checkData (robot->model(), robot->data()));
robot = unittest::makeDevice (unittest::HumanoidSimple);
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