| 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 |
|
|
|