examples/kinematics-derivatives.cpp
Go to the documentation of this file.
1 #include "pinocchio/parsers/urdf.hpp"
2 
3 #include "pinocchio/algorithm/joint-configuration.hpp"
4 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
5 
6 #include <iostream>
7 
8 // PINOCCHIO_MODEL_DIR is defined by the CMake but you can define your own directory here.
9 #ifndef PINOCCHIO_MODEL_DIR
10  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"
11 #endif
12 int main(int argc, char ** argv)
13 {
14  using namespace pinocchio;
15 
16  // You should change here to set up your own URDF file or just pass it as an argument of this example.
17  const std::string urdf_filename = (argc<=1) ? PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") : argv[1];
18 
19  // Load the URDF model
20  Model model;
21  pinocchio::urdf::buildModel(urdf_filename,model);
22 
23  // Build a data related to model
24  Data data(model);
25 
26  // Sample a random joint configuration as well as random joint velocity and acceleration
28  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
29  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
30 
31  // Computes the kinematics derivatives for all the joints of the robot
32  computeForwardKinematicsDerivatives(model, data, q, v, a);
33 
34  // Retrieve the kinematics derivatives of a specific joint, expressed in the LOCAL frame of the joints.
36  Data::Matrix6x v_partial_dq(6,model.nv), a_partial_dq(6,model.nv), a_partial_dv(6,model.nv), a_partial_da(6,model.nv);
37  v_partial_dq.setZero();
38  a_partial_dq.setZero(); a_partial_dv.setZero(); a_partial_da.setZero();
39  getJointAccelerationDerivatives(model,data,joint_id,LOCAL,v_partial_dq,
40  a_partial_dq,a_partial_dv,a_partial_da);
41 
42  // Remark: we are not directly computing the quantity v_partial_dv as it is also equal to a_partial_da.
43 
44  // But we can also expressed the same quantities in the frame centered on the end-effector joint, but expressed in the axis aligned with the world frame.
45  getJointAccelerationDerivatives(model,data,joint_id,WORLD,v_partial_dq,
46  a_partial_dq,a_partial_dv,a_partial_da);
47 
48 }
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
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.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
The LOCAL frame convention corresponds to the frame directly attached to the moving part (Joint...
void computeForwardKinematicsDerivatives(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 > &a)
Computes all the terms required to compute the derivatives of the placement, spatial velocity and acc...
int main(int argc, char **argv)
void getJointAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &a_partial_dq, const Eigen::MatrixBase< Matrix6xOut3 > &a_partial_dv, const Eigen::MatrixBase< Matrix6xOut4 > &a_partial_da)
Computes the partial derivaties of the spatial acceleration of a given with respect to the joint conf...
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...
Vec3f a
Main pinocchio namespace.
Definition: timings.cpp:28
int nv
Dimension of the velocity vector space.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
JointCollectionTpl & model
#define PINOCCHIO_MODEL_DIR


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