examples/kinematics-derivatives.cpp
Go to the documentation of this file.
2 
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
17  // example.
18  const std::string urdf_filename =
19  (argc <= 1) ? PINOCCHIO_MODEL_DIR
20  + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf")
21  : argv[1];
22 
23  // Load the URDF model
24  Model model;
26 
27  // Build a data related to model
28  Data data(model);
29 
30  // Sample a random joint configuration as well as random joint velocity and acceleration
31  Eigen::VectorXd q = randomConfiguration(model);
32  Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
33  Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
34 
35  // Computes the kinematics derivatives for all the joints of the robot
37 
38  // Retrieve the kinematics derivatives of a specific joint, expressed in the LOCAL frame of the
39  // joints.
40  JointIndex joint_id = (JointIndex)(model.njoints - 1);
41  Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv),
42  a_partial_da(6, model.nv);
43  v_partial_dq.setZero();
44  a_partial_dq.setZero();
45  a_partial_dv.setZero();
46  a_partial_da.setZero();
48  model, data, joint_id, LOCAL, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
49 
50  // Remark: we are not directly computing the quantity v_partial_dv as it is also equal to
51  // a_partial_da.
52 
53  // But we can also expressed the same quantities in the frame centered on the end-effector joint,
54  // but expressed in the axis aligned with the world frame.
56  model, data, joint_id, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
57 }
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
append-urdf-model-with-another-model.urdf_filename
string urdf_filename
Definition: append-urdf-model-with-another-model.py:16
pinocchio::DataTpl
Definition: context/generic.hpp:25
setup.data
data
Definition: cmake/cython/setup.in.py:48
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:325
pinocchio::getJointAccelerationDerivatives
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...
main
int main(int argc, char **argv)
Definition: examples/kinematics-derivatives.cpp:12
pinocchio::computeForwardKinematicsDerivatives
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...
joint-configuration.hpp
pinocchio::urdf::buildModel
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...
urdf.hpp
PINOCCHIO_MODEL_DIR
#define PINOCCHIO_MODEL_DIR
Definition: examples/kinematics-derivatives.cpp:10
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1117
a
Vec3f a
pinocchio::v
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
Definition: joint-configuration.hpp:1118
kinematics-derivatives.hpp
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1116
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27
pinocchio::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50


pinocchio
Author(s):
autogenerated on Sun Jun 16 2024 02:43:11