pub struct KinematicsWithShape {
pub kinematics: Arc<dyn Kinematics>,
pub body: RobotBody,
}
Expand description
Struct that combines the kinematic model of a robot with its geometrical shape. This struct provides both the kinematic functionality for computing joint positions and the physical structure used for collision detection and other geometric calculations.
Fields§
§kinematics: Arc<dyn Kinematics>
The kinematic model of the robot, typically used to compute forward and inverse kinematics.
This is an abstract trait (Kinematics
), allowing for different implementations of kinematic models.
body: RobotBody
The physical structure of the robot, represented by its joint geometries.
This RobotBody
contains information about the geometrical shapes of the joints
and provides functionality for collision detection.
Implementations§
Source§impl KinematicsWithShape
impl KinematicsWithShape
Sourcepub fn new(
opw_parameters: Parameters,
constraints: Constraints,
joint_meshes: [TriMesh; 6],
base_mesh: TriMesh,
base_transform: Isometry3<f64>,
tool_mesh: TriMesh,
tool_transform: Isometry3<f64>,
collision_environment: Vec<CollisionBody>,
first_collision_only: bool,
) -> Self
pub fn new( opw_parameters: Parameters, constraints: Constraints, joint_meshes: [TriMesh; 6], base_mesh: TriMesh, base_transform: Isometry3<f64>, tool_mesh: TriMesh, tool_transform: Isometry3<f64>, collision_environment: Vec<CollisionBody>, first_collision_only: bool, ) -> Self
Constructs a new KinematicsWithShape
instance for a 6DOF robot.
This method consumes all parameters, moving them inside the robot.
This is important for meshes that are bulky.
§Parameters
-
opw_parameters
- OPW parameters defining this robot -
constraints
: joint constraint limits -
joint_meshes
- An array of [TriMesh; 6
] representing the meshes for each joint’s body. -
base_mesh
- The mesh of the robot base. -
base_transform
- The transform to apply to the base mesh. This transform brings the robot into its intended location inside the robotic cell. -
tool_mesh
- The mesh of the robot’s tool, which will also be checked for collisions. -
tool_transform
- The transform to apply to the tool mesh. It defines the location of the “action point” of the robot whose location and rotation (pose) is the pose for direct and inverse kinematics calls. -
collision_environment
- A vector of collision objects represented byCollisionBody
, where each object includes a mesh and its transform. -
first_pose_only
- As collision check may be expensive, check if we need multiple checked solutions if inverse kinematics, or the first (best) is enough
§Returns
A KinematicsWithShape
instance with defined kinematic structure and shapes for collision detection.
Examples found in repository?
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 61 62 63 64 65 66 67
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::new(
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.) },
],
true,
)
}
Sourcepub fn with_safety(
opw_parameters: Parameters,
constraints: Constraints,
joint_meshes: [TriMesh; 6],
base_mesh: TriMesh,
base_transform: Isometry3<f64>,
tool_mesh: TriMesh,
tool_transform: Isometry3<f64>,
collision_environment: Vec<CollisionBody>,
safety: SafetyDistances,
) -> Self
pub fn with_safety( opw_parameters: Parameters, constraints: Constraints, joint_meshes: [TriMesh; 6], base_mesh: TriMesh, base_transform: Isometry3<f64>, tool_mesh: TriMesh, tool_transform: Isometry3<f64>, collision_environment: Vec<CollisionBody>, safety: SafetyDistances, ) -> Self
Constructs a new KinematicsWithShape
instance for a 6DOF robot.
This method consumes all parameters, moving them inside the robot.
This is important for meshes that are bulky.
§Parameters
-
opw_parameters
- OPW parameters defining this robot -
constraints
: joint constraint limits -
joint_meshes
- An array of [TriMesh; 6
] representing the meshes for each joint’s body. -
base_mesh
- The mesh of the robot base. -
base_transform
- The transform to apply to the base mesh. This transform brings the robot into its intended location inside the robotic cell. -
tool_mesh
- The mesh of the robot’s tool, which will also be checked for collisions. -
tool_transform
- The transform to apply to the tool mesh. It defines the location of the “action point” of the robot whose location and rotation (pose) is the pose for direct and inverse kinematics calls. -
collision_environment
- A vector of collision objects represented byCollisionBody
, where each object includes a mesh and its transform. -
safety
- defines the minimal allowed distances between collision objects and specifies other details on how collisions are checked. In practice robot must stay at some safe distance from collision objects rather than touching them.
§Returns
A KinematicsWithShape
instance with defined kinematic structure and shapes for collision detection.
Examples found in repository?
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 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101
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)
},
)
}
More examples
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 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124
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
},
)
}
Source§impl KinematicsWithShape
impl KinematicsWithShape
Sourcepub fn positioned_robot(&self, joint_positions: &Joints) -> PositionedRobot<'_>
pub fn positioned_robot(&self, joint_positions: &Joints) -> PositionedRobot<'_>
Method to compute and return a PositionedRobot
from the current RobotBody
and a set of joint positions.
This method uses the forward kinematics to compute the global transforms of each joint
and then creates the corresponding PositionedJoint
instances, which are collected into a PositionedRobot
.
§Arguments
joint_positions
- A reference to the joint values (angles/positions) to compute the forward kinematics.
§Returns
- A new instance of
PositionedRobot
containing the positioned joints with precomputed transforms.
Source§impl KinematicsWithShape
impl KinematicsWithShape
Sourcepub fn collides(&self, joints: &Joints) -> bool
pub fn collides(&self, joints: &Joints) -> bool
Check for collisions for the given joint position. Both self-collisions and collisions with environment are checked. This method simply returns true (if collides) or false (if not)
Examples found in repository?
135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156
fn main() {
// The robot itself.
let robot = create_rx160_robot();
// Do some inverse kinematics to show the concept.
let pose = Isometry3::from_parts(Translation3::new(0.0, 0.0, 1.5), UnitQuaternion::identity());
let solutions = robot.inverse(&pose);
dump_solutions(&solutions);
if robot.collides(&[173_f64.to_radians(), 0., -94_f64.to_radians(), 0., 0., 0.]) {
println!("Collision detected");
}
// In which position to show the robot on startup
let initial_angles = [173., -8., -94., 6., 83., 207.];
// Boundaries for XYZ draw-bars in visualization GUI
let tcp_box: [RangeInclusive<f64>; 3] = [-2.0..=2.0, -2.0..=2.0, 1.0..=2.0];
visualize(robot, initial_angles, tcp_box);
}
More examples
72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94
fn plan_path(
kinematics: &KinematicsWithShape,
start: Joints, goal: Joints,
) -> Result<Vec<Vec<f64>>, String> {
let collision_free = |joint_angles: &[f64]| -> bool {
let joints = &<Joints>::try_from(joint_angles).expect("Cannot convert vector to array");
!kinematics.collides(joints)
};
// Constraint compliant random joint configuration generator.
let random_joint_angles = || -> Vec<f64> {
// RRT requires vector and we return array so convert
return kinematics.constraints()
.expect("Set joint ranges on kinematics").random_angles().to_vec();
};
// Plan the path with RRT
dual_rrt_connect(
&start, &goal, collision_free,
random_joint_angles, 3_f64.to_radians(), // Step size in joint space
2000, // Max iterations
)
}
Sourcepub fn non_colliding_offsets(
&self,
joints: &Joints,
from: &Joints,
to: &Joints,
) -> Solutions
pub fn non_colliding_offsets( &self, joints: &Joints, from: &Joints, to: &Joints, ) -> Solutions
Return non colliding offsets, tweaking each joint plus minus either side, either into ‘to’ or into ‘from’. This is required for planning algorithms like A*. We can do less collision checks as we only need to check the joint branch of the robot we moved.
Trait Implementations§
Source§impl Kinematics for KinematicsWithShape
impl Kinematics for KinematicsWithShape
Source§fn inverse(&self, pose: &Pose) -> Solutions
fn inverse(&self, pose: &Pose) -> Solutions
Delegates call to underlying Kinematics, but will filter away colliding poses
Source§fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions
fn inverse_continuing(&self, pose: &Pose, previous: &Joints) -> Solutions
Delegates call to underlying Kinematics, but will filter away colliding poses
Source§fn inverse_5dof(&self, pose: &Pose, j6: f64) -> Solutions
fn inverse_5dof(&self, pose: &Pose, j6: f64) -> Solutions
Delegates call to underlying Kinematics, but will filter away colliding poses
Source§fn inverse_continuing_5dof(&self, pose: &Pose, prev: &Joints) -> Solutions
fn inverse_continuing_5dof(&self, pose: &Pose, prev: &Joints) -> Solutions
Delegates call to underlying Kinematics, but will filter away colliding poses
Source§fn forward(&self, qs: &Joints) -> Pose
fn forward(&self, qs: &Joints) -> Pose
Source§fn constraints(&self) -> &Option<Constraints>
fn constraints(&self) -> &Option<Constraints>
Source§fn kinematic_singularity(&self, qs: &Joints) -> Option<Singularity>
fn kinematic_singularity(&self, qs: &Joints) -> Option<Singularity>
Auto Trait Implementations§
impl Freeze for KinematicsWithShape
impl !RefUnwindSafe for KinematicsWithShape
impl Send for KinematicsWithShape
impl Sync for KinematicsWithShape
impl Unpin for KinematicsWithShape
impl !UnwindSafe for KinematicsWithShape
Blanket Implementations§
Source§impl<T, U> AsBindGroupShaderType<U> for T
impl<T, U> AsBindGroupShaderType<U> for T
Source§fn as_bind_group_shader_type(&self, _images: &RenderAssets<Image>) -> U
fn as_bind_group_shader_type(&self, _images: &RenderAssets<Image>) -> U
T
ShaderType
for self
. When used in AsBindGroup
derives, it is safe to assume that all images in self
exist.Source§impl<T> BorrowMut<T> for Twhere
T: ?Sized,
impl<T> BorrowMut<T> for Twhere
T: ?Sized,
Source§fn borrow_mut(&mut self) -> &mut T
fn borrow_mut(&mut self) -> &mut T
Source§impl<T> Downcast for Twhere
T: Any,
impl<T> Downcast for Twhere
T: Any,
Source§fn into_any(self: Box<T>) -> Box<dyn Any>
fn into_any(self: Box<T>) -> Box<dyn Any>
Box<dyn Trait>
(where Trait: Downcast
) to Box<dyn Any>
. Box<dyn Any>
can
then be further downcast
into Box<ConcreteType>
where ConcreteType
implements Trait
.Source§fn into_any_rc(self: Rc<T>) -> Rc<dyn Any>
fn into_any_rc(self: Rc<T>) -> Rc<dyn Any>
Rc<Trait>
(where Trait: Downcast
) to Rc<Any>
. Rc<Any>
can then be
further downcast
into Rc<ConcreteType>
where ConcreteType
implements Trait
.Source§fn as_any(&self) -> &(dyn Any + 'static)
fn as_any(&self) -> &(dyn Any + 'static)
&Trait
(where Trait: Downcast
) to &Any
. This is needed since Rust cannot
generate &Any
’s vtable from &Trait
’s.Source§fn as_any_mut(&mut self) -> &mut (dyn Any + 'static)
fn as_any_mut(&mut self) -> &mut (dyn Any + 'static)
&mut Trait
(where Trait: Downcast
) to &Any
. This is needed since Rust cannot
generate &mut Any
’s vtable from &mut Trait
’s.Source§impl<T> DowncastSync for T
impl<T> DowncastSync for T
Source§impl<T> Instrument for T
impl<T> Instrument for T
Source§fn instrument(self, span: Span) -> Instrumented<Self>
fn instrument(self, span: Span) -> Instrumented<Self>
Source§fn in_current_span(self) -> Instrumented<Self>
fn in_current_span(self) -> Instrumented<Self>
Source§impl<T> IntoEither for T
impl<T> IntoEither for T
Source§fn into_either(self, into_left: bool) -> Either<Self, Self>
fn into_either(self, into_left: bool) -> Either<Self, Self>
self
into a Left
variant of Either<Self, Self>
if into_left
is true
.
Converts self
into a Right
variant of Either<Self, Self>
otherwise. Read moreSource§fn into_either_with<F>(self, into_left: F) -> Either<Self, Self>
fn into_either_with<F>(self, into_left: F) -> Either<Self, Self>
self
into a Left
variant of Either<Self, Self>
if into_left(&self)
returns true
.
Converts self
into a Right
variant of Either<Self, Self>
otherwise. Read moreSource§impl<T> Pointable for T
impl<T> Pointable for T
Source§impl<SS, SP> SupersetOf<SS> for SPwhere
SS: SubsetOf<SP>,
impl<SS, SP> SupersetOf<SS> for SPwhere
SS: SubsetOf<SP>,
Source§fn to_subset(&self) -> Option<SS>
fn to_subset(&self) -> Option<SS>
self
from the equivalent element of its
superset. Read moreSource§fn is_in_subset(&self) -> bool
fn is_in_subset(&self) -> bool
self
is actually part of its subset T
(and can be converted to it).Source§fn to_subset_unchecked(&self) -> SS
fn to_subset_unchecked(&self) -> SS
self.to_subset
but without any property checks. Always succeeds.Source§fn from_subset(element: &SS) -> SP
fn from_subset(element: &SS) -> SP
self
to the equivalent element of its superset.