Line |
Branch |
Exec |
Source |
1 |
|
|
#include "pinocchio/parsers/urdf.hpp" |
2 |
|
|
|
3 |
|
|
#include "pinocchio/algorithm/joint-configuration.hpp" |
4 |
|
|
#include "pinocchio/algorithm/aba-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 |
|
|
|
13 |
|
✗ |
int main(int argc, char ** argv) |
14 |
|
|
{ |
15 |
|
|
using namespace pinocchio; |
16 |
|
|
|
17 |
|
|
// You should change here to set up your own URDF file or just pass it as an argument of this |
18 |
|
|
// example. |
19 |
|
|
const std::string urdf_filename = |
20 |
|
|
(argc <= 1) ? PINOCCHIO_MODEL_DIR |
21 |
|
✗ |
+ std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") |
22 |
|
✗ |
: argv[1]; |
23 |
|
|
|
24 |
|
|
// Load the URDF model |
25 |
|
✗ |
Model model; |
26 |
|
✗ |
pinocchio::urdf::buildModel(urdf_filename, model); |
27 |
|
|
|
28 |
|
|
// Build a data related to model |
29 |
|
✗ |
Data data(model); |
30 |
|
|
|
31 |
|
|
// Sample a random joint configuration as well as random joint velocity and torque |
32 |
|
✗ |
Eigen::VectorXd q = randomConfiguration(model); |
33 |
|
✗ |
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
34 |
|
✗ |
Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv); |
35 |
|
|
|
36 |
|
|
// Allocate result container |
37 |
|
✗ |
Eigen::MatrixXd djoint_acc_dq = Eigen::MatrixXd::Zero(model.nv, model.nv); |
38 |
|
✗ |
Eigen::MatrixXd djoint_acc_dv = Eigen::MatrixXd::Zero(model.nv, model.nv); |
39 |
|
✗ |
Eigen::MatrixXd djoint_acc_dtau = Eigen::MatrixXd::Zero(model.nv, model.nv); |
40 |
|
|
|
41 |
|
|
// Computes the forward dynamics (ABA) derivatives for all the joints of the robot |
42 |
|
✗ |
computeABADerivatives(model, data, q, v, tau, djoint_acc_dq, djoint_acc_dv, djoint_acc_dtau); |
43 |
|
|
|
44 |
|
|
// Get access to the joint acceleration |
45 |
|
✗ |
std::cout << "Joint acceleration: " << data.ddq.transpose() << std::endl; |
46 |
|
|
} |
47 |
|
|
|