pub fn visualize_robot_with_safety(
robot: KinematicsWithShape,
intial_angles: [f32; 6],
tcp_box: [RangeInclusive<f64>; 3],
safety_distances: &SafetyDistances,
)
pub fn visualize_robot_with_safety(
robot: KinematicsWithShape,
intial_angles: [f32; 6],
tcp_box: [RangeInclusive<f64>; 3],
safety_distances: &SafetyDistances,
)