openrr_planner/planner/
ik_planner.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)]
17use k::nalgebra as na;
18use na::RealField;
19use ncollide3d::shape::Compound;
20
21use super::joint_path_planner::JointPathPlanner;
22use crate::errors::*;
23
24/// Joint path planner which supports inverse kinematics
25pub struct JointPathPlannerWithIk<T, I>
26where
27    I: k::InverseKinematicsSolver<T>,
28    T: RealField + Copy + k::SubsetOf<f64>,
29{
30    /// Joint Path Planner to be used to find collision free path
31    ///
32    /// Currently, `JointPathPlanner<N, k::Chain<N>>` is used.
33    pub path_planner: JointPathPlanner<T>,
34    /// Inverse kinematics solver to find the goal joint angles
35    pub ik_solver: I,
36}
37
38impl<T, I> JointPathPlannerWithIk<T, I>
39where
40    //T: RealField + num_traits::Float + k::SubsetOf<f64>,
41    T: RealField + k::SubsetOf<f64> + num_traits::Float,
42    I: k::InverseKinematicsSolver<T>,
43{
44    /// Create instance from `JointPathPlannerBuilder` and `InverseKinematicsSolver`
45    ///
46    /// # Example
47    ///
48    /// ```
49    /// // Create path planner with loading urdf file and set end link name
50    /// let robot = k::Chain::from_urdf_file("sample.urdf").unwrap();
51    /// let planner = openrr_planner::JointPathPlannerBuilder::from_urdf_file("sample.urdf")
52    ///     .unwrap()
53    ///     .collision_check_margin(0.01)
54    ///     .reference_robot(std::sync::Arc::new(robot))
55    ///     .finalize()
56    ///     .unwrap();
57    /// // Create inverse kinematics solver
58    /// let solver = openrr_planner::JacobianIkSolver::default();
59    /// // Create path planner with IK solver
60    /// let _planner = openrr_planner::JointPathPlannerWithIk::new(planner, solver);
61    /// ```
62    pub fn new(path_planner: JointPathPlanner<T>, ik_solver: I) -> Self {
63        Self {
64            path_planner,
65            ik_solver,
66        }
67    }
68
69    /// Just solve IK and do not plan
70    pub fn solve_ik(
71        &mut self,
72        arm: &k::SerialChain<T>,
73        target_pose: &na::Isometry3<T>,
74    ) -> Result<()> {
75        Ok(self.ik_solver.solve(arm, target_pose)?)
76    }
77
78    /// Just solve IK with constraints and do not plan
79    pub fn solve_ik_with_constraints(
80        &mut self,
81        arm: &k::SerialChain<T>,
82        target_pose: &na::Isometry3<T>,
83        c: &k::Constraints,
84    ) -> Result<()> {
85        Ok(self.ik_solver.solve_with_constraints(arm, target_pose, c)?)
86    }
87
88    pub fn colliding_link_names(&self, objects: &Compound<T>) -> Vec<String> {
89        self.path_planner.env_collision_link_names(objects)
90    }
91
92    /// Solve IK and get the path to the final joint positions
93    pub fn plan_with_ik(
94        &mut self,
95        target_name: &str,
96        target_pose: &na::Isometry3<T>,
97        objects: &Compound<T>,
98    ) -> Result<Vec<Vec<T>>> {
99        self.plan_with_ik_with_constraints(
100            target_name,
101            target_pose,
102            objects,
103            &k::Constraints::default(),
104        )
105    }
106
107    /// Solve IK with constraints and get the path to the final joint positions
108    pub fn plan_with_ik_with_constraints(
109        &mut self,
110        target_name: &str,
111        target_pose: &na::Isometry3<T>,
112        objects: &Compound<T>,
113        constraints: &k::Constraints,
114    ) -> Result<Vec<Vec<T>>> {
115        self.path_planner.sync_joint_positions_with_reference();
116
117        let end_link = self
118            .path_planner
119            .collision_check_robot()
120            .find(target_name)
121            .ok_or_else(|| Error::NotFound(target_name.to_owned()))?;
122        let arm = k::SerialChain::from_end(end_link);
123        let initial = arm.joint_positions();
124        let using_joint_names = arm
125            .iter_joints()
126            .map(|j| j.name.clone())
127            .collect::<Vec<String>>();
128        self.ik_solver
129            .solve_with_constraints(&arm, target_pose, constraints)?;
130        let goal = arm.joint_positions();
131        self.path_planner
132            .plan(using_joint_names.as_slice(), &initial, &goal, objects)
133    }
134
135    /// Do not solve IK but get the path to the target joint positions
136    pub fn plan_joints<K>(
137        &mut self,
138        using_joint_names: &[String],
139        start_angles: &[T],
140        goal_angles: &[T],
141        objects: &Compound<T>,
142    ) -> Result<Vec<Vec<T>>> {
143        self.path_planner
144            .plan(using_joint_names, start_angles, goal_angles, objects)
145    }
146
147    /// Calculate the transforms of all of the links
148    pub fn update_transforms(&self) -> Vec<na::Isometry3<T>> {
149        self.path_planner.update_transforms()
150    }
151
152    /// Get the names of the links
153    pub fn joint_names(&self) -> Vec<String> {
154        self.path_planner.joint_names()
155    }
156}
openrr_planner/planner/ik_planner.rs

openrr_planner/planner/
ik_planner.rs
