Line |
Branch |
Exec |
Source |
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 |
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; |
25 |
|
✗ |
pinocchio::urdf::buildModel(urdf_filename, 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 |
36 |
|
✗ |
computeForwardKinematicsDerivatives(model, data, q, v, a); |
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(); |
47 |
|
✗ |
getJointAccelerationDerivatives( |
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. |
55 |
|
✗ |
getJointAccelerationDerivatives( |
56 |
|
|
model, data, joint_id, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); |
57 |
|
|
} |
58 |
|
|
|