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" 15 #include "pinocchio/utils/timer.hpp" 18 int main(
int argc,
const char ** argv)
20 using namespace Eigen;
25 const int NBT = 1000*100;
28 std::cout <<
"(the time score in debug mode is not relevant) " << std::endl;
34 if(argc>1) filename = argv[1];
39 const std::string ff_option = argv[2];
40 if(ff_option ==
"-no-ff")
51 std::cout <<
"nq = " << model.
nq << std::endl;
54 VectorXd qmax = Eigen::VectorXd::Ones(model.
nq);
61 for(
size_t i=0;
i<NBT;++
i)
71 std::cout <<
"Zero Order Kinematics = \t"; timer.
toc(std::cout,NBT);
78 std::cout <<
"computeJointJacobian = \t\t"; timer.
toc(std::cout,NBT);
85 std::cout <<
"computeJointJacobians(q) = \t"; timer.
toc(std::cout,NBT);
92 std::cout <<
"computeJointJacobians() = \t"; timer.
toc(std::cout,NBT);
99 std::cout <<
"getJointJacobian(LOCAL) = \t"; timer.
toc(std::cout,NBT);
106 std::cout <<
"getJointJacobian(WORLD) = \t"; timer.
toc(std::cout,NBT);
108 std::cout <<
"--" << std::endl;
The WORLD frame convention corresponds to the frame concident with the Universe/Inertial frame but mo...
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
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
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.
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.
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
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
int nq
Dimension of the configuration vector representation.