timings-jacobian.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2018 CNRS
3 //
4 
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 #include "pinocchio/algorithm/joint-configuration.hpp"
8 #include "pinocchio/algorithm/kinematics.hpp"
9 #include "pinocchio/algorithm/jacobian.hpp"
10 #include "pinocchio/parsers/urdf.hpp"
11 #include "pinocchio/parsers/sample-models.hpp"
12 
13 #include <iostream>
14 
15 #include "pinocchio/utils/timer.hpp"
16 
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 
32 
33  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
34  if(argc>1) filename = argv[1];
35 
36  bool with_ff = true;
37  if(argc>2)
38  {
39  const std::string ff_option = argv[2];
40  if(ff_option == "-no-ff")
41  with_ff = false;
42  }
43 
44  if( filename == "HS")
46  else
47  if(with_ff)
49  else
50  pinocchio::urdf::buildModel(filename,model);
51  std::cout << "nq = " << model.nq << std::endl;
52 
53  pinocchio::Data data(model);
54  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
55 
57  J.setZero();
59 
61  for(size_t i=0;i<NBT;++i)
62  {
63  qs[i] = randomConfiguration(model,-qmax,qmax);
64  }
65 
66  timer.tic();
67  SMOOTH(NBT)
68  {
69  forwardKinematics(model,data,qs[_smooth]);
70  }
71  std::cout << "Zero Order Kinematics = \t"; timer.toc(std::cout,NBT);
72 
73  timer.tic();
74  SMOOTH(NBT)
75  {
76  computeJointJacobian(model,data,qs[_smooth],JOINT_ID,J);
77  }
78  std::cout << "computeJointJacobian = \t\t"; timer.toc(std::cout,NBT);
79 
80  timer.tic();
81  SMOOTH(NBT)
82  {
83  computeJointJacobians(model,data,qs[_smooth]);
84  }
85  std::cout << "computeJointJacobians(q) = \t"; timer.toc(std::cout,NBT);
86 
87  timer.tic();
88  SMOOTH(NBT)
89  {
90  computeJointJacobians(model,data);
91  }
92  std::cout << "computeJointJacobians() = \t"; timer.toc(std::cout,NBT);
93 
94  timer.tic();
95  SMOOTH(NBT)
96  {
97  getJointJacobian(model,data,JOINT_ID,LOCAL,J);
98  }
99  std::cout << "getJointJacobian(LOCAL) = \t"; timer.toc(std::cout,NBT);
100 
101  timer.tic();
102  SMOOTH(NBT)
103  {
104  getJointJacobian(model,data,JOINT_ID,WORLD,J);
105  }
106  std::cout << "getJointJacobian(WORLD) = \t"; timer.toc(std::cout,NBT);
107 
108  std::cout << "--" << std::endl;
109  return 0;
110 }
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
double toc()
Definition: timer.hpp:68
int njoints
Number of joints.
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.
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...
#define PINOCCHIO_MODEL_DIR
void tic()
Definition: timer.hpp:63
int main(int argc, const char **argv)
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.
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
pinocchio::JointIndex JointIndex
#define SMOOTH(s)
Definition: timer.hpp:38
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, 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...
Main pinocchio namespace.
Definition: timings.cpp:28
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.
filename
JointModelFreeFlyerTpl< double > JointModelFreeFlyer
void computeJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex jointId, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store t...
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)
int nq
Dimension of the configuration vector representation.


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