Commit 6e6bbfb9 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

[LieGroup] Add LieGroup dDifference with matrix product.

parent 4de5e1a4
......@@ -46,7 +46,7 @@ struct LieGroupWrapperTpl
return lg.difference(q0,q1);
}
static JacobianMatrix_t dDifference(const LieGroupType& lg,
static JacobianMatrix_t dDifference1(const LieGroupType& lg,
const ConfigVector_t& q0, const ConfigVector_t& q1,
const ArgumentPosition arg)
{
......@@ -55,6 +55,44 @@ struct LieGroupWrapperTpl
return J;
}
static JacobianMatrix_t dDifference2(const LieGroupType& lg,
const ConfigVector_t& q0, const ConfigVector_t& q1,
const ArgumentPosition arg,
const JacobianMatrix_t& Jin, int self)
{
JacobianMatrix_t J (Jin.rows(), Jin.cols());
switch (arg) {
case ARG0:
lg.template dDifference<ARG0>(q0, q1, Jin, self, J, SETTO);
break;
case ARG1:
lg.template dDifference<ARG1>(q0, q1, Jin, self, J, SETTO);
break;
default:
throw std::invalid_argument("arg must be either ARG0 or ARG1");
}
return J;
}
static JacobianMatrix_t dDifference3(const LieGroupType& lg,
const ConfigVector_t& q0, const ConfigVector_t& q1,
const ArgumentPosition arg,
int self, const JacobianMatrix_t& Jin)
{
JacobianMatrix_t J (Jin.rows(), Jin.cols());
switch (arg) {
case ARG0:
lg.template dDifference<ARG0>(q0, q1, self, Jin, J, SETTO);
break;
case ARG1:
lg.template dDifference<ARG1>(q0, q1, self, Jin, J, SETTO);
break;
default:
throw std::invalid_argument("arg must be either ARG0 or ARG1");
}
return J;
}
static JacobianMatrix_t dIntegrate_dq1(const LieGroupType& lg,
const ConfigVector_t& q, const TangentVector_t& v)
{
......@@ -152,7 +190,9 @@ public:
.def("dIntegrateTransport", LieGroupWrapper::dIntegrateTransport2)
.def("difference", LieGroupWrapper::difference)
.def("dDifference", LieGroupWrapper::dDifference)
.def("dDifference", LieGroupWrapper::dDifference1)
.def("dDifference", LieGroupWrapper::dDifference2)
.def("dDifference", LieGroupWrapper::dDifference3)
.def("interpolate", LieGroupWrapper::interpolate)
......
......@@ -110,6 +110,14 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference_product_impl(const ConfigL_t & q0,
const ConfigR_t & q1,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dDifferenceOnTheLeft,
const AssignmentOperatorType op) const;
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
......
......@@ -71,6 +71,42 @@ dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
}
}
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
dDifference_product_impl(const ConfigL_t & q0,
const ConfigR_t & q1,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dDifferenceOnTheLeft,
const AssignmentOperatorType op) const
{
Index id_q = 0, id_v = 0;
for(size_t k = 0; k < liegroups.size(); ++k)
{
const Index & nq = lg_nqs[k];
const Index & nv = lg_nvs[k];
if (dDifferenceOnTheLeft)
::pinocchio::dDifference<arg>(liegroups[k],
q0.segment(id_q,nq),
q1.segment(id_q,nq),
SELF,
Jin.middleRows(id_v,nv),
Jout.middleRows(id_v,nv),
op);
else
::pinocchio::dDifference<arg>(liegroups[k],
q0.segment(id_q,nq),
q1.segment(id_q,nq),
Jin.middleCols(id_v,nv),
SELF,
Jout.middleCols(id_v,nv),
op);
id_q += nq; id_v += nv;
}
}
template<typename _Scalar, int _Options, template<typename,int> class LieGroupCollectionTpl>
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
void CartesianProductOperationVariantTpl<_Scalar,_Options,LieGroupCollectionTpl>::
......
......@@ -427,10 +427,6 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
// {
// PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
// return dDifference(q0.derived(),q1.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
// }
/**
*
......@@ -448,6 +444,23 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t> & J,
const ArgumentPosition arg) const;
template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
int self,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op = SETTO) const;
template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
int self,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op = SETTO) const;
/**
* @brief Squared distance between two joint configurations.
*
......@@ -533,6 +546,14 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
const ArgumentPosition arg,
const AssignmentOperatorType op) const;
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference_product_impl(const ConfigL_t & q0,
const ConfigR_t & q1,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dDifferenceOnTheLeft,
const AssignmentOperatorType op) const;
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
......
......@@ -401,6 +401,48 @@ namespace pinocchio {
}
}
template <class Derived>
template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
int self,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op) const
{
PINOCCHIO_UNUSED_VARIABLE(self);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
assert(Jin.cols() == nv());
assert(Jout.cols() == nv());
assert(Jout.rows() == Jin.rows());
derived().template dDifference_product_impl<arg>(
q0.derived(), q1.derived(),
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
false, op);
}
template <class Derived>
template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
int self,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const AssignmentOperatorType op) const
{
PINOCCHIO_UNUSED_VARIABLE(self);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
assert(Jin.rows() == nv());
assert(Jout.rows() == nv());
assert(Jout.cols() == Jin.cols());
derived().template dDifference_product_impl<arg>(
q0.derived(), q1.derived(),
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout),
true, op);
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t>
typename LieGroupBase<Derived>::Scalar
......@@ -524,6 +566,34 @@ namespace pinocchio {
}
}
template <class Derived>
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dDifference_product_impl(const ConfigL_t & q0,
const ConfigR_t & q1,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool dDifferenceOnTheLeft,
const AssignmentOperatorType op) const
{
Index nv_ (nv());
JacobianMatrix_t J (nv_, nv_);
dDifference<arg>(q0, q1, J);
switch (op) {
case SETTO:
if(dDifferenceOnTheLeft) Jout = J * Jin;
else Jout = Jin * J;
return;
case ADDTO:
if(dDifferenceOnTheLeft) Jout += J * Jin;
else Jout += Jin * J;
return;
case RMTO:
if(dDifferenceOnTheLeft) Jout -= J * Jin;
else Jout -= Jin * J;
return;
}
}
template <class Derived>
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void LieGroupBase<Derived>::interpolate_impl(
......
......@@ -150,6 +150,24 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianOut_t> & J,
const ArgumentPosition arg);
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
int self,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const ArgumentPosition arg);
template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
int self,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const ArgumentPosition arg);
template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void dIntegrateTransport(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<Config_t > & q,
......
......@@ -508,6 +508,69 @@ namespace pinocchio
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
}
template <class M1_t, class M2_t, class M3_t, class M4_t, bool dDifferenceOnTheLeft, ArgumentPosition arg>
struct LieGroupDDifferenceProductVisitor
: visitor::LieGroupVisitorBase< LieGroupDDifferenceProductVisitor<M1_t, M2_t, M3_t, M4_t, dDifferenceOnTheLeft, arg> >
{
typedef boost::fusion::vector<const M1_t &,
const M2_t &,
const M3_t &,
M4_t &,
const AssignmentOperatorType> ArgsType;
LIE_GROUP_VISITOR(LieGroupDDifferenceProductVisitor);
template<typename LieGroupDerived>
static void algo(const LieGroupBase<LieGroupDerived> & lg,
const M1_t& q0,
const M2_t& q1,
const M3_t& J_in,
M4_t& J_out,
const AssignmentOperatorType op)
{
if(dDifferenceOnTheLeft) lg.template dDifference<arg>(q0, q1, SELF, J_in, J_out, op);
else lg.template dDifference<arg>(q0, q1, J_in, SELF, J_out, op);
}
};
template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
int self,
const Eigen::MatrixBase<JacobianOut_t> & J_out,
const AssignmentOperatorType op)
{
PINOCCHIO_UNUSED_VARIABLE(self);
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, false, arg> Operation;
Operation::run(lg,typename Operation::ArgsType(
q0.derived(),q1.derived(),J_in.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
}
template<ArgumentPosition arg, typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference(const LieGroupGenericTpl<LieGroupCollection> & lg,
const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
int self,
const Eigen::MatrixBase<JacobianIn_t> & J_in,
const Eigen::MatrixBase<JacobianOut_t> & J_out,
const AssignmentOperatorType op)
{
PINOCCHIO_UNUSED_VARIABLE(self);
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
typedef LieGroupDDifferenceProductVisitor<ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, true, arg> Operation;
Operation::run(lg,typename Operation::ArgsType(
q0.derived(),q1.derived(),J_in.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
}
template <class M1_t, class M2_t, class M3_t, class M4_t>
struct LieGroupDIntegrateTransportVisitor
: visitor::LieGroupVisitorBase< LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, M4_t> >
......
......@@ -87,6 +87,30 @@ namespace pinocchio
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).setIdentity();
}
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t>
void dDifference_product_impl(const ConfigL_t &,
const ConfigR_t &,
const JacobianIn_t & Jin,
JacobianOut_t & Jout,
bool,
const AssignmentOperatorType op) const
{
switch (op) {
case SETTO:
if (arg == ARG0) Jout = - Jin;
else Jout = Jin;
return;
case ADDTO:
if (arg == ARG0) Jout -= Jin;
else Jout += Jin;
return;
case RMTO:
if (arg == ARG0) Jout += Jin;
else Jout -= Jin;
return;
}
}
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t>
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q,
const Eigen::MatrixBase<Velocity_t> & v,
......
......@@ -20,6 +20,8 @@ class TestLiegroupBindings(unittest.TestCase):
self.assertEqual(R3.nv+SO3.nv, R3xSO3.nv)
def test_dIntegrate(self):
SELF = 0
for lg in [ pin.liegroups.R3(),
pin.liegroups.SO3(),
pin.liegroups.SE3(),
......@@ -29,16 +31,46 @@ class TestLiegroupBindings(unittest.TestCase):
v = np.random.rand(lg.nv)
J = lg.dIntegrate_dq(q, v)
#self.assertTrue(np.allclose(np.eye(3),J))
SELF = 0
J0 = np.random.rand(lg.nv,lg.nv)
J1 = lg.dIntegrate_dq(q, v, SELF, J0)
self.assertTrue(np.allclose(J @ J0,J1))
self.assertTrue(np.allclose(np.dot(J, J0),J1))
J1 = lg.dIntegrate_dq(q, v, J0, SELF)
self.assertTrue(np.allclose(J0 @ J,J1))
self.assertTrue(np.allclose(np.dot(J0, J),J1))
J = lg.dIntegrate_dv(q, v)
J0 = np.random.rand(lg.nv,lg.nv)
J1 = lg.dIntegrate_dv(q, v, SELF, J0)
self.assertTrue(np.allclose(np.dot(J, J0),J1))
J1 = lg.dIntegrate_dv(q, v, J0, SELF)
self.assertTrue(np.allclose(np.dot(J0, J),J1))
def test_dDifference(self):
for lg in [ pin.liegroups.R3(),
pin.liegroups.SO3(),
pin.liegroups.SE3(),
pin.liegroups.R3() * pin.liegroups.SO3(),
]:
q0 = lg.random()
q1 = lg.random()
for arg in [ pin.ARG0, pin.ARG1 ]:
J = lg.dDifference(q0, q1, arg)
SELF = 0
J0 = np.random.rand(lg.nv,lg.nv)
J1 = lg.dDifference(q0, q1, arg, SELF, J0)
self.assertTrue(np.allclose(np.dot(J, J0),J1))
J1 = lg.dDifference(q0, q1, arg, J0, SELF)
self.assertTrue(np.allclose(np.dot(J0, J),J1))
if __name__ == '__main__':
unittest.main()
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