timings-constrained-dynamics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 LAAS-CNRS INRIA
3 //
4 
13 
14 #include <iostream>
15 
17 
18 int main(int argc, const char ** argv)
19 {
20  using namespace Eigen;
21  using namespace pinocchio;
22 
24 #ifdef NDEBUG
25  const int NBT = 1000 * 100;
26 #else
27  const int NBT = 1;
28  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
29 #endif
30 
31  // Build model
32  Model model;
33 
34  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
35  if (argc > 1)
36  filename = argv[1];
37  bool with_ff = true;
38 
39  if (argc > 2)
40  {
41  const std::string ff_option = argv[2];
42  if (ff_option == "-no-ff")
43  with_ff = false;
44  }
45 
46  if (filename == "HS")
48  else if (with_ff)
50  // pinocchio::urdf::buildModel(filename,JointModelRX(),model);
51  else
53 
54  const std::string RA = "RARM_LINK6";
55  const JointIndex RA_id = model.frames[model.getFrameId(RA)].parentJoint;
56  const std::string LA = "LARM_LINK6";
57  const JointIndex LA_id = model.frames[model.getFrameId(LA)].parentJoint;
58  const std::string RF = "RLEG_LINK6";
59  const JointIndex RF_id = model.frames[model.getFrameId(RF)].parentJoint;
60  const std::string LF = "LLEG_LINK6";
61  const JointIndex LF_id = model.frames[model.getFrameId(LF)].parentJoint;
62 
63  RigidConstraintModel ci_RF_6D(CONTACT_6D, model, RF_id, LOCAL);
64  RigidConstraintData cd_RF_6D(ci_RF_6D);
65  // RigidConstraintModel ci_RF_3D(CONTACT_3D,model.getJointId(RF),WORLD);
66 
67  RigidConstraintModel ci_LF_6D(CONTACT_6D, model, LF_id, LOCAL);
68  RigidConstraintData cd_LF_6D(ci_LF_6D);
69  // RigidConstraintModel ci_LF_3D(CONTACT_3D,model.getJointId(LF),WORLD);
70 
71  // RigidConstraintModel ci_RA_3D(CONTACT_3D,model.getJointId(RA),WORLD);
72  // RigidConstraintModel ci_LA_3D(CONTACT_3D,model.getJointId(LA),WORLD);
73 
74  // Define contact infos structure
77 
78  ContactCholeskyDecomposition contact_chol_empty(model, contact_models_empty);
79 
82  contact_models_6D.push_back(ci_RF_6D);
83  contact_data_6D.push_back(cd_RF_6D);
84 
85  ContactCholeskyDecomposition contact_chol_6D(model, contact_models_6D);
86 
88  contact_models_6D6D.push_back(ci_RF_6D);
89  contact_models_6D6D.push_back(ci_LF_6D);
91  contact_data_6D6D.push_back(cd_RF_6D);
92  contact_data_6D6D.push_back(cd_LF_6D);
93 
94  ContactCholeskyDecomposition contact_chol_6D6D(model, contact_models_6D6D);
95 
96  std::cout << "nq = " << model.nq << std::endl;
97  std::cout << "nv = " << model.nv << std::endl;
98  std::cout << "--" << std::endl;
99 
100  Data data(model);
101  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
102 
104  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
105  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
106  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
107 
108  for (size_t i = 0; i < NBT; ++i)
109  {
110  qs[i] = randomConfiguration(model, -qmax, qmax);
111  qdots[i] = Eigen::VectorXd::Random(model.nv);
112  qddots[i] = Eigen::VectorXd::Random(model.nv);
113  taus[i] = Eigen::VectorXd::Random(model.nv);
114  }
115 
116  timer.tic();
117  SMOOTH(NBT)
118  {
119  computeABADerivatives(model, data, qs[_smooth], qdots[_smooth], taus[_smooth]);
120  }
121  std::cout << "ABA derivatives= \t\t\t";
122  timer.toc(std::cout, NBT);
123 
124  double total_time = 0;
125  initConstraintDynamics(model, data, contact_models_empty);
126  SMOOTH(NBT)
127  {
129  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_empty,
130  contact_data_empty);
131  timer.tic();
132  computeConstraintDynamicsDerivatives(model, data, contact_models_empty, contact_data_empty);
133  total_time += timer.toc(timer.DEFAULT_UNIT);
134  }
135  std::cout << "constraintDynamicsDerivs {} = \t\t" << (total_time / NBT) << std::endl;
136 
137  total_time = 0;
138  initConstraintDynamics(model, data, contact_models_6D);
139  SMOOTH(NBT)
140  {
142  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D, contact_data_6D);
143  timer.tic();
144  computeConstraintDynamicsDerivatives(model, data, contact_models_6D, contact_data_6D);
145  total_time += timer.toc(timer.DEFAULT_UNIT);
146  }
147  std::cout << "constraintDynamicsDerivs {6D} = \t\t" << (total_time / NBT) << std::endl;
148 
149  total_time = 0;
150  initConstraintDynamics(model, data, contact_models_6D6D);
151  SMOOTH(NBT)
152  {
154  model, data, qs[_smooth], qdots[_smooth], taus[_smooth], contact_models_6D6D,
155  contact_data_6D6D);
156  timer.tic();
157  computeConstraintDynamicsDerivatives(model, data, contact_models_6D6D, contact_data_6D6D);
158  total_time += timer.toc(timer.DEFAULT_UNIT);
159  }
160  std::cout << "constraintDynamicsDerivs {6D,6D} = \t" << (total_time / NBT) << std::endl;
161  return 0;
162 }
PinocchioTicToc::toc
double toc()
Definition: timer.hpp:88
Eigen
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::computeConstraintDynamicsDerivatives
void computeConstraintDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const ProximalSettingsTpl< Scalar > &settings, const Eigen::MatrixBase< MatrixType1 > &ddq_partial_dq, const Eigen::MatrixBase< MatrixType2 > &ddq_partial_dv, const Eigen::MatrixBase< MatrixType3 > &ddq_partial_dtau, const Eigen::MatrixBase< MatrixType4 > &lambda_partial_dq, const Eigen::MatrixBase< MatrixType5 > &lambda_partial_dv, const Eigen::MatrixBase< MatrixType6 > &lambda_partial_dtau)
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
setup.data
data
Definition: cmake/cython/setup.in.py:48
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
constrained-dynamics-derivatives.hpp
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
aba-derivatives.hpp
timer.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
SMOOTH
#define SMOOTH(s)
Definition: timer.hpp:38
joint-configuration.hpp
pinocchio::RigidConstraintModelTpl
Definition: algorithm/constraints/fwd.hpp:14
filename
filename
urdf.hpp
pinocchio::RigidConstraintDataTpl
Definition: algorithm/constraints/fwd.hpp:16
pinocchio::initConstraintDynamics
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
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...
constraintDynamics
VectorXd constraintDynamics(const Model &model, Data &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Model::JointIndex id)
Definition: unittest/constrained-dynamics-derivatives.cpp:185
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
main
int main(int argc, const char **argv)
Definition: timings-constrained-dynamics-derivatives.cpp:18
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::python::computeABADerivatives
bp::tuple computeABADerivatives(const context::Model &model, context::Data &data, const context::VectorXs &q, const context::VectorXs &v, const context::VectorXs &tau)
Definition: expose-aba-derivatives.cpp:20
pinocchio::ContactCholeskyDecompositionTpl< context::Scalar, context::Options >
contact-dynamics.hpp
sample-models.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
constrained-dynamics.hpp
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 Wed Dec 25 2024 03:41:19