dynibo 0.5.2

Tree-structured robot kinematics and dynamics with runtime-size workspace APIs
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105

use std::path::PathBuf;

use dynibo::{IndexedLoad, InverseKinematicsOptions, Robot, Wrench};
use nalgebra::{DMatrixView, DVectorView, Isometry3, Vector3};

fn print_vector(label: &str, values: &[f64]) {
    println!(
        "{label}: {:.5}",
        DVectorView::from_slice(values, values.len()).transpose()
    );
}

fn main() -> dynibo::Result<()> {
    let urdf =
        PathBuf::from(env!("CARGO_MANIFEST_DIR")).join("examples/data/franka/franka_fer.urdf");
    let mut robot = Robot::from_urdf(urdf)?;
    let flange = robot.link_id("fer_link8")?;

    // Only non-fixed joints occupy state-vector entries; fer_joint8 is fixed.
    let q = [0.0, -0.3, 0.0, -1.8, 0.0, 1.5, 0.7];
    let qd = [0.10, -0.20, 0.15, 0.05, -0.10, 0.20, -0.05];
    let qdd = [0.20, 0.10, -0.10, 0.05, 0.10, -0.05, 0.15];
    let ik_initial_q = [0.05, -0.2, -0.05, -1.6, 0.05, 1.4, 0.6];
    let generalized_count = robot.generalized_count();

    let mut jacobian = vec![0.0; 6 * generalized_count];
    let mut jacobian_derivative = vec![0.0; 6 * generalized_count];
    let mut mass_matrix = vec![0.0; generalized_count * generalized_count];
    let mut velocity_forces = vec![0.0; generalized_count];
    let mut gravity = vec![0.0; generalized_count];
    let mut joint_forces = vec![0.0; generalized_count];

    // forward_kinematics -- target-link pose in the world frame.
    let pose = robot.forward_kinematics(&q, flange)?;

    // jacobian and jacobian_derivative -- column-major, angular rows first.
    robot.jacobian(&q, flange, &mut jacobian)?;
    robot.jacobian_derivative(&q, &qd, flange, &mut jacobian_derivative)?;

    // forward_velocity_kinematics and forward_acceleration_kinematics.
    let velocity = robot.forward_velocity_kinematics(&q, &qd, flange, &Isometry3::identity())?;
    let acceleration = robot.forward_acceleration_kinematics(&q, &qd, &qdd, flange)?;

    // inverse_kinematics -- recover a known, reachable pose with damped least squares.
    let mut ik_solution = vec![0.0; robot.joint_count()];
    robot.inverse_kinematics(
        &ik_initial_q,
        flange,
        &pose,
        InverseKinematicsOptions::default(),
        &mut ik_solution,
    )?;

    // mass_matrix and velocity_product_forces.
    robot.mass_matrix(&q, &mut mass_matrix)?;
    robot.velocity_product_forces(&q, &qd, &mut velocity_forces)?;

    // gravity and inverse_dynamics, both with a link-local external load.
    let loads = [IndexedLoad {
        link: flange,
        wrench: Wrench::new(Vector3::zeros(), Vector3::new(0.0, 0.0, -5.0)),
    }];
    robot.gravity(&q, &loads, &mut gravity)?;
    robot.inverse_dynamics(&q, &qd, &qdd, &loads, &mut joint_forces)?;

    println!(
        "loaded {}: {} links, {} non-fixed joints",
        robot.name(),
        robot.link_count(),
        robot.joint_count()
    );
    println!(
        "forward_kinematics translation [m]: {:.5}",
        pose.translation.vector.transpose()
    );
    println!(
        "forward_kinematics rotation:\n{:.5}",
        pose.rotation.to_rotation_matrix()
    );
    println!(
        "jacobian (6 x {generalized_count}):\n{:.5}",
        DMatrixView::from_slice(&jacobian, 6, generalized_count)
    );
    println!(
        "jacobian_derivative (6 x {generalized_count}):\n{:.5}",
        DMatrixView::from_slice(&jacobian_derivative, 6, generalized_count)
    );
    println!(
        "forward_velocity_kinematics: {:.5}",
        velocity.to_vector().transpose()
    );
    println!(
        "forward_acceleration_kinematics: {:.5}",
        acceleration.to_vector().transpose()
    );
    print_vector("inverse_kinematics", &ik_solution);
    println!(
        "mass_matrix ({generalized_count} x {generalized_count}):\n{:.5}",
        DMatrixView::from_slice(&mass_matrix, generalized_count, generalized_count)
    );
    print_vector("velocity_product_forces", &velocity_forces);
    print_vector("gravity (including external load)", &gravity);
    print_vector("inverse_dynamics (including external load)", &joint_forces);
    Ok(())
}