finite-differences.cpp
Go to the documentation of this file.
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>
20 {
21  Data::Matrix6x res(6,model.nv); res.setZero();
22  VectorXd q_integrate (model.nq);
23  VectorXd v_integrate (model.nv); v_integrate.setZero();
24 
25  forwardKinematics(model,data,q);
26  const SE3 oMi_ref = data.oMi[joint_id];
27 
28  double eps = 1e-8;
29  for(int k=0; k<model.nv; ++k)
30  {
31  // Integrate along kth direction
32  v_integrate[k] = eps;
33  q_integrate = integrate(model,q,v_integrate);
34 
35  forwardKinematics(model,data,q_integrate);
36  const SE3 & oMi = data.oMi[joint_id];
37 
38  if (local)
39  res.col(k) = log6(oMi_ref.inverse()*oMi).toVector();
40  else
41  res.col(k) = oMi_ref.act(log6(oMi_ref.inverse()*oMi)).toVector();
42 
43  res.col(k) /= eps;
44 
45  v_integrate[k] = 0.;
46  }
47 
48  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  static JointModel_ run()
64  {
65  JointModel_ jmodel;
66  jmodel.setIndexes(0,0,0);
67  return jmodel;
68  }
69 };
70 
71 template<typename Scalar, int Options>
72 struct init<pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
73 {
75 
76  static JointModel run()
77  {
78  typedef typename JointModel::Vector3 Vector3;
79  JointModel jmodel(Vector3::Random().normalized());
80 
81  jmodel.setIndexes(0,0,0);
82  return jmodel;
83  }
84 };
85 
86 template<typename Scalar, int Options>
87 struct init<pinocchio::JointModelRevoluteUnboundedUnalignedTpl<Scalar,Options> >
88 {
90 
91  static JointModel run()
92  {
93  typedef typename JointModel::Vector3 Vector3;
94  JointModel jmodel(Vector3::Random().normalized());
95 
96  jmodel.setIndexes(0,0,0);
97  return jmodel;
98  }
99 };
100 
101 template<typename Scalar, int Options>
102 struct init<pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
103 {
105 
106  static JointModel run()
107  {
108  typedef typename JointModel::Vector3 Vector3;
109  JointModel jmodel(Vector3::Random().normalized());
110 
111  jmodel.setIndexes(0,0,0);
112  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 {
120 
121  static JointModel run()
122  {
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 {
135 
136  static JointModel run()
137  {
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 {
152 
153  static JointModel run()
154  {
155  JointModel_ jmodel_ref = init<JointModel_>::run();
156 
157  JointModel jmodel(jmodel_ref,1.,0.);
158  jmodel.setIndexes(0,0,0);
159 
160  return jmodel;
161  }
162 };
163 
165 {
166  void operator()(JointModelComposite & /*jmodel*/) const
167  {}
168 
169  template<typename JointModel>
170  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 
177  std::cout << "name: " << jmodel.classname() << std::endl;
178 
179  typename JointModel::JointDataDerived jdata_ = jmodel.createData();
181  DataBaseType & jdata = static_cast<DataBaseType &>(jdata_);
182 
183  CV q = LieGroupType().random();
184  jmodel.calc(jdata.derived(),q);
185  SE3 M_ref(jdata.M());
186 
187  CV q_int(q);
188  const Eigen::DenseIndex nv = jdata.S().nv();
189  TV v(nv); v.setZero();
190  double eps = 1e-8;
191 
192  Eigen::Matrix<double,6,JointModel::NV> S(6,nv), S_ref(jdata.S().matrix());
193 
194  for(int k=0;k<nv;++k)
195  {
196  v[k] = eps;
197  q_int = LieGroupType().integrate(q,v);
198  jmodel.calc(jdata.derived(),q_int);
199  SE3 M_int = jdata.M();
200 
201  S.col(k) = log6(M_ref.inverse()*M_int).toVector();
202  S.col(k) /= eps;
203 
204  v[k] = 0.;
205  }
206 
207  BOOST_CHECK(S.isApprox(S_ref,eps*1e1));
208  std::cout << "S_ref:\n" << S_ref << std::endl;
209  std::cout << "S:\n" << S << std::endl;
210  }
211 };
212 
213 BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
214 
215 BOOST_AUTO_TEST_CASE (test_S_finit_diff)
216 {
217  boost::mpl::for_each<JointModelVariant::types>(FiniteDiffJoint());
218 }
219 
220 BOOST_AUTO_TEST_CASE (test_jacobian_vs_finit_diff)
221 {
224  pinocchio::Data data(model);
225 
226  VectorXd q = VectorXd::Ones(model.nq);
227  q.segment<4>(3).normalize();
228  computeJointJacobians(model,data,q);
229 
230  Model::Index idx = model.existJointName("rarm2")?model.getJointId("rarm2"):(Model::Index)(model.njoints-1);
231  Data::Matrix6x Jrh(6,model.nv); Jrh.fill(0);
232 
233  getJointJacobian(model,data,idx,WORLD,Jrh);
234  Data::Matrix6x Jrh_finite_diff = finiteDiffJacobian<false>(model,data,q,idx);
235  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));
236 
237  getJointJacobian(model,data,idx,LOCAL,Jrh);
238  Jrh_finite_diff = finiteDiffJacobian<true>(model,data,q,idx);
239  BOOST_CHECK(Jrh_finite_diff.isApprox(Jrh,1e-8*1e1));
240 }
241 
242 BOOST_AUTO_TEST_SUITE_END()
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
BOOST_AUTO_TEST_CASE(test_S_finit_diff)
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
pinocchio::JointModelMimic< JointModel_ > JointModel
void setIndexes(JointIndex id, int q, int v)
Data::Matrix6x finiteDiffJacobian(const Model &model, Data &data, const Eigen::VectorXd &q, const Model::JointIndex joint_id)
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time...
pinocchio::JointModelRevoluteUnboundedUnalignedTpl< Scalar, Options > JointModel
int eps
Definition: dcrba.py:384
int njoints
Number of joints.
MotionTpl< Scalar, Options > log6(const SE3Tpl< Scalar, Options > &M)
Log: SE3 -> se3.
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
bool existJointName(const std::string &name) const
Check if a joint given by its name exists.
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed either in the world (rf = WORLD) frame or i...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
SE3::Scalar Scalar
Definition: conversions.cpp:13
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
JointModelRevoluteTpl< double, 0, 1 > JointModelRY
SE3Tpl inverse() const
aXb = bXa.inverse()
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
void operator()(JointModelComposite &) const
pinocchio::JointIndex JointIndex
static std::string classname()
#define BOOST_TEST_MODULE
Eigen::Matrix< Scalar, 3, 1, _Options > Vector3
pinocchio::JointModelCompositeTpl< Scalar, Options, JointCollection > JointModel
pinocchio::JointModelRevoluteUnalignedTpl< Scalar, Options > JointModel
void setIndexes(JointIndex id, int nq, int nv)
Main pinocchio namespace.
Definition: timings.cpp:28
JointIndex getJointId(const std::string &name) const
Return the index of a joint given by its name.
res
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame...
int nv
Dimension of the velocity vector space.
void filterValue(MatrixBase< Matrix > &mat, typename Matrix::Scalar value)
void operator()(JointModelBase< JointModel > &) const
pinocchio::JointModelPrismaticUnalignedTpl< Scalar, Options > JointModel
SE3GroupAction< D >::ReturnType act(const D &d) const
ay = aXb.act(by)
Definition: se3-base.hpp:90
static JointModel_ run()
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
pinocchio::JointModelTpl< Scalar, Options, JointCollection > JointModel
JointDataDerived createData() const
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
void calc(JointDataDerived &data, const Eigen::MatrixBase< ConfigVector > &q) const
JointModelRevoluteTpl< double, 0, 0 > JointModelRX
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30