| Directory: | ./ |
|---|---|
| File: | examples/kinematics-derivatives.cpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 20 | 20 | 100.0% |
| Branches: | 25 | 58 | 43.1% |
| 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 | 1 | 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 |
2/8✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
|
2 | + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf") |
| 21 |
4/12✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 1 times.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
|
2 | : argv[1]; |
| 22 | |||
| 23 | // Load the URDF model | ||
| 24 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Model model; |
| 25 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pinocchio::urdf::buildModel(urdf_filename, model); |
| 26 | |||
| 27 | // Build a data related to model | ||
| 28 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Data data(model); |
| 29 | |||
| 30 | // Sample a random joint configuration as well as random joint velocity and acceleration | ||
| 31 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Eigen::VectorXd q = randomConfiguration(model); |
| 32 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); |
| 33 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); |
| 34 | |||
| 35 | // Computes the kinematics derivatives for all the joints of the robot | ||
| 36 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | 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 | 1 | JointIndex joint_id = (JointIndex)(model.njoints - 1); | |
| 41 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | Data::Matrix6x v_partial_dq(6, model.nv), a_partial_dq(6, model.nv), a_partial_dv(6, model.nv), |
| 42 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | a_partial_da(6, model.nv); |
| 43 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | v_partial_dq.setZero(); |
| 44 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | a_partial_dq.setZero(); |
| 45 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | a_partial_dv.setZero(); |
| 46 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | a_partial_da.setZero(); |
| 47 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | 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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | getJointAccelerationDerivatives( |
| 56 | model, data, joint_id, WORLD, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da); | ||
| 57 | 1 | } | |
| 58 |