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