openrr_planner/
ik.rs

1/*
2Copyright 2017 Takashi Ogura
3
4Licensed under the Apache License, Version 2.0 (the "License");
5you may not use this file except in compliance with the License.
6You may obtain a copy of the License at
7
8    http://www.apache.org/licenses/LICENSE-2.0
9
10Unless required by applicable law or agreed to in writing, software
11distributed under the License is distributed on an "AS IS" BASIS,
12WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13See the License for the specific language governing permissions and
14limitations under the License.
15*/
16#![allow(clippy::trivially_copy_pass_by_ref)]
17
18use k::{nalgebra as na, InverseKinematicsSolver, SubsetOf};
19use na::RealField;
20use parking_lot::Mutex;
21use rayon::iter::{IntoParallelRefIterator, ParallelIterator};
22
23use crate::funcs::*;
24
25/// Randomize initial joint angles before solving
26#[derive(Debug)]
27pub struct RandomInitializeIkSolver<T, I>
28where
29    I: InverseKinematicsSolver<T>,
30    T: RealField,
31{
32    /// The IK solver to be used after set random joint angles
33    pub solver: I,
34    /// The number to try to solve
35    pub num_max_try: usize,
36    phantom: ::std::marker::PhantomData<T>,
37}
38
39impl<T, I> RandomInitializeIkSolver<T, I>
40where
41    T: RealField,
42    I: InverseKinematicsSolver<T>,
43{
44    pub fn new(solver: I, num_max_try: usize) -> Self {
45        RandomInitializeIkSolver {
46            solver,
47            num_max_try,
48            phantom: ::std::marker::PhantomData,
49        }
50    }
51}
52
53impl<T, I> InverseKinematicsSolver<T> for RandomInitializeIkSolver<T, I>
54where
55    T: RealField + Copy + SubsetOf<f64>,
56    I: InverseKinematicsSolver<T>,
57{
58    fn solve_with_constraints(
59        &self,
60        arm: &k::SerialChain<T>,
61        target_pose: &na::Isometry3<T>,
62        constraints: &k::Constraints,
63    ) -> ::std::result::Result<(), k::Error> {
64        let mut result = Err(k::Error::NotConvergedError {
65            num_tried: 0,
66            position_diff: na::Vector3::new(0.0, 0.0, 0.0),
67            rotation_diff: na::Vector3::new(0.0, 0.0, 0.0),
68        });
69        let limits = arm.iter_joints().map(|j| j.limits).collect();
70        let initial_angles = arm.joint_positions();
71
72        for _ in 0..self.num_max_try {
73            result = self
74                .solver
75                .solve_with_constraints(arm, target_pose, constraints);
76            if result.is_ok() {
77                return result;
78            }
79            let mut new_angles = generate_random_joint_positions_from_limits(&limits);
80            modify_to_nearest_angle(&initial_angles, &mut new_angles, &limits);
81            arm.set_joint_positions(&new_angles)?;
82        }
83        // failed
84        arm.set_joint_positions(&initial_angles)?;
85        result
86    }
87}
88
89/// Check the poses which can be reached by the robot arm
90pub fn get_reachable_region<T, I>(
91    ik_solver: &I,
92    arm: &k::SerialChain<T>,
93    initial_pose: &na::Isometry3<T>,
94    constraints: &k::Constraints,
95    max_point: na::Vector3<T>,
96    min_point: na::Vector3<T>,
97    unit_check_length: T,
98) -> Vec<na::Isometry3<T>>
99where
100    T: RealField + Copy + k::SubsetOf<f64> + Send + Sync,
101    I: InverseKinematicsSolver<T> + Send + Sync,
102{
103    let initial_angles = arm.joint_positions();
104    let solved_poses = Mutex::new(Vec::new());
105    let target_pose = *initial_pose;
106
107    let mut z_points = vec![];
108    let mut z = min_point[2];
109    while z < max_point[2] {
110        z_points.push(z);
111        z += unit_check_length;
112    }
113
114    z_points.par_iter().for_each(|&z| {
115        let arm = arm.clone();
116        let mut target_pose = target_pose;
117        target_pose.translation.vector[2] = z;
118        let mut y = min_point[1];
119        while y < max_point[1] {
120            target_pose.translation.vector[1] = y;
121            let mut x = min_point[0];
122            while x < max_point[0] {
123                target_pose.translation.vector[0] = x;
124                arm.set_joint_positions_unchecked(&initial_angles);
125                if ik_solver
126                    .solve_with_constraints(&arm, &target_pose, constraints)
127                    .is_ok()
128                {
129                    solved_poses.lock().push(target_pose);
130                }
131                x += unit_check_length;
132            }
133            y += unit_check_length;
134        }
135    });
136    solved_poses.into_inner()
137}
138
139#[cfg(test)]
140mod tests {
141    use super::*;
142
143    #[test]
144    fn get_region() {
145        let robot = k::Chain::<f32>::from_urdf_file("sample.urdf").unwrap();
146        let target_link = robot.find("l_tool_fixed").unwrap();
147        let chain = k::SerialChain::from_end(target_link);
148
149        // Set initial joint angles
150        let angles = vec![0.2, 0.2, 0.0, -1.0, 0.0, 0.0];
151
152        chain.set_joint_positions(&angles).unwrap();
153        println!("initial angles={:?}", chain.joint_positions());
154
155        // Get the transform of the end of the manipulator (forward kinematics)
156        chain.update_transforms();
157        let target = target_link.world_transform().unwrap();
158        println!("{:?}", target.translation);
159        // Create IK solver with default settings
160        let solver = k::JacobianIkSolver::default();
161
162        // Create a set of joints from end joint
163        let arm = k::SerialChain::from_end(target_link);
164        let regions = get_reachable_region(
165            &solver,
166            &arm,
167            &target,
168            &k::Constraints::default(),
169            na::Vector3::new(0.8, 0.9, 0.9),
170            na::Vector3::new(0.0, -0.9, 0.0),
171            0.1,
172        );
173        assert_eq!(regions.len(), 114);
174    }
175}
openrr_planner/ik.rs

openrr_planner/
ik.rs
