Skip to main content

inverse_dynamics/
inverse_dynamics.rs

1use dynamics_rs::prelude::*;
2
3fn main() {
4    let urdf_path = "./examples/descriptions/ur5/ur5_robot.urdf"; // Path to the URDF file
5    let mesh_path = "./examples/descriptions/ur5/"; // Optional mesh path
6
7    // Build models from the URDF file
8    let (model, _coll_model, _viz_model) =
9        build_models_from_urdf(urdf_path, Some(mesh_path)).expect("Failed to parse URDF file");
10
11    // Generate a random configuration, velocity and acceleration
12    let q = random_configuration(&model);
13    let v = Configuration::from_element(model.nv, 1.0);
14    let a = Configuration::from_element(model.nv, 1.0);
15
16    // Create data structure for the model
17    let mut data = model.create_data();
18
19    // Compute inverse dynamics
20    inverse_dynamics(&model, &mut data, &q, &v, &a, None)
21        .expect("Failed to compute inverse dynamics");
22
23    // Print the computed torques
24    println!("Computed torques: {}", data.tau);
25}