Some tweaks in Cartesian planner.

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "rs-opw-kinematics"
-version = "1.8.2"
+version = "1.8.3"
 autoexamples = true
 edition = "2021"
 authors = ["Bourumir Wyngs <bourumir.wyngs [at] gmail.com>"]
@@ -60,7 +60,7 @@ visualization = ["bevy", "bevy_egui", "collisions", "allow_filesystem"]
 stroke_planning = ["rrt", "collisions"]
 
 # To disable filesystem, collision and visualizatio support:
-# rs-opw-kinematics = { version = "1.8.2", default-features = false }
+# rs-opw-kinematics = { version = "1.8.3", default-features = false }
 
 [profile.release]
 

README.md

@@ -76,7 +76,7 @@ If you have the OPW robot and not sure how to configure it for this tool, contac
 ## Constraints
 
 Since 1.1.0, it is possible to
-set [constraints](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/constraints/index.html) for the joints.
+set [constraints](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/constraints/index.html) for the joints.
 Robot poses where any of the joints are outside
 the specified constraint range are not included in the returned list of solutions. It is also possible to
 influence the sorting of the result list by giving some preference to the center of constraints.
@@ -90,19 +90,19 @@ if _from_ = 5° and _to_ = 15°, values 6°, 8°, and 11° are va
 Constraints are tested for the range from -2π to 2π, but as angles repeat with the period of 2π, the
 constraint from -π to π already permits free rotation, covering any angle.
 
-Since 1.7.0, convenience method exists to specify constraints as ranges in degrees.
+Since 1.8.2, convenience method exists to specify constraints as ranges in degrees.
 
 Please see the [example](examples/constraints.rs).
 
 ## Jacobian: torques and velocities
 
 Since 1.3.2, it is possible to obtain
-the [Jacobian](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/jacobian/struct.Jacobian.html) that represents
+the [Jacobian](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/jacobian/struct.Jacobian.html) that represents
 the relationship between the joint velocities
 and the end-effector velocities. The computed Jacobian object provides:
 
-- Joint [velocities](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/jacobian/struct.Jacobian.html#method.velocities) required to achieve a desired end-effector velocity.
-- Joint [torques](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/jacobian/struct.Jacobian.html#method.torques) required to achieve a desired end-effector force/torque.
+- Joint [velocities](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/jacobian/struct.Jacobian.html#method.velocities) required to achieve a desired end-effector velocity.
+- Joint [torques](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/jacobian/struct.Jacobian.html#method.torques) required to achieve a desired end-effector force/torque.
 
 The same Joints structure is reused, the six values now representing either angular velocities in radians per second
 or torques in Newton meters. For the end effector, it is possible to use either
@@ -118,15 +118,15 @@ Please see the [example](examples/jacobian.rs).
 ## The tool and the base
 
 Since 1.3.2, robot can be equipped with
-the [tool](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/tool/struct.Tool.html), defined as
+the [tool](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/tool/struct.Tool.html), defined as
 nalgebra::[Isometry3](https://docs.rs/nalgebra/latest/nalgebra/geometry/type.Isometry3.html). The tool isometry defines
 both
 additional translation and additional rotation. The "pose" as defined in forward and inverse kinematics
 now becomes the pose of the tool center point, not any part of the robot. The robot can also be placed
-on a [base](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/tool/struct.Base.html), further supporting the conditions much closer to the real industrial environment.
+on a [base](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/tool/struct.Base.html), further supporting the conditions much closer to the real industrial environment.
 
 "Robot with the tool" and "Robot on the base" can be constructed around
-any [Kinematics](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/kinematic_traits/trait.Kinematics.html)
+any [Kinematics](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/kinematic_traits/trait.Kinematics.html)
 trait, and implement
 this trait themselves. It is possible to cascade them, constructing a robot on a base and with the tool (or
 two tools if the first is a receptacle of the tool changer).
@@ -135,21 +135,21 @@ Please see the [example](examples/tool_and_base.rs).
 
 ## The frame
 
-Since 1.7.0 this package supports the frame transform that allows to transform the robot trajectory (in terms of joint
+Since 1.8.2 this package supports the frame transform that allows to transform the robot trajectory (in terms of joint
 angles)
 prepared for one location to make the same kind of movements in another location (translated and rotated).
 Frame in robotics is most commonly defined by the 3 pairs of points (to and from) if the transform includes
 also rotation, or just a single pair is enough if only shift (but not a rotation) is involved.
 
 Once constructed by specifying original and transformed points, the Frame object can take "canonical" joint angles
 and calculated joint angles for the transformed (shifted and rotated) trajectory. See the
-[frame](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/frame/index.html) documentation and [example](examples/frame.rs) for details.
+[frame](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/frame/index.html) documentation and [example](examples/frame.rs) for details.
 
 ## Individual link positions
 
 It is now possible to obtain positions of individual links in forward kinematics. This would be needed for
 collision avoidance and graphical rendering of the robot.
-See [forward_with_joint_poses](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/kinematic_traits/trait.Kinematics.html#tymethod.forward_with_joint_poses)
+See [forward_with_joint_poses](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/kinematic_traits/trait.Kinematics.html#tymethod.forward_with_joint_poses)
 method.
 
 ## 5 DOF inverse kinematics
@@ -194,13 +194,13 @@ The scaling factor `s` determines how much influence J₂ has on J₃. A scaling
 factor of 1.0 is the common, as this value ensures the end-effector’s
 orientation remains unchanged if only J₃ and J₂ move.
 
-See [Parallelogram](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/parralelogram/struct.Parralelogram.html) and [example](examples/paralellogram.rs).
+See [Parallelogram](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/parralelogram/struct.Parralelogram.html) and [example](examples/paralellogram.rs).
 
 ## Collision avoidance
 The new class 
-[KinematicsWithShape](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/kinematics_with_shape/struct.KinematicsWithShape.html) 
+[KinematicsWithShape](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/kinematics_with_shape/struct.KinematicsWithShape.html) 
 combines kinematics and collision checking. It implements the 
-[Kinematics](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/kinematic_traits/trait.Kinematics.html) trait, 
+[Kinematics](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/kinematic_traits/trait.Kinematics.html) trait, 
 providing both forward and inverse kinematic solutions. During inverse kinematics, any colliding poses are excluded 
 from the solution list.
 
@@ -216,7 +216,7 @@ Setting a zero safety distance is only advisable if the meshes are "inflated" (m
 but achieving this reliably is challenging and requires careful validation. On the other hand, checking for contact 
 rather than maintaining a safety distance is significantly faster.
 
-Safety distances are controlled through [SafetyDistances](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/collisions/struct.SafetyDistances.html) structure as shown in the example below. 
+Safety distances are controlled through [SafetyDistances](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/collisions/struct.SafetyDistances.html) structure as shown in the example below. 
 
 Safety distances can be configured separately for robot-to-robot and robot-to-environment collisions. 
 Shorter distances can be specified for joints that naturally operate in proximity.
@@ -470,10 +470,10 @@ constraints to be sure everything is collision-free anyway.
 Please see the [example](examples/cartesian_stroke.rs).
 
 ## Visualization
-[KinematicsWithShape](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/kinematics_with_shape/struct.KinematicsWithShape.html)
+[KinematicsWithShape](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/kinematics_with_shape/struct.KinematicsWithShape.html)
 is also straightforward to visualize, as it fully integrates both the kinematics and 3D meshes representing the robot.
 To display it, simply pass this structure to the built-in function 
-[visualize_robot](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/visualization/fn.visualize_robot.html):
+[visualize_robot](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/visualization/fn.visualize_robot.html):
 
 ```Rust
 fn main() {
@@ -511,7 +511,7 @@ Cargo.toml:
 
 ```toml
 [dependencies]
-rs-opw-kinematics = ">=1.7.0, <2.0.0"
+rs-opw-kinematics = ">=1.8.2, <2.0.0"
 ```
 
 Simple "hello world" demonstrating singularity evasion would look more or less like this:
@@ -537,7 +537,7 @@ fn main() {
 }
 ```
 
-Starting from version 1.7.0, rs-opw-kinematics has evolved beyond being just a set of "useful building blocks." It now
+Starting from version 1.8.2, rs-opw-kinematics has evolved beyond being just a set of "useful building blocks." It now
 enables the creation of a complete robot setup, which includes mounting the robot on a base, equipping it with a tool,
 and integrating collision checking. See example _complete_visible_robot_.
 
@@ -567,7 +567,7 @@ Since version 1.2.0, parameters and constraints can also be directly extracted f
 ```
 
 There is also more advanced
-function [rs_opw_kinematics::urdf::from_urdf](https://docs.rs/rs-opw-kinematics/1.7.0/rs_opw_kinematics/urdf/fn.from_urdf.html)
+function [rs_opw_kinematics::urdf::from_urdf](https://docs.rs/rs-opw-kinematics/1.8.2/rs_opw_kinematics/urdf/fn.from_urdf.html)
 that takes URDF string rather than the file, provides error handling and much more control over how the solver
 is constructed from the extracted values.
 

src/path_plan/cartesian.rs

@@ -193,6 +193,7 @@ impl Cartesian<'_> {
             return Err("Unable to start from onboarding point".into());
         }
         let poses = self.with_intermediate_poses(land, &steps, park);
+        println!("Probing {} strategies", strategies.len());
         strategies
             .par_iter()
             .find_map_any(
@@ -294,12 +295,14 @@ impl Cartesian<'_> {
         Ok(trace)
     }
 
+    // Transition cartesian way from 'from' into 'to' while assuming 'from'
+    // Returns the resolved end pose.
     // Transition cartesian way from 'from' into 'to' while assuming 'from'
     // Returns the resolved end pose.
     fn step_adaptive_linear_transition(
         &self,
         starting: &Joints,
-        from: &AnnotatedPose,
+        from: &AnnotatedPose, // FK of "starting"
         to: &AnnotatedPose,
         depth: usize,
     ) -> Result<Joints, Transition> {
@@ -314,51 +317,40 @@ impl Cartesian<'_> {
 
         pub const DIV_RATIO: f64 = 0.5;
 
-        let midpoint = from.interpolate(to, DIV_RATIO);
-        let poses = vec![from, &midpoint, to];
-
-        let mut current = *starting;
-        let mut prev_pose = from;
-        for pose in poses {
-            let mut success = false;
-
-            // Not checked for collisions yet
-            let solutions = self
-                .robot
-                .kinematics
-                .inverse_continuing(&pose.pose, &current);
-
-            // Solutions are already sorted best first
-            for next in &solutions {
-                // Internal "miniposes" generated through recursion are not checked for collision.
-                // Outer calling code is reposible for check if 'current' is collision free
-                if self.transitionable(&current, next) {
-                    success = true;
-                    current = *next;
-                    break; // break out of solutions loop
-                }
-            }
-            if !success {
-                // Try recursive call. If succeeds, assume as done anyway.
-                if depth >= self.linear_recursion_depth {
-                    return Err(Transition {
-                        from: *prev_pose,
-                        to: *pose,
-                        previous: current,
-                        solutions: solutions,
-                    });
-                }
+        // Not checked for collisions yet
+        let solutions = self.robot.kinematics.inverse_continuing(&to.pose, &starting);
 
-                // Try to bridge till them middle first, and then from the middle
-                // This will result in shorter distance between from and to.
-                let till_middle =
-                    self.step_adaptive_linear_transition(starting, from, &midpoint, depth + 1)?;
-                current =
-                    self.step_adaptive_linear_transition(&till_middle, &midpoint, to, depth + 1)?;
+        // Solutions are already sorted best first
+        for next in &solutions {
+            // Internal "miniposes" generated through recursion are not checked for collision.
+            // They only check agains continuity of the robot movement (no unexpected jerks)
+            if self.transitionable(&starting, next) {
+                return Ok(next.clone());
             }
-            prev_pose = pose;
         }
-        Ok(current)
+
+        // Transitioning not successful.
+        // Recursive call reduces step, the goal is to check if there is a continuous
+        // linear path on any step.
+        if depth < self.linear_recursion_depth {
+
+            // Try to bridge till them middle first, and then from the middle
+            // This will result in a shorter distance between from and to.            
+            let midpose = from.interpolate(to, DIV_RATIO);
+            let middle_joints =
+                self.step_adaptive_linear_transition(starting, from, &midpose, depth + 1)?;
+
+            // If both bridgings were successful, return the final position that resulted from
+            // bridging from middle joints to the final pose on this step
+            return  Ok(self.step_adaptive_linear_transition(&middle_joints, &midpose, to, depth + 1)?);
+        }
+
+        Err(Transition {
+            from: from.clone(),
+            to: to.clone(),
+            previous: starting.clone(),
+            solutions: solutions,
+        })
     }
 
     fn log_failed_transition(&self, transition: &Transition, step: i32) {