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
extern crate optima;
use nalgebra::Vector3;
use optima::robot_set_modules::robot_set::RobotSet;
use optima::scenes::robot_geometric_shape_scene::{EnvObjInfoBlock, EnvObjPoseConstraint, RobotGeometricShapeScene};
use optima::utils::utils_robot::robot_module_utils::RobotNames;
use optima::utils::utils_se3::optima_se3_pose::{OptimaSE3Pose, OptimaSE3PoseType};
fn main() {
// Initialize a `RobotSet`. In this case, it is just a base model ur5 robot.
let robot_set = RobotSet::new_from_robot_names(vec![RobotNames::new_base("ur5")]);
// We use the `RobotSet` to initialize a `RobotGeometricShapeScene`. By default, the
// environment is empty.
let mut robot_geometric_shape_scene = RobotGeometricShapeScene::new(robot_set, None).expect("error");
// This line adds a "sphere" object to the environment, scaled down to a size of 0.1.
// Note that any mesh directory in `optima_toolbox/optima_assets/optima_scenes/mesh_files`
// can be loaded in.
let env_obj_idx = robot_geometric_shape_scene.add_environment_object(EnvObjInfoBlock {
asset_name: "sphere".to_string(),
scale: Vector3::new(0.1, 0.1, 0.1),
..Default::default()
}, false).expect("error");
// Adding the object to the scene above returns the index of the added environment object.
// Indices are assigned in order, so the first returned index will be 0.
assert_eq!(env_obj_idx, 0);
// This line can update the rotation and translation of the given environment object in the scene.
// In this case, we are moving the sphere such that its position will be at (0.5,0.5,0).
// Note that we can also set the transform of an object such that it is relative to another
// shape in the environment.
robot_geometric_shape_scene.set_curr_single_env_obj_pose_constraint(env_obj_idx, EnvObjPoseConstraint::Absolute(OptimaSE3Pose::new_from_euler_angles(0., 0., 0., 0.5, 0.5, 0., &OptimaSE3PoseType::ImplicitDualQuaternion))).expect("error");
// This prints a summary of the whole scene.
robot_geometric_shape_scene.print_summary();
}