openrr_planner/
ik.rs

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

openrr_planner/
ik.rs
