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
extern crate optima;
use optima::robot_modules::robot::Robot;
use optima::robot_modules::robot_configuration_module::{ContiguousChainMobilityMode, RobotConfigurationModule};
use optima::utils::utils_robot::robot_module_utils::RobotNames;
fn main() {
// Initialize a new robot configuration for the ur5 robot.
let mut robot_configuration = RobotConfigurationModule::new_from_names(RobotNames::new_base("ur5")).expect("error");
// Sets the 0 joint idx (should_pan_joint, in the case of the ur5 robot) with joint_sub_idx 0 (the
// default sub-idx for joints with only 1 degree of freedom) to a fixed value of 1.2. From here on
// out, this joint will no longer be considered a degree of freedom and, instead,
// will remain locked at 1.2.
robot_configuration.set_fixed_joint(0, 0, 1.2).expect("error");
// Sets link with idx 5 (wrist_2_link, in the case of the ur5 robot) as a "dead end link". A
// dead-end link means that this link, as well as all predecessor links in the kinematic chain,
// will be removed from the model. Note that this will also remove all joints that connect
// these removed links, thus possibly reducing the number of degrees of freedom of the model.
// Links and joints that are removed in this way are said to be not "present" in the model.
robot_configuration.set_dead_end_link(5).expect("error");
// Sets the base of the robot to be mobile. In this case, the base is floating (not
// realistic for the ur5 robot, but is very useful, for example, for the hips
// of humanoid robots). This base will automatically add 6 degrees of freedom
// to the model, 3 for translation (x, y, z) and 3 for rotation (rx, ry, rz).
// All lower and upper bounds are set for each of these degrees of freedom.
// All mobility modes are PlanarTranslation (x and y translation), PlanarRotation (rz rotation),
// PlanarTranslationAndRotation (x and y translation + z rotation),
// Static (i.e., 0 DOFs), and Floating (x, y, z translation + rx, ry, rz rotation).
robot_configuration.set_mobile_base(ContiguousChainMobilityMode::Floating {
x_bounds: (-2.0, 2.0),
y_bounds: (-2.0, 2.0),
z_bounds: (-2.0, 2.0),
xr_bounds: (-3.14, 3.14),
yr_bounds: (-3.14, 3.14),
zr_bounds: (-3.14, 3.14)
}).expect("error");
// Saves the robot configuration for later use. This will add a file to
// `optima_toolbox/optima_assets/optima_robots/ur5/configurations/test_configuration.JSON`.
// NOTE: if you run this example multiple times and the test_configuration file is already
// present, a prompt will show up in the console asking if you would like to save over
// the already saved file.
robot_configuration.save("test_configuration").expect("error");
// For illustrative purposes, let's now load in our robot configuration from the file that
// was just saved. The second parameter in the `RobotNames` struct is now an option of
// `Some` with the name of our recently saved configuration.
let loaded_robot_configuration = RobotConfigurationModule::new_from_names(RobotNames::new("ur5", Some("test_configuration"))).expect("error");
// This configuration can now be used to instantiate a `Robot`.
let robot = Robot::new_from_robot_configuration_module(loaded_robot_configuration);
// when we print information about the robot, we see that the information that we
// inputted above is reflected in the printed output.
robot.robot_configuration_module().print_summary();
robot.robot_joint_state_module().print_dof_summary();
}