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

all: correct rebase

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