Robot Geometric Shape Scenes

All tutorials up to this point have involved robot sets in empty environments. However, robots are commonly surrounded by other obstacles. Optima allows for the modeling of these obstacles as static or dynamic geometric shapes in a RobotGeometricShapeScene. This struct is another abstraction layer on top of the RobotSet struct that allows for easy intersection checks, proximity checks, contact checks, ray casting, etc.

Example code can be found here:

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();
}

Explanations are written in this code line by line. I will show how to use this robot geometric shape scene for various queries in the following tutorials, we are just focused on initializing the scene here.

The code for this tutorial can be found in the file optima_toolbox/optima/examples/7_robot_geometric_shape_scene.rs and can be run from the optima_toolbox/optima directory using the following command:

cargo run --example 7_robot_geometric_shape_scene

The output of this example should be the following:

1 robots.
 Robot 0 ---> "ur5"
   Base Offset: EulerAnglesAndTranslation { euler_angles: [[0.0, -0.0, 0.0]], translation: [[0.0, 0.0, 0.0]], phantom_data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.0, 0.0, 0.0]], is_identity: true, rot_is_identity: true, translation_is_zeros: true }, pose_type: EulerAnglesAndTranslation }

1 objects.
 Object 0 ---> 
    Object Info: EnvObjInfoBlock { asset_name: "sphere", scale: [[0.1, 0.1, 0.1]], shape_representation: BestFitConvexShape, decomposition_resolution: Medium, pose_constraint: Absolute(ImplicitDualQuaternion { data: ImplicitDualQuaternion { rotation: [0.0, 0.0, 0.0, 1.0], translation: [[0.5, 0.5, 0.0]], is_identity: false, rot_is_identity: true, translation_is_zeros: false }, pose_type: ImplicitDualQuaternion }) }