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

all: correct rebase

parent 184bb873
......@@ -198,7 +198,7 @@ namespace pinocchio
template<typename Scalar, int Options, typename ForceDerived>
struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceDerived>
{
typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
};
......
......@@ -60,7 +60,7 @@ namespace pinocchio
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE;
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
Transformation_t M;
......@@ -68,8 +68,6 @@ namespace pinocchio
Motion_t v;
Bias_t c;
F_t F;
// [ABA] specific data
U_t U;
D_t Dinv;
......@@ -98,7 +96,7 @@ namespace pinocchio
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef JointRevoluteUnboundedUnalignedTpl<_Scalar,_Options> JointDerived;
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE;
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
typedef JointModelBase<JointModelRevoluteUnboundedUnalignedTpl> Base;
......
......@@ -84,6 +84,21 @@ struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
}
};
template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
{
typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;
static JointModel run()
{
typedef typename JointModel::Vector3 Vector3;
JointModel jmodel(Vector3::Random().normalized());
jmodel.setIndexes(0,0,0);
return jmodel;
}
};
template<typename Scalar, int Options>
struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
{
......@@ -166,17 +181,18 @@ struct FiniteDiffJoint
typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType;
DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);
CV q(jmodel.nq()); LieGroupType().random(q);
CV q = LieGroupType().random();
jmodel.calc(jdata.derived(),q);
SE3 M_ref(jdata.M());
CV q_int(jmodel.nq());
TV v(jmodel.nv()); v.setZero();
CV q_int(q);
const Eigen::DenseIndex nv = jdata.S().nv();
TV v(nv); v.setZero();
double eps = 1e-8;
Eigen::Matrix<double,6,JointModel::NV> S(6,jmodel.nv()), S_ref(jdata.S().matrix());
Eigen::Matrix<double,6,JointModel::NV> S(6,nv), S_ref(jdata.S().matrix());
for(int k=0;k<jmodel.nv();++k)
for(int k=0;k<nv;++k)
{
v[k] = eps;
q_int = LieGroupType().integrate(q,v);
......
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