Skip to content

GitLab

Unverified Commit b12d918a authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1169 from proyan/devel 

dIntegrate with ADDTO and RMTO assignment operators.
parents f779762d 429dd038
Hide whitespace changes
Inline Side-by-side
bindings/python/algorithm/expose-joints.cpp
View file @ b12d918a
......@@ -30,8 +30,8 @@ namespace pinocchio
Eigen::MatrixXd J0(Eigen::MatrixXd::Zero(model.nv,model.nv));
Eigen::MatrixXd J1(Eigen::MatrixXd::Zero(model.nv,model.nv));
dIntegrate(model,q,v,J0,ARG0);
dIntegrate(model,q,v,J1,ARG1);
dIntegrate(model,q,v,J0,ARG0,SETTO);
dIntegrate(model,q,v,J1,ARG1,SETTO);
return bp::make_tuple(J0,J1);
}
......@@ -43,7 +43,7 @@ namespace pinocchio
{
Eigen::MatrixXd J(Eigen::MatrixXd::Zero(model.nv,model.nv));
dIntegrate(model,q,v,J,arg);
dIntegrate(model,q,v,J,arg, SETTO);
return J;
}
......
src/algorithm/joint-configuration.hpp
View file @ b12d918a
......@@ -285,7 +285,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg);
const ArgumentPosition arg,
const AssignmentOperatorType op);
/**
*
......@@ -312,9 +313,39 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,SETTO);
}
/**
*
* @brief Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the tangent space at identity.
*
* @details This jacobian has to be interpreted in terms of Lie group, not vector space: as such,
* it is expressed in the tangent space only, not the configuration space.
* Calling \f$ f(q, v) \f$ the integrate function, these jacobians satisfy the following relationships in the
* tangent space:
* - Jacobian relative to q: \f$ f(q \oplus \delta q, v) \ominus f(q, v) = J_q(q, v) \delta q + o(\delta q)\f$.
* - Jacobian relative to v: \f$ f(q, v + \delta v) \ominus f(q, v) = J_v(q, v) \delta v + o(\delta v)\f$.
*
* @param[in] model Model of the kinematic tree on which the integration operation is performed.
* @param[in] q Initial configuration (size model.nq)
* @param[in] v Joint velocity (size model.nv)
* @param[out] J Jacobian of the Integrate operation, either with respect to q or v (size model.nv x model.nv).
* @param[in] arg Argument (either q or v) with respect to which the differentiation is performed.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
void dIntegrate(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg,
const AssignmentOperatorType op)
{
dIntegrate<LieGroupMap,Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType,JacobianMatrixType>(model, q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op);
}
/**
*
* @brief Computes the Jacobian of a small variation of the configuration vector into the tangent space at identity.
......
src/algorithm/joint-configuration.hxx
View file @ b12d918a
......@@ -152,7 +152,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
const ArgumentPosition arg,
const AssignmentOperatorType op)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT(q.size() == model.nq, "The configuration vector is not of the right size");
PINOCCHIO_CHECK_INPUT_ARGUMENT(v.size() == model.nv, "The joint velocity vector is not of the right size");
......@@ -163,7 +164,7 @@ namespace pinocchio
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg,op);
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i)
{
Algo::run(model.joints[i], args);
......
src/fwd.hpp
View file @ b12d918a
......@@ -55,6 +55,15 @@ namespace pinocchio
ARG3 = 3,
ARG4 = 4
};
enum AssignmentOperatorType
{
SETTO,
ADDTO,
RMTO
};
/// \brief Return type undefined
/// This is an helper structure to help internal diagnosis.
......
src/multibody/liegroup/cartesian-product.hpp
View file @ b12d918a
......@@ -125,23 +125,59 @@ namespace pinocchio
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
J12(J).setZero();
J21(J).setZero();
lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J));
lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J));
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) const
{
switch(op)
{
case SETTO:
J12(J).setZero();
J21(J).setZero();
lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J),SETTO);
lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J),SETTO);
break;
case ADDTO:
lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J),ADDTO);
lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J),ADDTO);
break;
case RMTO:
lg1_.dIntegrate_dq(Q1(q), V1(v), J11(J),RMTO);
lg2_.dIntegrate_dq(Q2(q), V2(v), J22(J),RMTO);
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
J12(J).setZero();
J21(J).setZero();
lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J));
lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J));
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) const
{
switch(op)
{
case SETTO:
J12(J).setZero();
J21(J).setZero();
lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J),SETTO);
lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J),SETTO);
break;
case ADDTO:
lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J),ADDTO);
lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J),ADDTO);
break;
case RMTO:
lg1_.dIntegrate_dv(Q1(q), V1(v), J11(J),RMTO);
lg2_.dIntegrate_dv(Q2(q), V2(v), J22(J),RMTO);
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template <class ConfigL_t, class ConfigR_t>
......
src/multibody/liegroup/liegroup-algo.hxx
View file @ b12d918a
......@@ -30,6 +30,7 @@ namespace pinocchio
#define PINOCCHIO_DETAILS_WRITE_ARGS_2(JM) PINOCCHIO_DETAILS_WRITE_ARGS_1(JM), typename boost::fusion::result_of::at_c<ArgsType, 1>::type a1
#define PINOCCHIO_DETAILS_WRITE_ARGS_3(JM) PINOCCHIO_DETAILS_WRITE_ARGS_2(JM), typename boost::fusion::result_of::at_c<ArgsType, 2>::type a2
#define PINOCCHIO_DETAILS_WRITE_ARGS_4(JM) PINOCCHIO_DETAILS_WRITE_ARGS_3(JM), typename boost::fusion::result_of::at_c<ArgsType, 3>::type a3
#define PINOCCHIO_DETAILS_WRITE_ARGS_5(JM) PINOCCHIO_DETAILS_WRITE_ARGS_4(JM), typename boost::fusion::result_of::at_c<ArgsType, 4>::type a4
#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_1(Algo) \
template <typename Visitor, typename JointCollection> \
......@@ -62,6 +63,14 @@ namespace pinocchio
static void run (PINOCCHIO_DETAILS_WRITE_ARGS_4(JointModelCompositeTpl<JointCollection>)) \
{ ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2,a3)); } \
}
#define PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(Algo) \
template <typename Visitor, typename JointCollection> \
struct Algo <Visitor, JointModelCompositeTpl<JointCollection> > { \
typedef typename Visitor::ArgsType ArgsType; \
static void run (PINOCCHIO_DETAILS_WRITE_ARGS_5(JointModelCompositeTpl<JointCollection>)) \
{ ::pinocchio::details::Dispatch< Visitor >::run(jmodel.derived(), ArgsType(a0,a1,a2,a3,a4)); } \
}
#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_1(Algo, Visitor) \
typedef LieGroup_t LieGroupMap; \
......@@ -98,6 +107,16 @@ namespace pinocchio
template<typename JointModel> \
struct AlgoDispatch : Algo<Visitor, JointModel> \
{ using Algo<Visitor, JointModel>::run; };
#define PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(Algo, Visitor) \
typedef LieGroup_t LieGroupMap; \
template<typename JointModel> \
static void algo(PINOCCHIO_DETAILS_WRITE_ARGS_5(JointModel)) \
{ AlgoDispatch<JointModel>::run(jmodel, a0, a1, a2, a3, a4); } \
template<typename JointModel> \
struct AlgoDispatch : Algo<Visitor, JointModel> \
{ using Algo<Visitor, JointModel>::run; };
} // namespace details
template<typename Visitor, typename JointModel> struct IntegrateStepAlgo;
......@@ -143,10 +162,11 @@ namespace pinocchio
typedef boost::fusion::vector<const ConfigVectorIn &,
const TangentVectorIn &,
JacobianMatrixType &,
const ArgumentPosition &
const ArgumentPosition &,
const AssignmentOperatorType&
> ArgsType;
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_4(dIntegrateStepAlgo, dIntegrateStep)
PINOCCHIO_DETAILS_VISITOR_METHOD_ALGO_5(dIntegrateStepAlgo, dIntegrateStep)
};
template<typename Visitor, typename JointModel>
......@@ -157,7 +177,8 @@ namespace pinocchio
const Eigen::MatrixBase<ConfigVectorIn> & q,
const Eigen::MatrixBase<TangentVector> & v,
const Eigen::MatrixBase<JacobianMatrixType> & mat,
const ArgumentPosition & arg)
const ArgumentPosition & arg,
const AssignmentOperatorType& op)
{
typedef typename Visitor::LieGroupMap LieGroupMap;
......@@ -165,11 +186,11 @@ namespace pinocchio
lgo.dIntegrate(jmodel.jointConfigSelector (q.derived()),
jmodel.jointVelocitySelector(v.derived()),
jmodel.jointBlock(PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,mat)),
arg);
arg, op);
}
};
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_4(dIntegrateStepAlgo);
PINOCCHIO_DETAILS_DISPATCH_JOINT_COMPOSITE_5(dIntegrateStepAlgo);
template<typename Visitor, typename JointModel> struct dDifferenceStepAlgo;
......
src/multibody/liegroup/liegroup-base.hpp
View file @ b12d918a
......@@ -92,13 +92,13 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
*
* @param[out] J the Jacobian of the Integrate operation w.r.t. the argument arg.
*/
template <ArgumentPosition arg, class Config_t, class Tangent_t, class JacobianOut_t>
template <ArgumentPosition arg, AssignmentOperatorType op, class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
{
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg);
return dIntegrate(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),arg,op);
}
/**
......@@ -117,7 +117,8 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
void dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J,
const ArgumentPosition arg) const;
const ArgumentPosition arg,
const AssignmentOperatorType op) const;
/**
* @brief Computes the Jacobian of a small variation of the configuration vector into tangent space at identity.
......@@ -133,7 +134,8 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dq(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) const;
/**
* @brief Computes the Jacobian of a small variation of the tangent vector into tangent space at identity.
......@@ -149,7 +151,8 @@ PINOCCHIO_LIE_GROUP_PUBLIC_INTERFACE_GENERIC(Derived,typename)
template <class Config_t, class Tangent_t, class JacobianOut_t>
void dIntegrate_dv(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const;
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) const;
/**
* @brief Interpolation between two joint's configurations
......
src/multibody/liegroup/liegroup-base.hxx
View file @ b12d918a
......@@ -41,15 +41,16 @@ namespace pinocchio {
void LieGroupBase<Derived>::dIntegrate(const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J,
const ArgumentPosition arg) const
const ArgumentPosition arg,
const AssignmentOperatorType op) const
{
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1");
switch (arg) {
case ARG0:
dIntegrate_dq(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J)); 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)); return;
dIntegrate_dv(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op); return;
default: return;
}
}
......@@ -59,14 +60,15 @@ namespace pinocchio {
void LieGroupBase<Derived>::dIntegrate_dq(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) 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().dIntegrate_dq_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J));
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op);
}
template <class Derived>
......@@ -74,14 +76,16 @@ namespace pinocchio {
void LieGroupBase<Derived>::dIntegrate_dv(
const Eigen::MatrixBase<Config_t > & q,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J) const
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op) 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().dIntegrate_dv_impl(q.derived(),
v.derived(),
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J));
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),
op);
}
/**
......
src/multibody/liegroup/special-euclidean.hpp
View file @ b12d918a
......@@ -87,20 +87,42 @@ namespace pinocchio
template<typename Matrix2Like, typename Vector2Like, typename Matrix3Like>
static void toInverseActionMatrix(const Eigen::MatrixBase<Matrix2Like> & R,
const Eigen::MatrixBase<Vector2Like> & t,
const Eigen::MatrixBase<Matrix3Like> & M)
const Eigen::MatrixBase<Matrix3Like> & M,
const AssignmentOperatorType op)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2);
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector2Like, 2);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
typedef typename Matrix3Like::Scalar Scalar;
Matrix3Like & Mout = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,M);
Mout.template topLeftCorner<2,2>() = R.transpose();
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv(R.transpose() * t);
Mout.template topRightCorner<2,1>() << - tinv(1), tinv(0);
Mout.template bottomLeftCorner<1,2>().setZero();
Mout(2,2) = (Scalar)1;
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Vector2Like) tinv((R.transpose() * t).reverse());
tinv[0] *= Scalar(-1.);
switch(op)
{
case SETTO:
Mout.template topLeftCorner<2,2>() = R.transpose();
Mout.template topRightCorner<2,1>() = tinv;
Mout.template bottomLeftCorner<1,2>().setZero();
Mout(2,2) = (Scalar)1;
break;
case ADDTO:
Mout.template topLeftCorner<2,2>() += R.transpose();
Mout.template topRightCorner<2,1>() += tinv;
Mout(2,2) += (Scalar)1;
break;
case RMTO:
Mout.template topLeftCorner<2,2>() -= R.transpose();
Mout.template topRightCorner<2,1>() -= tinv;
Mout(2,2) -= (Scalar)1;
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template<typename Matrix2Like, typename Vector2Like, typename TangentVector>
......@@ -277,7 +299,8 @@ namespace pinocchio
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J)
const Eigen::MatrixBase<JacobianOut_t>& J,
const AssignmentOperatorType op)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
......@@ -285,21 +308,45 @@ namespace pinocchio
Vector2 t;
exp(v, R, t);
toInverseActionMatrix(R, t, Jout);
toInverseActionMatrix(R, t, Jout, op);
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t> & J)
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
// TODO sparse version
MotionTpl<Scalar,0> nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2];
Eigen::Matrix<Scalar,6,6> Jtmp6;
Jexp6(nu, Jtmp6);
Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
switch(op)
{
case SETTO:
Jout << Jtmp6.template topLeftCorner<2,2>(), Jtmp6.template topRightCorner<2,1>(),
Jtmp6.template bottomLeftCorner<1,2>(), Jtmp6.template bottomRightCorner<1,1>();
break;
case ADDTO:
Jout.template topLeftCorner<2,2>() += Jtmp6.template topLeftCorner<2,2>();
Jout.template topRightCorner<2,1>() += Jtmp6.template topRightCorner<2,1>();
Jout.template bottomLeftCorner<1,2>() += Jtmp6.template bottomLeftCorner<1,2>();
Jout.template bottomRightCorner<1,1>() += Jtmp6.template bottomRightCorner<1,1>();
break;
case RMTO:
Jout.template topLeftCorner<2,2>() -= Jtmp6.template topLeftCorner<2,2>();
Jout.template topRightCorner<2,1>() -= Jtmp6.template topRightCorner<2,1>();
Jout.template bottomLeftCorner<1,2>() -= Jtmp6.template bottomLeftCorner<1,2>();
Jout.template bottomRightCorner<1,1>() -= Jtmp6.template bottomRightCorner<1,1>();
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
// interpolate_impl use default implementation.
......@@ -529,18 +576,49 @@ namespace pinocchio
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J)
const Eigen::MatrixBase<JacobianOut_t>& J,
const AssignmentOperatorType op)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
switch(op)
{
case SETTO:
Jout = exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
case ADDTO:
Jout += exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
case RMTO:
Jout -= exp6(MotionRef<const Tangent_t>(v.derived())).toDualActionMatrix().transpose();
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & v,
const Eigen::MatrixBase<JacobianOut_t>& J)
{
Jexp6(MotionRef<const Tangent_t>(v.derived()), J.derived());
const Eigen::MatrixBase<JacobianOut_t>& J,
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());
break;
case RMTO:
Jexp6<RMTO>(MotionRef<const Tangent_t>(v.derived()), J.derived());
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
// interpolate_impl use default implementation.
......
src/multibody/liegroup/special-orthogonal.hpp
View file @ b12d918a
//
// Copyright (c) 2016-2018 CNRS
// Copyright (c) 2016-2020 CNRS INRIA
//
#ifndef __pinocchio_special_orthogonal_operation_hpp__
......@@ -81,7 +81,7 @@ namespace pinocchio
{
typedef typename Matrix2Like::Scalar Scalar;
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like,2,2);
return (Scalar)1;
return Scalar(1);
}
/// Get dimension of Lie Group vector representation
......@@ -172,21 +172,51 @@ namespace pinocchio
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t>& J)
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
Jout(0,0) = 1;
switch(op)
{
case SETTO:
Jout(0,0) = Scalar(1);
break;
case ADDTO:
Jout(0,0) += Scalar(1);
break;
case RMTO:
Jout(0,0) -= Scalar(1);
break;
default:
assert(false && "Wrong Op requesed value");
break;
}
}
template <class Config_t, class Tangent_t, class JacobianOut_t>
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t>& J)
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/,
const Eigen::MatrixBase<Tangent_t> & /*v*/,
const Eigen::MatrixBase<JacobianOut_t> & J,
const AssignmentOperatorType op)
{
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J);
Jout(0,0) = 1;