Commit d616c56e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add Joint::lowerBounds and upperBounds + getRotationSubJacobian

parent 8fa6d849
......@@ -179,6 +179,10 @@ namespace hpp {
void lowerBound (size_type rank, value_type lowerBound);
/// Set upper bound of given degree of freedom
void upperBound (size_type rank, value_type upperBound);
/// Set lower bounds
void lowerBounds (vectorIn_t lowerBounds);
/// Set upper bounds
void upperBounds (vectorIn_t upperBounds);
/// Get upper bound on linear velocity of the joint frame
/// \return coefficient \f$\lambda\f$ such that
......
......@@ -19,18 +19,28 @@
#include <pinocchio/multibody/liegroup/cartesian-product.hpp>
#include <hpp/util/exception-factory.hh>
namespace hpp {
namespace pinocchio {
namespace liegroup {
template<typename LieGroup1, typename LieGroup2>
struct CartesianProductOperation : public se3::CartesianProductOperation<LieGroup1, LieGroup2>
{
enum {
BoundSize = LieGroup1::BoundSize + LieGroup2::BoundSize,
NR = LieGroup1::BoundSize + LieGroup2::BoundSize,
NT = LieGroup1::BoundSize + LieGroup2::BoundSize
};
typedef se3::CartesianProductOperation<LieGroup1, LieGroup2> Base;
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return se3::CartesianProductOperation<LieGroup1,LieGroup2>::squaredDistance(q0, q1);
return Base::squaredDistance(q0, q1);
}
template <class ConfigL_t, class ConfigR_t>
......@@ -42,6 +52,46 @@ namespace hpp {
return LieGroup1::squaredDistance(q0.template head<LieGroup1::NQ>(), q1.template head<LieGroup1::NQ>(), w)
+ LieGroup2::squaredDistance(q0.template tail<LieGroup2::NQ>(), q1.template tail<LieGroup2::NQ>(), w);
}
template <class ConfigIn_t, class ConfigOut_t>
static void setBound(
const Eigen::MatrixBase<ConfigIn_t > & bound,
const Eigen::MatrixBase<ConfigOut_t> & out)
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, Base::ConfigVector_t)
ConfigOut_t& oout = const_cast<Eigen::MatrixBase<ConfigOut_t>&> (out).derived();
if (bound.size() == BoundSize) {
if (LieGroup1::BoundSize > 0)
LieGroup1::setBound(bound.template head<LieGroup1::BoundSize>(),
oout. template head<LieGroup1::NQ >());
if (LieGroup2::BoundSize > 0)
LieGroup2::setBound(bound.template tail<LieGroup2::BoundSize>(),
oout. template tail<LieGroup2::NQ >());
} else if (bound.size() == Base::NQ) {
LieGroup1::setBound(bound.template head<LieGroup1::NQ>(),
oout. template head<LieGroup1::NQ>());
LieGroup2::setBound(bound.template tail<LieGroup2::NQ>(),
oout. template tail<LieGroup2::NQ>());
} else {
HPP_THROW(std::invalid_argument,
"Expected vector of size " << BoundSize << " or " << Base::NQ
<< ", got size " << bound.size());
}
}
template <class JacobianIn_t, class JacobianOut_t>
static void getRotationSubJacobian(
const Eigen::MatrixBase<JacobianIn_t > & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout)
{
JacobianOut_t& J = const_cast<Eigen::MatrixBase<JacobianOut_t>&> (Jout).derived();
if (LieGroup1::NR > 0)
LieGroup1::getRotationSubJacobian(Jin.template leftCols<LieGroup1::NV>(),
J .template leftCols<LieGroup1::NR>());
if (LieGroup2::NR > 0)
LieGroup2::getRotationSubJacobian(Jin.template rightCols<LieGroup2::NV>(),
J .template rightCols<LieGroup2::NR>());
}
}; // struct CartesianProductOperation
} // namespace liegroup
} // namespace pinocchio
......
......@@ -25,12 +25,19 @@ namespace hpp {
template<int N>
struct SpecialOrthogonalOperation : public se3::SpecialOrthogonalOperation<N>
{
typedef se3::SpecialOrthogonalOperation<N> Base;
enum {
BoundSize = 0,
NR = Base::NV,
NT = 0
};
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return se3::SpecialOrthogonalOperation<N>::squaredDistance(q0, q1);
return Base::squaredDistance(q0, q1);
}
template <class ConfigL_t, class ConfigR_t>
......@@ -41,6 +48,27 @@ namespace hpp {
{
return w * squaredDistance(q0, q1);
}
template <class ConfigIn_t, class ConfigOut_t>
static void setBound(
const Eigen::MatrixBase<ConfigIn_t > & bound,
const Eigen::MatrixBase<ConfigOut_t> & out)
{
if (bound.size() == 0) return;
if (bound.size() != Base::NQ) {
HPP_THROW(std::invalid_argument, "Expected vector of size 0 or "
<< Base::NQ << ", got size " << bound.size());
}
const_cast<Eigen::MatrixBase<ConfigOut_t>&>(out).head(bound.size()) = bound;
}
template <class JacobianIn_t, class JacobianOut_t>
static void getRotationSubJacobian(
const Eigen::MatrixBase<JacobianIn_t > & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout)
{
const_cast<Eigen::MatrixBase<JacobianOut_t>&> (Jout) = Jin;
}
};
} // namespace liegroup
} // namespace pinocchio
......
......@@ -19,18 +19,44 @@
#include <pinocchio/multibody/liegroup/vector-space.hpp>
#include <hpp/util/exception-factory.hh>
namespace hpp {
namespace pinocchio {
namespace liegroup {
namespace details {
template <bool Test> struct assign_if {
template <class D1, class D2> static void run(
const Eigen::MatrixBase<D1> & Jin,
const Eigen::MatrixBase<D2> & Jout)
{}
};
template <> struct assign_if<true> {
template <class D1, class D2> static void run(
const Eigen::MatrixBase<D1> & Jin,
const Eigen::MatrixBase<D2> & Jout)
{
const_cast<Eigen::MatrixBase<D2>&> (Jout) = Jin;
}
};
}
template<int Size, bool rot>
struct VectorSpaceOperation : public se3::VectorSpaceOperation<Size>
{
typedef se3::VectorSpaceOperation<Size> Base;
enum {
BoundSize = Size,
NR = (rot ? Size : 0),
NT = (rot ? 0 : Size)
};
template <class ConfigL_t, class ConfigR_t>
static double squaredDistance(
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
{
return se3::VectorSpaceOperation<Size>::squaredDistance(q0, q1);
return Base::squaredDistance(q0, q1);
}
template <class ConfigL_t, class ConfigR_t>
......@@ -42,6 +68,26 @@ namespace hpp {
if (rot) return w * squaredDistance(q0, q1);
else return squaredDistance(q0, q1);
}
template <class ConfigIn_t, class ConfigOut_t>
static void setBound(
const Eigen::MatrixBase<ConfigIn_t > & bounds,
const Eigen::MatrixBase<ConfigOut_t> & out)
{
if (bounds.size() != BoundSize) {
HPP_THROW(std::invalid_argument, "Expected vector of size " <<
BoundSize << ", got size " << bounds.size());
}
const_cast<Eigen::MatrixBase<ConfigOut_t>&> (out) = bounds;
}
template <class JacobianIn_t, class JacobianOut_t>
static void getRotationSubJacobian(
const Eigen::MatrixBase<JacobianIn_t > & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout)
{
details::assign_if<rot>::run(Jin, Jout);
}
};
} // namespace liegroup
} // namespace pinocchio
......
......@@ -25,6 +25,8 @@
# include <hpp/pinocchio/device.hh>
# include <hpp/pinocchio/body.hh>
# include "joint/bound.hh"
# define CALL_JOINT(method, valueIfZero) \
(jointIndex > 0 ? model().joints[jointIndex].method() : valueIfZero);
namespace hpp {
......@@ -175,6 +177,16 @@ namespace hpp {
assert(rank < configSize());
model().upperPositionLimit[idx] = upperBound;
}
void Joint::lowerBounds (vectorIn_t lowerBounds)
{
SetBoundStep::run(model().joints[jointIndex],
SetBoundStep::ArgsType(lowerBounds, model().lowerPositionLimit));
}
void Joint::upperBounds (vectorIn_t upperBounds)
{
SetBoundStep::run(model().joints[jointIndex],
SetBoundStep::ArgsType(upperBounds, model().upperPositionLimit));
}
/* --- MAX DISTANCE ------------------------------------------------------*/
......
//
// Copyright (c) 2016 CNRS
// Author: 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/>.
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <hpp/pinocchio/liegroup.hh>
namespace hpp {
namespace pinocchio {
struct SetBoundStep : public se3::fusion::JointModelVisitor<SetBoundStep>
{
typedef boost::fusion::vector<const Configuration_t &,
Configuration_t &> ArgsType;
JOINT_MODEL_VISITOR_INIT(SetBoundStep);
template<typename JointModel>
static void algo(const se3::JointModelBase<JointModel> & jmodel,
const Configuration_t & bounds,
Configuration_t& out)
{
::hpp::pinocchio::LieGroupTpl::template operation<JointModel>::type
::setBound(bounds,
jmodel.jointConfigSelector(out));
}
};
template <>
void SetBoundStep::algo<se3::JointModelComposite>(
const se3::JointModelBase<se3::JointModelComposite> & jmodel,
const Configuration_t & bounds,
Configuration_t & out)
{
se3::details::Dispatch<SetBoundStep>::run(
jmodel.derived(),
ArgsType(bounds, out));
}
} // namespace pinocchio
} // namespace hpp
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