timings-delassus-operations.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2024 INRIA
3 //
4 
5 #include "pinocchio/algorithm/joint-configuration.hpp"
6 #include "pinocchio/algorithm/kinematics.hpp"
7 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
8 #include "pinocchio/algorithm/rnea-derivatives.hpp"
9 #include "pinocchio/algorithm/aba-derivatives.hpp"
10 #include "pinocchio/algorithm/aba.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/crba.hpp"
13 #include "pinocchio/algorithm/contact-dynamics.hpp"
14 #include "pinocchio/algorithm/constrained-dynamics.hpp"
15 #include "pinocchio/algorithm/cholesky.hpp"
16 #include "pinocchio/parsers/urdf.hpp"
17 #include "pinocchio/multibody/sample-models.hpp"
18 #include "pinocchio/algorithm/delassus.hpp"
19 
20 #include <iostream>
21 
22 #include "pinocchio/utils/timer.hpp"
23 
24 int main(int argc, const char ** argv)
25 {
26  using namespace Eigen;
27  using namespace pinocchio;
28 
29  PinocchioTicToc timer(PinocchioTicToc::US);
30 #ifdef NDEBUG
31  const int NBT = 1000 * 100;
32 #else
33  const int NBT = 1;
34  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
35 #endif
36 
37  // Build model
38  Model model;
39 
40  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
41  if (argc > 1)
42  filename = argv[1];
43  bool with_ff = true;
44 
45  if (argc > 2)
46  {
47  const std::string ff_option = argv[2];
48  if (ff_option == "-no-ff")
49  with_ff = false;
50  }
51 
52  if (filename == "HS")
53  buildModels::humanoidRandom(model, true);
54  else if (with_ff)
55  pinocchio::urdf::buildModel(filename, JointModelFreeFlyer(), model);
56  // pinocchio::urdf::buildModel(filename,JointModelRX(),model);
57  else
58  pinocchio::urdf::buildModel(filename, model);
59 
60  const std::string RA = "RARM_LINK6";
61  const JointIndex RA_id = model.frames[model.getFrameId(RA)].parent;
62  const std::string LA = "LARM_LINK6";
63  const JointIndex LA_id = model.frames[model.getFrameId(LA)].parent;
64  const std::string RF = "RLEG_LINK6";
65  const JointIndex RF_id = model.frames[model.getFrameId(RF)].parent;
66  const std::string LF = "LLEG_LINK6";
67  const JointIndex LF_id = model.frames[model.getFrameId(LF)].parent;
68 
69  RigidConstraintModel ci_RF_6D(CONTACT_6D, model, RF_id, LOCAL);
70  RigidConstraintModel ci_RF_3D(CONTACT_3D, model, RF_id, LOCAL);
71 
72  RigidConstraintModel ci_LF_6D(CONTACT_6D, model, LF_id, LOCAL);
73  RigidConstraintModel ci_LF_3D(CONTACT_3D, model, LF_id, LOCAL);
74 
75  RigidConstraintModel ci_RA_3D(CONTACT_3D, model, RA_id, LOCAL);
76  RigidConstraintModel ci_LA_3D(CONTACT_3D, model, LA_id, LOCAL);
77 
78  // Define contact infos structure
79  static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
80  static PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty;
81  ContactCholeskyDecomposition contact_chol_empty(model, contact_models_empty);
82 
83  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D;
84  contact_models_6D.push_back(ci_RF_6D);
85  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D;
86  contact_data_6D.push_back(RigidConstraintData(ci_RF_6D));
87  ContactCholeskyDecomposition contact_chol_6D(model, contact_models_6D);
88 
89  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
90  contact_models_6D6D.push_back(ci_RF_6D);
91  contact_models_6D6D.push_back(ci_LF_6D);
92  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D;
93  contact_data_6D6D.push_back(RigidConstraintData(ci_RF_6D));
94  contact_data_6D6D.push_back(RigidConstraintData(ci_LF_6D));
95  ContactCholeskyDecomposition contact_chol_6D6D(model, contact_models_6D6D);
96 
97  ProximalSettings prox_settings;
98  prox_settings.max_iter = 10;
99  prox_settings.mu = 1e8;
100 
101  std::cout << "nq = " << model.nq << std::endl;
102  std::cout << "nv = " << model.nv << std::endl;
103  std::cout << "--" << std::endl;
104 
105  Data data(model);
106  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
107 
108  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
109  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
110  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
111  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
112 
113  static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
114  static PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::MatrixXs)
115  col_major_square_matrices(NBT);
116  static PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(context::RowMatrixXs)
117  row_major_square_matrices(NBT);
118 
119  const int num_constraints = 12;
120 
121  for (size_t i = 0; i < NBT; ++i)
122  {
123  qs[i] = randomConfiguration(model, -qmax, qmax);
124  qdots[i] = Eigen::VectorXd::Random(model.nv);
125  qddots[i] = Eigen::VectorXd::Random(model.nv);
126  taus[i] = Eigen::VectorXd::Random(model.nv);
127 
128  col_major_square_matrices[i] =
129  context::MatrixXs::Random(num_constraints, num_constraints)
130  + 1. * context::MatrixXs::Identity(num_constraints, num_constraints);
131  row_major_square_matrices[i] = col_major_square_matrices[i];
132  }
133 
134  double total_time = 0;
135  SMOOTH(NBT)
136  {
137  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
138  timer.tic();
139  contact_chol_6D6D.compute(model, data, contact_models_6D6D, contact_data_6D6D);
140  total_time += timer.toc(timer.DEFAULT_UNIT);
141  }
142  std::cout << "contactCholesky {6D,6D} = \t\t" << (total_time / NBT) << " "
143  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
144 
145  total_time = 0;
146  MatrixXd H_inverse(contact_chol_6D6D.size(), contact_chol_6D6D.size());
147  SMOOTH(NBT)
148  {
149  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
150  contact_chol_6D6D.compute(model, data, contact_models_6D6D, contact_data_6D6D);
151  timer.tic();
152  contact_chol_6D6D.inverse(H_inverse);
153  total_time += timer.toc(timer.DEFAULT_UNIT);
154  }
155  std::cout << "contactCholeskyInverse {6D,6D} = \t\t" << (total_time / NBT) << " "
156  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
157 
158  const context::VectorXs rhs_vector = context::VectorXs::Random(num_constraints);
159  total_time = 0;
160  SMOOTH(NBT)
161  {
162  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
163  contact_chol_6D6D.compute(model, data, contact_models_6D6D, contact_data_6D6D);
164  timer.tic();
165  contact_chol_6D6D.getDelassusCholeskyExpression().updateDamping(1.);
166  total_time += timer.toc(timer.DEFAULT_UNIT);
167  }
168  std::cout << "delassus.compute() {6D,6D} = \t\t" << (total_time / NBT) << " "
169  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
170 
171  total_time = 0;
172  SMOOTH(NBT)
173  {
174  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
175  contact_chol_6D6D.compute(model, data, contact_models_6D6D, contact_data_6D6D);
176  timer.tic();
177  contact_chol_6D6D.getDelassusCholeskyExpression().solveInPlace(rhs_vector);
178  total_time += timer.toc(timer.DEFAULT_UNIT);
179  }
180  std::cout << "delassus.solveInPlace() {6D,6D} = \t\t" << (total_time / NBT) << " "
181  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
182 
183  Eigen::LLT<context::MatrixXs> col_major_ldlt(num_constraints);
184  total_time = 0;
185  SMOOTH(NBT)
186  {
187  timer.tic();
188  col_major_ldlt.compute(col_major_square_matrices[_smooth]);
189  total_time += timer.toc(timer.DEFAULT_UNIT);
190  }
191  std::cout << "col_major_ldlt.compute() = \t\t" << (total_time / NBT) << " "
192  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
193 
194  total_time = 0;
195  SMOOTH(NBT)
196  {
197  col_major_ldlt.compute(col_major_square_matrices[_smooth]);
198  timer.tic();
199  col_major_ldlt.solveInPlace(rhs_vector);
200  total_time += timer.toc(timer.DEFAULT_UNIT);
201  }
202  std::cout << "col_major_ldlt.solveInPlace() = \t\t" << (total_time / NBT) << " "
203  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
204 
205  MatrixXd J(contact_chol_6D6D.constraintDim(), model.nv);
206  J.setZero();
207 
208  MatrixXd MJtJ_inv(
209  model.nv + contact_chol_6D6D.constraintDim(), model.nv + contact_chol_6D6D.constraintDim());
210  MJtJ_inv.setZero();
211 
212  VectorXd gamma(contact_chol_6D6D.constraintDim());
213  gamma.setZero();
214 
215  total_time = 0;
216  SMOOTH(NBT)
217  {
218  computeAllTerms(model, data, qs[_smooth], qdots[_smooth]);
219  getJointJacobian(model, data, ci_RF_6D.joint1_id, ci_RF_6D.reference_frame, J.middleRows<6>(0));
220  getJointJacobian(model, data, ci_LF_6D.joint1_id, ci_LF_6D.reference_frame, J.middleRows<6>(6));
221 
222  forwardDynamics(model, data, qs[_smooth], qdots[_smooth], taus[_smooth], J, gamma);
223 
224  timer.tic();
225  cholesky::decompose(model, data);
226  getKKTContactDynamicMatrixInverse(model, data, J, MJtJ_inv);
227  total_time += timer.toc(timer.DEFAULT_UNIT);
228  }
229  std::cout << "KKTContactDynamicMatrixInverse {6D,6D} = \t\t" << (total_time / NBT) << " "
230  << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
231 
232  VectorXd q = randomConfiguration(model);
233  VectorXd v = Eigen::VectorXd::Random(model.nv);
234 
235  computeAllTerms(model, data, q, v);
236  contact_chol_6D6D.compute(model, data, contact_models_6D6D, contact_data_6D6D, 1e-6);
237  contact_chol_6D6D.inverse(H_inverse);
238 
239  Data::MatrixXs dampedDelassusInverse;
240  dampedDelassusInverse.resize(
241  contact_chol_6D6D.constraintDim(), contact_chol_6D6D.constraintDim());
242 
243  initPvDelassus(model, data, contact_models_6D6D); // Allocate memory
244 
245  timer.tic();
246  SMOOTH(NBT)
247  {
248  // computeABADerivatives(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
249  computeDampedDelassusMatrixInverse(
250  model, data, qs[_smooth], contact_models_6D6D, contact_data_6D6D, dampedDelassusInverse,
251  1e-6);
252  }
253  std::cout << "cABA-OSIM = \t\t\t";
254  timer.toc(std::cout, NBT);
255 
256  timer.tic();
257  SMOOTH(NBT)
258  {
259  // computeABADerivatives(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
260  computeDampedDelassusMatrixInverse(
261  model, data, qs[_smooth], contact_models_6D6D, contact_data_6D6D, dampedDelassusInverse, 1e-6,
262  false, false);
263  }
264  std::cout << "EFPA = \t\t\t";
265  timer.toc(std::cout, NBT);
266 
267  std::cout << "--" << std::endl;
268 
269  return 0;
270 }
pinocchio::RigidConstraintData
RigidConstraintDataTpl< context::Scalar, context::Options > RigidConstraintData
Definition: algorithm/fwd.hpp:27
pinocchio::DataTpl::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: multibody/data.hpp:74
pinocchio::computeDampedDelassusMatrixInverse
void computeDampedDelassusMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, DataAllocator > &contact_data, const Eigen::MatrixBase< MatrixType > &damped_delassus_inverse, const Scalar mu, const bool scaled=false, const bool Pv=true)
Computes the inverse of the Delassus matrix associated to a set of given constraints.
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:88
pinocchio::getKKTContactDynamicMatrixInverse
PINOCCHIO_DEPRECATED void getKKTContactDynamicMatrixInverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< KKTMatrixType > &KKTMatrix_inv)
Computes the inverse of the KKT matrix for dynamics with contact constraints.
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
delassus.hpp
main
int main(int argc, const char **argv)
Definition: timings-delassus-operations.cpp:24
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
kinematics.hpp
rnea-derivatives.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
pinocchio::buildModels::humanoidRandom
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
pinocchio::python::context::JointModelFreeFlyer
JointModelFreeFlyerTpl< Scalar > JointModelFreeFlyer
Definition: bindings/python/context/generic.hpp:126
pinocchio::initPvDelassus
void initPvDelassus(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
PinocchioTicToc::unitName
static std::string unitName(Unit u)
Definition: timer.hpp:58
PinocchioTicToc::DEFAULT_UNIT
Unit DEFAULT_UNIT
Definition: timer.hpp:56
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
pinocchio::CONTACT_6D
@ CONTACT_6D
Point contact model.
Definition: algorithm/contact-info.hpp:22
rnea.hpp
aba-derivatives.hpp
timer.hpp
pinocchio::context::RowMatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor|Options > RowMatrixXs
Definition: context/generic.hpp:51
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
joint-configuration.hpp
pinocchio::context::VectorXs
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
Definition: context/generic.hpp:47
pinocchio::ProximalSettingsTpl
Structure containing all the settings parameters for the proximal algorithms.
Definition: algorithm/fwd.hpp:13
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
pinocchio::context::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: context/generic.hpp:49
pinocchio::getJointJacobian
void getJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame opt...
filename
filename
urdf.hpp
pinocchio::forwardDynamics
const PINOCCHIO_DEPRECATED DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & forwardDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Eigen::MatrixBase< ConstraintMatrixType > &J, const Eigen::MatrixBase< DriftVectorType > &gamma, const Scalar inv_damping=0.)
Compute the forward dynamics with contact constraints. Internally, pinocchio::computeAllTerms is call...
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
cartpole.v
v
Definition: cartpole.py:153
pinocchio::urdf::buildModel
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, const std::string &rootJointName, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
q
q
PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
Definition: container/aligned-vector.hpp:13
PinocchioTicToc::US
@ US
Definition: timer.hpp:53
cholesky.hpp
pinocchio::cholesky::decompose
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & decompose(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the Cholesky decomposition of the joint space inertia matrix M contained in data.
simulation-contact-dynamics.num_constraints
num_constraints
Definition: simulation-contact-dynamics.py:72
kinematics-derivatives.hpp
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
contact-cholesky.contact_models
list contact_models
Definition: contact-cholesky.py:22
pinocchio::ContactCholeskyDecompositionTpl< context::Scalar, context::Options >
contact-dynamics.hpp
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
constrained-dynamics.hpp
simulation-closed-kinematic-chains.prox_settings
prox_settings
Definition: simulation-closed-kinematic-chains.py:163
crba.hpp
pinocchio::CONTACT_3D
@ CONTACT_3D
Definition: algorithm/contact-info.hpp:21
pinocchio::computeAllTerms
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
PinocchioTicToc
Definition: timer.hpp:47
meshcat-viewer.qs
qs
Definition: meshcat-viewer.py:128
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: build-reduced-model.cpp:11
PinocchioTicToc::tic
void tic()
Definition: timer.hpp:82
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:33