Struct SafetyDistances

pub struct SafetyDistances {
    pub to_environment: f32,
    pub to_robot_default: f32,
    pub special_distances: HashMap<(u16, u16), f32>,
    pub mode: CheckMode,
}

Defines tolerance bounds, how far it should be between any part of the robot, or environment object, or any two parts of the robot. As some robot joints may come very close together, they may require specialized distances.

Fields

to_environment: f32

Allowed distance between robot and environment objects.

to_robot_default: f32

Default allowed distance between any two parts of the robot.

special_distances: HashMap<(u16, u16), f32>

Special cases where different (normally shorter) distance is allowed. Some parts of the robot naturally come very close even if not adjacent, and these need the shorter distance to be specified. Specify NEVER_COLLIDES as a distance for parts that cannot collide. Initialize it like this:

   use std::collections::HashMap;
   use rs_opw_kinematics::collisions::{SafetyDistances, NEVER_COLLIDES};
   use rs_opw_kinematics::kinematic_traits::{J1, J2, J3, J4, J5, J6, J_BASE, J_TOOL};

   // Always specify less numbered joints first, then
   // the tool, then environment objects.
    SafetyDistances::distances(&[
      // Due construction of this robot, these joints are very close, so
      // special rules are needed for them.
      ((J2, J_BASE), NEVER_COLLIDES), // base and J2 cannot collide
      ((J3, J_BASE), NEVER_COLLIDES), // base and J3 cannot collide
      ((J2, J4), NEVER_COLLIDES),
      ((J3, J4), NEVER_COLLIDES),
      ((J4, J_TOOL), 0.02_f32), // reduce distance requirement to 2 cm.
      ((J4, J6), 0.02_f32),     // reduce distance requirement to 2 cm.
      ]);

mode: CheckMode

Specifies if either first collision only is required, or all must be checked, or off, or “touch only” mode

Implementations

impl SafetyDistances

pub fn distances(pairs: &[((usize, usize), f32)]) -> HashMap<(u16, u16), f32>

Examples found in repository

examples/cartesian_stroke.rs (lines 88-97)

pub fn create_rx160_robot() -> KinematicsWithShape {
    use rs_opw_kinematics::read_trimesh::load_trimesh_from_stl;

    let monolith = load_trimesh_from_stl("src/tests/data/object.stl");

    KinematicsWithShape::with_safety(
        Parameters {
            a1: 0.15,
            a2: 0.0,
            b: 0.0,
            c1: 0.55,
            c2: 0.825,
            c3: 0.625,
            c4: 0.11,
            ..Parameters::new()
        },
        // Constraints are used also to sample constraint-compliant random positions
        // as needed by this path planner.
        Constraints::from_degrees(
            [
                -225.0..=225.0,
                -225.0..=225.0,
                -225.0..=225.0,
                -225.0..=225.0,
                -225.0..=225.0,
                -225.0..=225.0,
            ],
            BY_PREV,
        ),
        [
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_1.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_2.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_3.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_4.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_5.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_6.stl"),
        ],
        load_trimesh_from_stl("src/tests/data/staubli/rx160/base_link.stl"),
        Isometry3::from_parts(
            Translation3::new(0.4, 0.7, 0.0).into(),
            UnitQuaternion::identity(),
        ),
        load_trimesh_from_stl("src/tests/data/flag.stl"),
        Isometry3::from_parts(
            Translation3::new(0.0, 0.0, 0.5).into(),
            UnitQuaternion::identity(),
        ),
        vec![
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(1., 0., 0.),
            },
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(-1., 0., 0.),
            },
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(0., 1., 0.),
            },
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(0., -1., 0.),
            },
        ],
        SafetyDistances {
            to_environment: 0.05,   // Robot should not come closer than 5 cm to pillars
            to_robot_default: 0.05, // No closer than 5 cm to itself.
            special_distances: SafetyDistances::distances(&[
                // Due construction of this robot, these joints are very close, so
                // special rules are needed for them.
                ((J2, J_BASE), NEVER_COLLIDES), // base and J2 cannot collide
                ((J3, J_BASE), NEVER_COLLIDES), // base and J3 cannot collide
                ((J2, J4), NEVER_COLLIDES),
                ((J3, J4), NEVER_COLLIDES),
                ((J4, J_TOOL), 0.02_f32), // reduce distance requirement to 2 cm.
                ((J4, J6), 0.02_f32),     // reduce distance requirement to 2 cm.
            ]),
            mode: CheckMode::FirstCollisionOnly, // First pose only (true, enough for path planning)
        },
    )
}

examples/complete_visible_robot.rs (lines 110-119)

pub fn create_rx160_robot() -> KinematicsWithShape {
    use rs_opw_kinematics::read_trimesh::{load_trimesh_from_stl, load_trimesh_from_ply };

    // Environment object to collide with.
    let monolith = load_trimesh_from_stl("src/tests/data/object.stl");

    KinematicsWithShape::with_safety(
        // OPW parameters for Staubli RX 160
        Parameters {
            a1: 0.15,
            a2: 0.0,
            b: 0.0,
            c1: 0.55,
            c2: 0.825,
            c3: 0.625,
            c4: 0.11,
            ..Parameters::new()
        },
        // Define constraints directly in degrees, converting internally to radians.
        Constraints::from_degrees(
            [
                -225.0..=225.0,
                -225.0..=225.0,
                -225.0..=225.0,
                -225.0..=225.0,
                -225.0..=225.0,
                -360.0..=360.0,
            ],
            BY_PREV, // Prioritize previous joint position
        ),
        // Joint meshes
        [
            // If your meshes, if offset in .stl file, use Trimesh::transform_vertices,
            // you may also need Trimesh::scale in some extreme cases.
            // If your joints or tool consist of multiple meshes, combine these
            // with Trimesh::append
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_1.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_2.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_3.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_4.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_5.stl"),
            load_trimesh_from_stl("src/tests/data/staubli/rx160/link_6.stl"),
        ],
        // Base link mesh
        load_trimesh_from_stl("src/tests/data/staubli/rx160/base_link.stl"),
        // Base transform, this is where the robot is standing
        Isometry3::from_parts(
            Translation3::new(0.4, 0.7, 0.0).into(),
            UnitQuaternion::identity(),
        ),
        // Tool mesh. Load it from .ply file for feature demonstration
        load_trimesh_from_ply("src/tests/data/flag.ply"),
        // Tool transform, tip (not base) of the tool. The point past this
        // transform is known as tool center point (TCP).
        Isometry3::from_parts(
            Translation3::new(0.0, 0.0, 0.5).into(),
            UnitQuaternion::identity(),
        ),
        // Objects around the robot, with global transforms for them.
        vec![
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(1., 0., 0.),
            },
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(-1., 0., 0.),
            },
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(0., 1., 0.),
            },
            CollisionBody {
                mesh: monolith.clone(),
                pose: Isometry3::translation(0., -1., 0.),
            },
        ],
        SafetyDistances {
            to_environment: 0.05,   // Robot should not come closer than 5 cm to pillars
            to_robot_default: 0.05, // No closer than 5 cm to itself.
            special_distances: SafetyDistances::distances(&[
                // Due construction of this robot, these joints are very close, so
                // special rules are needed for them.
                ((J2, J_BASE), NEVER_COLLIDES), // base and J2 cannot collide
                ((J3, J_BASE), NEVER_COLLIDES), // base and J3 cannot collide
                ((J2, J4), NEVER_COLLIDES),
                ((J3, J4), NEVER_COLLIDES),
                ((J4, J_TOOL), 0.02_f32), // reduce distance requirement to 2 cm.
                ((J4, J6), 0.02_f32),     // reduce distance requirement to 2 cm.
            ]),
            mode: CheckMode::AllCollsions, // we need to report all for visualization
            // mode: CheckMode::NoCheck, // this is very fast but no collision check
        },
    )
}

pub fn min_distance(&self, from: u16, to: u16) -> &f32

Returns minimal allowed distance by the specified objects. The order of objects is not important.

pub fn standard(mode: CheckMode) -> SafetyDistances

Creates the standard instance of safety distances that always uses touch check only (no safety margins) but can also disable collision checks completely if you pass CheckMode::NoCheck as parameter.

Trait Implementations

impl Clone for SafetyDistances

fn clone(&self) -> SafetyDistances

Returns a copy of the value.

fn clone_from(&mut self, source: &Self)

Performs copy-assignment from source.

impl Debug for SafetyDistances

fn fmt(&self, f: &mut Formatter<'_>) -> Result

Formats the value using the given formatter.