forward_kinematics/
forward_kinematics.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
12    let q = random_configuration(&model);
13    println!("Random configuration q: {}", q);
14
15    // Create data structure for the model
16    let mut data = model.create_data();
17
18    // Compute forward kinematics
19    forward_kinematics(&model, &mut data, &q, None, None)
20        .expect("Failed to compute forward kinematics");
21
22    // Print the placement of the joint 'wrist_3_joint'
23    let id = model.get_joint_id("wrist_3_joint").unwrap();
24    let placement = &data.joint_placements[id];
25    println!("Placement of 'wrist_3_joint':\n{}", placement);
26
27    // Compute the frame placements
28    update_frame_placements(&model, &mut data);
29
30    // Print the placement of the frame 'tool0'
31    let frame_id = model.get_frame_id("tool0", None).unwrap();
32    // we don't specify a frame type (None) as there is only one frame with this name
33    let frame_placement = &data.frame_placements[frame_id];
34    println!("Placement of frame 'tool0':\n{}", frame_placement);
35}
forward_kinematics/forward_kinematics.rs

forward_kinematics/
forward_kinematics.rs
