Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	unittest/finite-differences.cpp	Lines:	84	84	100.0 %
Date:	2024-04-26 13:14:21	Branches:	187	378	49.5 %


//
// Copyright (c) 2016-2019 CNRS, INRIA
//

#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/parsers/sample-models.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

using namespace pinocchio;
using namespace Eigen;

template<bool local>
Data::Matrix6x finiteDiffJacobian(const Model & model, Data & data, const Eigen::VectorXd & q, const Model::JointIndex joint_id)
{
  Data::Matrix6x res(6,model.nv); res.setZero();
  VectorXd q_integrate (model.nq);
  VectorXd v_integrate (model.nv); v_integrate.setZero();

  forwardKinematics(model,data,q);
  const SE3 oMi_ref = data.oMi[joint_id];

  double eps = 1e-8;
  for(int k=0; k<model.nv; ++k)
  {
    // Integrate along kth direction
    v_integrate[k] = eps;
    q_integrate = integrate(model,q,v_integrate);

    forwardKinematics(model,data,q_integrate);
    const SE3 & oMi = data.oMi[joint_id];

    if (local)
      res.col(k) = log6(oMi_ref.inverse()*oMi).toVector();
    else
      res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector();

    res.col(k) /= eps;

    v_integrate[k] = 0.;
  }

  return res;
}

template<typename Matrix>
void filterValue(MatrixBase<Matrix> & mat, typename Matrix::Scalar value)
{
  for(int k = 0; k < mat.size(); ++k)
    mat.derived().data()[k] =  math::fabs(mat.derived().data()[k]) <= value?0:mat.derived().data()[k];
}

template<typename JointModel_> struct init;

template<typename JointModel_>
struct init
{
  static JointModel_ run()
  {
    JointModel_ jmodel;
    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options>
struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
{
  typedef pinocchio::JointModelRevoluteUnalignedTpl<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::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> >
{
  typedef pinocchio::JointModelPrismaticUnalignedTpl<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,int> class JointCollection>
struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    JointModel jmodel((JointModelRX()));

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename Scalar, int Options, template<typename,int> class JointCollection>
struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
{
  typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;

  static JointModel run()
  {
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
    typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
    JointModel jmodel((JointModelRX()));
    jmodel.addJoint(JointModelRY());

    jmodel.setIndexes(0,0,0);
    return jmodel;
  }
};

template<typename JointModel_>
struct init<pinocchio::JointModelMimic<JointModel_> >
{
  typedef pinocchio::JointModelMimic<JointModel_> JointModel;

  static JointModel run()
  {
    JointModel_ jmodel_ref = init<JointModel_>::run();

    JointModel jmodel(jmodel_ref,1.,0.);
    jmodel.setIndexes(0,0,0);

    return jmodel;
  }
};

struct FiniteDiffJoint
{
  void operator()(JointModelComposite & /*jmodel*/) const
  {}

  template<typename JointModel>
  void operator()(JointModelBase<JointModel> & /*jmodel*/) const
  {
    typedef typename JointModel::ConfigVector_t CV;
    typedef typename JointModel::TangentVector_t TV;
    typedef typename LieGroup<JointModel>::type LieGroupType;

    JointModel jmodel = init<JointModel>::run();
    std::cout << "name: " << jmodel.classname() << std::endl;

    typename JointModel::JointDataDerived jdata_ = jmodel.createData();
    typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType;
    DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);

    CV q = LieGroupType().random();
    jmodel.calc(jdata.derived(),q);
    SE3 M_ref(jdata.M());

    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,nv), S_ref(jdata.S().matrix());

    for(int k=0;k<nv;++k)
    {
      v[k] = eps;
      q_int = LieGroupType().integrate(q,v);
      jmodel.calc(jdata.derived(),q_int);
      SE3 M_int = jdata.M();

      S.col(k) = log6(M_ref.inverse()*M_int).toVector();
      S.col(k) /= eps;

      v[k] = 0.;
    }

    BOOST_CHECK(S.isApprox(S_ref,eps*1e1));
    std::cout << "S_ref:\n" << S_ref << std::endl;
    std::cout << "S:\n" << S << std::endl;
  }
};

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE (test_S_finit_diff)
{
  boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint());
}

BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
{
  pinocchio::Model model;
  pinocchio::buildModels::humanoidRandom(model);
  pinocchio::Data data(model);

  VectorXd q = VectorXd::Ones(model.nq);
  q.segment<4>(3).normalize();
  computeJointJacobians(model,data,q);

  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
  Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);

  getJointJacobian(model,data,idx,WORLD,Jrh);
  Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx);
  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));

  getJointJacobian(model,data,idx,LOCAL,Jrh);
  Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx);
  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));
}

BOOST_AUTO_TEST_SUITE_END()


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2019 CNRS, INRIA
3			//
4
5			#include "pinocchio/multibody/model.hpp"
6			#include "pinocchio/multibody/data.hpp"
7			#include "pinocchio/parsers/sample-models.hpp"
8			#include "pinocchio/algorithm/joint-configuration.hpp"
9			#include "pinocchio/algorithm/kinematics.hpp"
10			#include "pinocchio/algorithm/jacobian.hpp"
11
12			#include <boost/test/unit_test.hpp>
13			#include <boost/utility/binary.hpp>
14
15			using namespace pinocchio;
16			using namespace Eigen;
17
18			template<bool local>
19		4	Data::Matrix6x finiteDiffJacobian(const Model & model, Data & data, const Eigen::VectorXd & q, const Model::JointIndex joint_id)
20			{
21	✓✗✓✗	4	Data::Matrix6x res(6,model.nv); res.setZero();
22	✓✗	8	VectorXd q_integrate (model.nq);
23	✓✗✓✗	8	VectorXd v_integrate (model.nv); v_integrate.setZero();
24
25	✓✗	4	forwardKinematics(model,data,q);
26	✓✗	4	const SE3 oMi_ref = data.oMi[joint_id];
27
28		4	double eps = 1e-8;
29	✓✓	132	for(int k=0; k<model.nv; ++k)
30			{
31			// Integrate along kth direction
32	✓✗	128	v_integrate[k] = eps;
33	✓✗	128	q_integrate = integrate(model,q,v_integrate);
34
35	✓✗	128	forwardKinematics(model,data,q_integrate);
36		128	const SE3 & oMi = data.oMi[joint_id];
37
38			if (local)
39	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	64	res.col(k) = log6(oMi_ref.inverse()*oMi).toVector();
40			else
41	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	64	res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector();
42
43	✓✗✓✗	128	res.col(k) /= eps;
44
45	✓✗	128	v_integrate[k] = 0.;
46			}
47
48		8	return res;
49			}
50
51			template<typename Matrix>
52			void filterValue(MatrixBase<Matrix> & mat, typename Matrix::Scalar value)
53			{
54			for(int k = 0; k < mat.size(); ++k)
55			mat.derived().data()[k] = math::fabs(mat.derived().data()[k]) <= value?0:mat.derived().data()[k];
56			}
57
58			template<typename JointModel_> struct init;
59
60			template<typename JointModel_>
61			struct init
62			{
63		34	static JointModel_ run()
64			{
65		34	JointModel_ jmodel;
66		34	jmodel.setIndexes(0,0,0);
67		34	return jmodel;
68			}
69			};
70
71			template<typename Scalar, int Options>
72			struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
73			{
74			typedef pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> JointModel;
75
76		1	static JointModel run()
77			{
78			typedef typename JointModel::Vector3 Vector3;
79	✓✗✓✗	1	JointModel jmodel(Vector3::Random().normalized());
80
81		1	jmodel.setIndexes(0,0,0);
82		1	return jmodel;
83			}
84			};
85
86			template<typename Scalar, int Options>
87			struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
88			{
89			typedef pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> JointModel;
90
91		1	static JointModel run()
92			{
93			typedef typename JointModel::Vector3 Vector3;
94	✓✗✓✗	1	JointModel jmodel(Vector3::Random().normalized());
95
96		1	jmodel.setIndexes(0,0,0);
97		1	return jmodel;
98			}
99			};
100
101			template<typename Scalar, int Options>
102			struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
103			{
104			typedef pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> JointModel;
105
106		1	static JointModel run()
107			{
108			typedef typename JointModel::Vector3 Vector3;
109	✓✗✓✗	1	JointModel jmodel(Vector3::Random().normalized());
110
111		1	jmodel.setIndexes(0,0,0);
112		1	return jmodel;
113			}
114			};
115
116			template<typename Scalar, int Options, template<typename,int> class JointCollection>
117			struct init<pinocchio::JointModelTpl<Scalar,Options,JointCollection> >
118			{
119			typedef pinocchio::JointModelTpl<Scalar,Options,JointCollection> JointModel;
120
121			static JointModel run()
122			{
123			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
124			JointModel jmodel((JointModelRX()));
125
126			jmodel.setIndexes(0,0,0);
127			return jmodel;
128			}
129			};
130
131			template<typename Scalar, int Options, template<typename,int> class JointCollection>
132			struct init<pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> >
133			{
134			typedef pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollection> JointModel;
135
136			static JointModel run()
137			{
138			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,0> JointModelRX;
139			typedef pinocchio::JointModelRevoluteTpl<Scalar,Options,1> JointModelRY;
140			JointModel jmodel((JointModelRX()));
141			jmodel.addJoint(JointModelRY());
142
143			jmodel.setIndexes(0,0,0);
144			return jmodel;
145			}
146			};
147
148			template<typename JointModel_>
149			struct init<pinocchio::JointModelMimic<JointModel_> >
150			{
151			typedef pinocchio::JointModelMimic<JointModel_> JointModel;
152
153		6	static JointModel run()
154			{
155	✓✗	6	JointModel_ jmodel_ref = init<JointModel_>::run();
156
157	✓✗	6	JointModel jmodel(jmodel_ref,1.,0.);
158	✓✗	6	jmodel.setIndexes(0,0,0);
159
160		12	return jmodel;
161			}
162			};
163
164			struct FiniteDiffJoint
165			{
166		1	void operator()(JointModelComposite & /jmodel/) const
167		1	{}
168
169			template<typename JointModel>
170		40	void operator()(JointModelBase<JointModel> & /jmodel/) const
171			{
172			typedef typename JointModel::ConfigVector_t CV;
173			typedef typename JointModel::TangentVector_t TV;
174			typedef typename LieGroup<JointModel>::type LieGroupType;
175
176	✓✗	40	JointModel jmodel = init<JointModel>::run();
177	✓✗✓✗ ✓✗✓✗	40	std::cout << "name: " << jmodel.classname() << std::endl;
178
179	✓✗	40	typename JointModel::JointDataDerived jdata_ = jmodel.createData();
180			typedef JointDataBase<typename JointModel::JointDataDerived> DataBaseType;
181		40	DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);
182
183	✓✗✓✗	40	CV q = LieGroupType().random();
184	✓✗	40	jmodel.calc(jdata.derived(),q);
185	✓✗✓✗	40	SE3 M_ref(jdata.M());
186
187	✓✗	40	CV q_int(q);
188	✓✗✓✗	40	const Eigen::DenseIndex nv = jdata.S().nv();
189	✓✗✓✗	40	TV v(nv); v.setZero();
190		40	double eps = 1e-8;
191
192	✓✗✓✗ ✓✗✓✗	40	Eigen::Matrix<double,6,JointModel::NV> S(6,nv), S_ref(jdata.S().matrix());
193
194	✓✓	106	for(int k=0;k<nv;++k)
195			{
196	✓✗	66	v[k] = eps;
197	✓✗✓✗	66	q_int = LieGroupType().integrate(q,v);
198	✓✗	66	jmodel.calc(jdata.derived(),q_int);
199	✓✗✓✗	66	SE3 M_int = jdata.M();
200
201	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	66	S.col(k) = log6(M_ref.inverse()*M_int).toVector();
202	✓✗✓✗	66	S.col(k) /= eps;
203
204	✓✗	66	v[k] = 0.;
205			}
206
207	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	40	BOOST_CHECK(S.isApprox(S_ref,eps*1e1));
208	✓✗✓✗ ✓✗	40	std::cout << "S_ref:\n" << S_ref << std::endl;
209	✓✗✓✗ ✓✗	40	std::cout << "S:\n" << S << std::endl;
210		40	}
211			};
212
213			BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
214
215	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE (test_S_finit_diff)
216			{
217	✓✗	2	boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint());
218		2	}
219
220	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	4	BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
221			{
222	✓✗	4	pinocchio::Model model;
223	✓✗	2	pinocchio::buildModels::humanoidRandom(model);
224	✓✗	4	pinocchio::Data data(model);
225
226	✓✗✓✗	4	VectorXd q = VectorXd::Ones(model.nq);
227	✓✗✓✗	2	q.segment<4>(3).normalize();
228	✓✗	2	computeJointJacobians(model,data,q);
229
230	✓✗✓✗ ✗✓✗✗ ✗✗✗✓ ✗✓✗✗ ✗✗	2	Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
231	✓✗✓✗	4	Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
232
233	✓✗	2	getJointJacobian(model,data,idx,WORLD,Jrh);
234	✓✗	4	Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx);
235	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));
236
237	✓✗	2	getJointJacobian(model,data,idx,LOCAL,Jrh);
238	✓✗	2	Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx);
239	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	2	BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));
240		2	}
241
242			BOOST_AUTO_TEST_SUITE_END()