Commit d75444f5 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

move op from template to argument in dintegrate

parent edfa70a1
......@@ -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;
}
......
......@@ -280,12 +280,13 @@ namespace pinocchio
* @param[in] arg Argument (either q or v) with respect to which the differentiation is performed.
*
*/
template<typename LieGroup_t, AssignmentOperatorType op, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
template<typename LieGroup_t, 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 ArgumentPosition arg,
const AssignmentOperatorType op);
/**
*
......@@ -312,9 +313,10 @@ namespace pinocchio
const Eigen::MatrixBase<JacobianMatrixType> & J,
const ArgumentPosition arg)
{
dIntegrate<LieGroupMap,SETTO,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.
......@@ -333,15 +335,15 @@ namespace pinocchio
* @param[in] arg Argument (either q or v) with respect to which the differentiation is performed.
*
*/
template<AssignmentOperatorType op, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
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 ArgumentPosition arg,
const AssignmentOperatorType op)
{
dIntegrate<LieGroupMap,op,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,op);
}
/**
......
......@@ -147,12 +147,13 @@ namespace pinocchio
}
}
template<typename LieGroup_t, AssignmentOperatorType op, typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType, typename JacobianMatrixType>
template<typename LieGroup_t, 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 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");
......@@ -162,8 +163,8 @@ namespace pinocchio
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef typename Model::JointIndex JointIndex;
typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType,op> Algo;
typename Algo::ArgsType args(q.derived(),v.derived(),PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrixType,J),arg);
typedef dIntegrateStep<LieGroup_t,ConfigVectorType,TangentVectorType,JacobianMatrixType> Algo;
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);
......
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