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

LiegroupSpace::Jdifference can apply jacobian to the right or left.

parent 2b53b4a0
......@@ -175,6 +175,7 @@ namespace hpp {
/// \param[out] J0 the Jacobian of v with respect to q0.
/// \param[out] J1 the Jacobian of v with respect to q1.
/// \note to compute only one jacobian, provide for J0 or J1 an empty matrix.
template <bool ApplyOnTheLeft>
void Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const;
/// Return name of Lie group
......
......@@ -22,6 +22,7 @@
namespace hpp {
namespace pinocchio {
namespace liegroupType {
template <bool ApplyOnTheLeft>
struct JdifferenceVisitor : public boost::static_visitor <>
{
JdifferenceVisitor (vectorIn_t& q0, vectorIn_t& q1,
......@@ -44,10 +45,18 @@ namespace hpp {
q1_.segment<LgT::NQ>(iq_, lg.nq()),
J0int,
J1int);
if (J0_.size() > 0)
J0_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J0int);
if (J1_.size() > 0)
J1_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J1int);
if (J0_.size() > 0) {
if (ApplyOnTheLeft)
J0_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J0int);
else
J0_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J0int);
}
if (J1_.size() > 0) {
if (ApplyOnTheLeft)
J1_.middleRows<LgT::NV> (iv_, lg.nv()).applyOnTheLeft (J1int);
else
J1_.middleCols<LgT::NV> (iv_, lg.nv()).applyOnTheRight (J1int);
}
iq_ += lg.nq();
iv_ += lg.nv();
}
......
......@@ -137,11 +137,12 @@ namespace hpp {
assert (row == nv());
}
template <bool ApplyOnTheLeft>
void LiegroupSpace::Jdifference (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const
{
assert (q0.size() == nq() && q1.size() == nq());
liegroupType::JdifferenceVisitor jdv (q0,q1,J0,J1);
liegroupType::JdifferenceVisitor<ApplyOnTheLeft> jdv (q0,q1,J0,J1);
for (std::size_t i = 0; i < liegroupTypes_.size (); ++i) {
boost::apply_visitor (jdv, liegroupTypes_ [i]);
}
......@@ -149,6 +150,9 @@ namespace hpp {
assert (jdv.iv_ == nv());
}
template void LiegroupSpace::Jdifference<true > (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const;
template void LiegroupSpace::Jdifference<false> (vectorIn_t q0, vectorIn_t q1, matrixOut_t J0, matrixOut_t J1) const;
struct NameVisitor : public boost::static_visitor <>
{
template <typename LgT1> void operator () (const LgT1& lg1)
......
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