Verified Commit e45a384b authored by Justin Carpentier's avatar Justin Carpentier
Browse files

liegroup/operations: remove InPlace suffix

This is not mandatory as the function signatures are explicit
parent bfbb7c0b
......@@ -414,7 +414,7 @@ namespace pinocchio
*
*/
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrateTransportInPlace(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
......@@ -437,13 +437,13 @@ namespace pinocchio
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrateTransportInPlace(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
dIntegrateTransportInPlace<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
dIntegrateTransport<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
}
/**
......
......@@ -198,7 +198,7 @@ namespace pinocchio
template<typename LieGroup_t, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrateTransportInPlace(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
void dIntegrateTransport(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
......@@ -211,7 +211,7 @@ namespace pinocchio
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
typedef dIntegrateTransportStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
......
......@@ -204,7 +204,7 @@ namespace pinocchio
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & q,
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & Jin) const
{
......@@ -214,7 +214,7 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & q,
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & Jin) const
{
......
......@@ -233,11 +233,11 @@ namespace pinocchio
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateTransportStepAlgo);
template<typename Visitor, typename JointModel> struct dIntegrateTransportInPlaceStepAlgo;
template<typename Visitor, typename JointModel> struct dIntegrateTransportStepAlgo;
template<typename LieGroup_t, typename ConfigVectorIn, typename TangentVectorIn, typename JacobianMatrixType>
struct dIntegrateTransportInPlaceStep
: public fusion::JointUnaryVisitorBase< dIntegrateTransportInPlaceStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
struct dIntegrateTransportStep
: public fusion::JointUnaryVisitorBase< dIntegrateTransportStep<LieGroup_t,ConfigVectorIn,TangentVectorIn,JacobianMatrixType> >
{
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
......@@ -245,11 +245,11 @@ namespace pinocchio
const ArgumentPosition &
> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateTransportInPlaceStepAlgo, dIntegrateTransportInPlaceStep)
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateTransportStepAlgo, dIntegrateTransportStep)
};
template<typename Visitor, typename JointModel>
struct dIntegrateTransportInPlaceStepAlgo
struct dIntegrateTransportStepAlgo
{
template<typename ConfigVectorIn, typename TangentVector, typename JacobianMatrixType>
static void run(const JointModelBase<JointModel> & jmodel,
......@@ -261,14 +261,14 @@ namespace pinocchio
typedef typename Visitor::LieGroupMap LieGroupMap;
typename LieGroupMap::template operation<JointModel>::type lgo;
lgo.dIntegrateTransportInPlace(jmodel.jointConfigSelector (q.derived()),
lgo.dIntegrateTransport(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointRows(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)),
arg);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateTransportInPlaceStepAlgo);
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateTransportStepAlgo);
template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo;
......
......@@ -161,6 +161,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @details This input and output has to be interpreted in terms of Lie group, not vector space:
* Thus, dIntegrate(q, v, J, arg) creates a manifold manifold M given by a small variation of the configuration vector or the tangent vector into the tangent space at identity.
* We are moving our input matrix onto this manifold M.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
......@@ -182,6 +183,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @details This input and output has to be interpreted in terms of Lie group, not vector space:
* Thus, dIntegrate(q, v, J, ARG0) creates a manifold manifold M given by a small variation of the configuration vector into the tangent space at identity.
* We are moving our input matrix onto this manifold M.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
......@@ -199,6 +201,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @details This input and output has to be interpreted in terms of Lie group, not vector space:
* Thus, dIntegrate(q, v, J, ARG1) creates a manifold manifold M given by a small variation of the tangent vector into the tangent space at identity.
* We are moving our input matrix onto this manifold M.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in] Jin the input matrix
......@@ -218,6 +221,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @details This input and output has to be interpreted in terms of Lie group, not vector space:
* Thus, dIntegrate(q, v, J, arg) creates a manifold manifold M given by a small variation of the configuration vector or the tangent vector into the tangent space at identity.
* We are moving our input matrix onto this manifold M.
*
* @param[in] q configuration vector.
* @param[in] v tangent vector
* @param[in,out] J the input matrix
......@@ -225,7 +229,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
*/
template<class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace(const Eigen::MatrixBase<Config_t > & q,
void dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J,
const ArgumentPosition arg) const;
......@@ -242,7 +246,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
*
*/
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq(const Eigen::MatrixBase<Config_t > & q,
void dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const;
/**
......@@ -258,7 +262,7 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
* @param[out] Jout Transported matrix
*/
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv(const Eigen::MatrixBase<Config_t > & q,
void dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const;
......
......@@ -48,9 +48,13 @@ namespace pinocchio {
switch (arg) {
case ARG0:
dIntegrate_dq(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op); return;
dIntegrate_dq(q.derived(),v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op);
return;
case ARG1:
dIntegrate_dv(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op); return;
dIntegrate_dv(q.derived(),v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op);
return;
default: return;
}
}
......@@ -90,8 +94,8 @@ namespace pinocchio {
template <class Derived>
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
void LieGroupBase<Derived>::dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout,
const ArgumentPosition arg) const
......@@ -99,21 +103,25 @@ namespace pinocchio {
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
dIntegrateTransport_dq(q.derived(),v.derived(),Jin.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); return;
case ARG1:
dIntegrateTransport_dv(q.derived(),v.derived(),Jin.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); return;
default: return;
case ARG0:
dIntegrateTransport_dq(q.derived(),v.derived(),Jin.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout));
return;
case ARG1:
dIntegrateTransport_dv(q.derived(),v.derived(),Jin.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout));
return;
default:
return;
}
}
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrateTransport_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
void LieGroupBase<Derived>::dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
......@@ -126,11 +134,10 @@ namespace pinocchio {
template <class Derived>
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
void LieGroupBase<Derived>::dIntegrateTransport_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
void LieGroupBase<Derived>::dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianIn_t> & Jin,
const Eigen::MatrixBase<JacobianOut_t> & Jout) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
......@@ -144,7 +151,7 @@ namespace pinocchio {
template <class Derived>
template<class Config_t, class Tangent_t, class Jacobian_t>
void LieGroupBase<Derived>::dIntegrateTransportInPlace(const Eigen::MatrixBase<Config_t > & q,
void LieGroupBase<Derived>::dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J,
const ArgumentPosition arg) const
......@@ -152,17 +159,22 @@ namespace pinocchio {
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
dIntegrateTransportInPlace_dq(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); return;
case ARG1:
dIntegrateTransportInPlace_dv(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); return;
default: return;
case ARG0:
dIntegrateTransport_dq(q.derived(),v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
return;
case ARG1:
dIntegrateTransport_dv(q.derived(),v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
return;
default:
return;
}
}
template <class Derived>
template <class Config_t, class Tangent_t, class Jacobian_t>
void LieGroupBase<Derived>::dIntegrateTransportInPlace_dq(
void LieGroupBase<Derived>::dIntegrateTransport_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
......@@ -170,24 +182,23 @@ namespace pinocchio {
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
derived().dIntegrateTransportInPlace_dq_impl(q.derived(),
derived().dIntegrateTransport_dq_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
}
template <class Derived>
template <class Config_t, class Tangent_t, class Jacobian_t>
void LieGroupBase<Derived>::dIntegrateTransportInPlace_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
void LieGroupBase<Derived>::dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
{
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t);
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t);
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
derived().dIntegrateTransportInPlace_dv_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
derived().dIntegrateTransport_dv_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J));
}
/**
......
......@@ -236,9 +236,9 @@ namespace pinocchio
}
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t>& J) const
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1,
const Eigen::MatrixBase<JacobianOut_t>& J) const
{
Matrix2 R0, R1; Vector2 t0, t1;
forwardKinematics(R0, t0, q0);
......@@ -378,15 +378,19 @@ namespace pinocchio
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6(nu, Jtmp6);
Jout.template topRows<2>().noalias() = Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>();
Jout.template topRows<2>().noalias() += Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>();
Jout.template bottomRows<1>().noalias() = Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>();
Jout.template bottomRows<1>().noalias() += Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>();
Jout.template topRows<2>().noalias()
= Jtmp6.template topLeftCorner<2,2>() * Jin.template topRows<2>();
Jout.template topRows<2>().noalias()
+= Jtmp6.template topRightCorner<2,1>() * Jin.template bottomRows<1>();
Jout.template bottomRows<1>().noalias()
= Jtmp6.template bottomLeftCorner<1,2>()* Jin.template topRows<2>();
Jout.template bottomRows<1>().noalias()
+= Jtmp6.template bottomRightCorner<1,1>() * Jin.template bottomRows<1>();
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
{
......@@ -396,7 +400,7 @@ namespace pinocchio
exp(v, R, t);
Vector2 tinv = (R.transpose() * t).reverse();
tinv[0] *= Scalar(-1.);
tinv[0] *= Scalar(-1);
//TODO: Aliasing
Jout.template topRows<2>() = R.transpose() * Jout.template topRows<2>();
//No Aliasing
......@@ -404,7 +408,7 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J) const
{
......@@ -414,33 +418,24 @@ namespace pinocchio
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6(nu, Jtmp6);
//TODO: Remove aliasing
Jout.template topRows<2>() = Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>();
Jout.template topRows<2>().noalias() += Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>();
Jout.template bottomRows<1>() = Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>();
Jout.template bottomRows<1>().noalias() += Jtmp6.template bottomLeftCorner<1,2>()* Jout.template topRows<2>();
Jout.template topRows<2>()
= Jtmp6.template topLeftCorner<2,2>() * Jout.template topRows<2>();
Jout.template topRows<2>().noalias()
+= Jtmp6.template topRightCorner<2,1>() * Jout.template bottomRows<1>();
Jout.template bottomRows<1>()
= Jtmp6.template bottomRightCorner<1,1>() * Jout.template bottomRows<1>();
Jout.template bottomRows<1>().noalias()
+= Jtmp6.template bottomLeftCorner<1,2>() * Jout.template topRows<2>();
}
// interpolate_impl use default implementation.
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
// static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
// const Eigen::MatrixBase<ConfigR_t> & q1,
// const Scalar& u,
// const Eigen::MatrixBase<ConfigOut_t>& qout)
// template <class ConfigL_t, class ConfigR_t>
// static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
// const Eigen::MatrixBase<ConfigR_t> & q1)
template <class Config_t>
static void normalize_impl (const Eigen::MatrixBase<Config_t>& qout)
static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
{
PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).template tail<2>().normalize();
}
template <class Config_t>
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const
void random_impl(const Eigen::MatrixBase<Config_t> & qout) const
{
R2crossSO2_t().random(qout);
}
......@@ -656,7 +651,7 @@ namespace pinocchio
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
switch(op)
{
{
case SETTO:
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
......@@ -669,7 +664,7 @@ namespace pinocchio
default:
assert(false && "Wrong Op requesed value");
break;
}
}
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
......@@ -679,20 +674,20 @@ namespace pinocchio
const AssignmentOperatorType op)
{
switch(op)
{
{
case SETTO:
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
case ADDTO:
Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
Jexp6<ADDTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
case RMTO:
Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
......@@ -705,9 +700,12 @@ namespace pinocchio
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
Jout.template topRows<3>().noalias() = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template bottomRows<3>().noalias() = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template topRows<3>().noalias()
= Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
Jout.template topRows<3>().noalias()
+= Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template bottomRows<3>().noalias()
= Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
}
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
......@@ -720,14 +718,17 @@ namespace pinocchio
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
Jout.template topRows<3>().noalias() = Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template bottomRows<3>().noalias() = Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template topRows<3>().noalias()
= Jtmp6.template topLeftCorner<3,3>() * Jin.template topRows<3>();
Jout.template topRows<3>().noalias()
+= Jtmp6.template topRightCorner<3,3>() * Jin.template bottomRows<3>();
Jout.template bottomRows<3>().noalias()
= Jtmp6.template bottomRightCorner<3,3>() * Jin.template bottomRows<3>();
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J_out) const
{
......@@ -736,13 +737,16 @@ namespace pinocchio
Jtmp6 = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
//Aliasing
Jout.template topRows<3>() = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template bottomRows<3>() = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template topRows<3>()
= Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
Jout.template topRows<3>().noalias()
+= Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template bottomRows<3>()
= Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<Jacobian_t> & J_out) const
{
......@@ -750,22 +754,14 @@ namespace pinocchio
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6<SETTO>(MotionRef<const Tangent_t>(v.derived()), Jtmp6);
Jout.template topRows<3>() = Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
Jout.template topRows<3>().noalias() += Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template bottomRows<3>() = Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template topRows<3>()
= Jtmp6.template topLeftCorner<3,3>() * Jout.template topRows<3>();
Jout.template topRows<3>().noalias()
+= Jtmp6.template topRightCorner<3,3>() * Jout.template bottomRows<3>();
Jout.template bottomRows<3>()
= Jtmp6.template bottomRightCorner<3,3>() * Jout.template bottomRows<3>();
}
// interpolate_impl use default implementation.
// template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
// static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
// const Eigen::MatrixBase<ConfigR_t> & q1,
// const Scalar& u,
// const Eigen::MatrixBase<ConfigOut_t>& qout)
// {
// }
template <class ConfigL_t, class ConfigR_t>
static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
const Eigen::MatrixBase<ConfigR_t> & q1)
......
......@@ -238,12 +238,12 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}
template <class Config_t, class Tangent_t, class Jacobian_t>
void dIntegrateTransportInPlace_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {}
......@@ -282,10 +282,6 @@ namespace pinocchio
}
}
// template <class ConfigL_t, class ConfigR_t>
// static double squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0,
// const Eigen::MatrixBase<ConfigR_t> & q1)
template <class Config_t>
static void normalize_impl (const Eigen::MatrixBase<Config_t> & qout)
{
......@@ -306,8 +302,7 @@ namespace pinocchio
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t>
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &,
const Eigen::MatrixBase<ConfigR_t> &,
const Eigen::MatrixBase<ConfigOut_t> & qout)
const
const Eigen::MatrixBase<ConfigOut_t> & qout) const
{
random_impl(qout);
}
......@@ -452,8 +447,8 @@ namespace pinocchio