pub struct Constraints {
pub from: [f64; 6],
pub to: [f64; 6],
pub centers: [f64; 6],
pub tolerances: [f64; 6],
pub sorting_weight: f64,
}
Fields§
§from: [f64; 6]
Normalized lower limit. If more than upper limit, the range wraps-around through 0
to: [f64; 6]
Normalized upper limit. If less than lower limit, the range wraps-around through 0
centers: [f64; 6]
§tolerances: [f64; 6]
§sorting_weight: f64
Used when sorting the solutions by both middle values of the constraints and the previous values of the joints. 1.0 gives the absolute priority to constraints, 0.0. gives absolute priority for joints.
Implementations§
Source§impl Constraints
impl Constraints
Sourcepub fn new(from: Joints, to: Joints, sorting_weight: f64) -> Self
pub fn new(from: Joints, to: Joints, sorting_weight: f64) -> Self
Create constraints that restrict the joint rotations between ‘from’ to ‘to’ values. Wrapping arround is supported so order is important. For instance, from = 0.1 and to = 0.2 (radians) means the joint is allowed to rotate to 0.11, 0.12 … 1.99, 0.2. from = 0.2 ant to = 0.1 is also valid but means the joint is allowed to rotate to 0.21, 0.22, 0.99, 2 * PI or 0.0 (wrapping around), then to 0.09 and finally 0.1, so the other side of the circle. The sorting_weight parameter influences sorting of the results: 0.0 (or BY_PREV) gives absolute priority to the previous values of the joints, 1.0 (or BY_CONSTRAINTS) gives absolute priority to the middle values of constraints. Intermediate values like 0.5 provide the weighted compromise.
Examples found in repository?
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 39
fn main() {
let robot = OPWKinematics::new_with_constraints(
Parameters::irb2400_10(), Constraints::new(
[-0.1, 0.0, 0.0, 0.0, -PI, -PI],
[PI, PI, 2.0 * PI, PI, PI, PI],
BY_PREV,
));
let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6]
let when_continuing_from_j6_0: [f64; 6] = [0.0, 0.11, 0.22, 0.8, 0.1, 0.0];
let pose: Pose = robot.forward(&joints); // Pose is alias of nalgebra::Isometry3<f64>
println!("If we do not have the previous pose yet, we can now ask to prever the pose \
closer to the center of constraints.");
let solutions = robot.inverse_continuing(&pose, &CONSTRAINT_CENTERED);
dump_solutions(&solutions);
println!("With constraints, sorted by proximity to the previous pose");
let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
dump_solutions(&solutions);
let robot = OPWKinematics::new_with_constraints(
Parameters::irb2400_10(), Constraints::new(
[-0.1, 0.0, 0.0, 0.0, -PI, -PI],
[PI, PI, 2.0 * PI, PI, PI, PI],
BY_CONSTRAINS,
));
println!("With constraints, sorted by proximity to the center of constraints");
let solutions = robot.inverse_continuing(&pose, &when_continuing_from_j6_0);
dump_solutions(&solutions);
}
More examples
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 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
fn main() {
let robot = OPWKinematics::new_with_constraints(
Parameters::irb2400_10(), Constraints::new(
[-0.1, 0.0, 0.0, 0.0, -PI, -PI],
[PI, PI, 2.0 * PI, PI, PI, PI],
BY_CONSTRAINS,
));
let joints: Joints = [0.0, 0.1, 0.2, 0.3, 0.0, 0.5]; // Joints are alias of [f64; 6]
let jakobian = rs_opw_kinematics::jacobian::Jacobian::new(&robot, &joints, 1E-6);
let desired_velocity_isometry =
Isometry3::new(Vector3::new(0.0, 1.0, 0.0),
Vector3::new(0.0, 0.0, 1.0));
let joint_velocities = jakobian.velocities(&desired_velocity_isometry);
println!("Computed joint velocities: {:?}", joint_velocities.unwrap());
let desired_force_torque =
Isometry3::new(Vector3::new(0.0, 0.0, 0.0),
Vector3::new(0.0, 0.0, 1.234));
let joint_torques = jakobian.torques(&desired_force_torque);
println!("Computed joint torques: {:?}", joint_torques);
// Robot with the tool, standing on a base:
let robot_alone = OPWKinematics::new(Parameters::staubli_tx2_160l());
// 1 meter high pedestal
let pedestal = 0.5;
let base_translation = Isometry3::from_parts(
Translation3::new(0.0, 0.0, pedestal).into(),
UnitQuaternion::identity(),
);
let robot_with_base = rs_opw_kinematics::tool::Base {
robot: Arc::new(robot_alone),
base: base_translation,
};
// Tool extends 1 meter in the Z direction, envisioning something like sword
let sword = 1.0;
let tool_translation = Isometry3::from_parts(
Translation3::new(0.0, 0.0, sword).into(),
UnitQuaternion::identity(),
);
// Create the Tool instance with the transformation
let robot_complete = rs_opw_kinematics::tool::Tool {
robot: Arc::new(robot_with_base),
tool: tool_translation,
};
let tcp_pose: Pose = robot_complete.forward(&joints);
println!("The sword tip is at: {:?}", tcp_pose);
}
Sourcepub fn from_degrees(
ranges: [RangeInclusive<f64>; 6],
sorting_weight: f64,
) -> Self
pub fn from_degrees( ranges: [RangeInclusive<f64>; 6], sorting_weight: f64, ) -> Self
Initializes Constraints
from an array of 6 ranges (RangeInclusive<f64>
),
where each range specifies the from
(start) and to
(end) values for each joint.
This is convenience method, where VALUES MUST BE GIVEN IN DEGREES.
§Parameters
ranges
: An array of 6RangeInclusive<f64>
values, each representing a range for one joint.- The start of each range is taken as the
from
bound. - The end of each range is taken as the
to
bound.
- The start of each range is taken as the
sorting_weight
: Af64
value representing the sorting weight for the constraint.
§Returns
A new instance of Constraints
with from
, to
, centers
, and tolerances
calculated
based on the specified ranges.
§Example
use rs_opw_kinematics::constraints::{Constraints, BY_PREV};
let constraints = Constraints::from_degrees(
[0.0..=90.0, 45.0..=135.0, -90.0..=90.0, 0.0..=180.0, -45.0..=45.0, -180.0..=180.0],
BY_PREV);
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,
)
}
More examples
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)
},
)
}
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
},
)
}
pub fn update_range(&mut self, from: Joints, to: Joints)
Sourcepub fn compliant(&self, angles: &[f64; 6]) -> bool
pub fn compliant(&self, angles: &[f64; 6]) -> bool
Checks if all values in the given vector or angles satisfy these constraints.
Sourcepub fn filter(&self, angles: &Vec<[f64; 6]>) -> Vec<[f64; 6]>
pub fn filter(&self, angles: &Vec<[f64; 6]>) -> Vec<[f64; 6]>
Return new vector of angle arrays, removing all that have members not satisfying these constraints.
pub fn to_yaml(&self) -> String
Sourcepub fn random_angles(&self) -> Joints
pub fn random_angles(&self) -> Joints
Generate a random valid angle within the defined constraints for each joint.
Examples found in repository?
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
)
}
Trait Implementations§
Source§impl Clone for Constraints
impl Clone for Constraints
Source§fn clone(&self) -> Constraints
fn clone(&self) -> Constraints
1.0.0 · Source§fn clone_from(&mut self, source: &Self)
fn clone_from(&mut self, source: &Self)
source
. Read moreSource§impl Debug for Constraints
impl Debug for Constraints
impl Copy for Constraints
Auto Trait Implementations§
impl Freeze for Constraints
impl RefUnwindSafe for Constraints
impl Send for Constraints
impl Sync for Constraints
impl Unpin for Constraints
impl UnwindSafe for Constraints
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> CloneToUninit for Twhere
T: Clone,
impl<T> CloneToUninit for Twhere
T: Clone,
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.