1use std::{path::Path, sync::Arc};
17
18use k::nalgebra as na;
19use na::RealField;
20use ncollide3d::shape::Compound;
21use schemars::JsonSchema;
22use serde::{Deserialize, Serialize};
23use tracing::*;
24
25use crate::{
26 collision::{CollisionDetector, RobotCollisionDetector},
27 errors::*,
28 funcs::*,
29};
30
31pub struct JointPathPlanner<N>
33where
34 N: RealField + Copy + k::SubsetOf<f64>,
35{
36 reference_robot: Arc<k::Chain<N>>,
38 robot_collision_detector: RobotCollisionDetector<N>,
40 pub step_length: N,
44 pub max_try: usize,
46 pub num_smoothing: usize,
48}
49
50impl<N> JointPathPlanner<N>
51where
52 N: RealField + num_traits::Float + k::SubsetOf<f64>,
53{
54 pub fn new(
56 reference_robot: Arc<k::Chain<N>>,
57 robot_collision_detector: RobotCollisionDetector<N>,
58 step_length: N,
59 max_try: usize,
60 num_smoothing: usize,
61 ) -> Self {
62 JointPathPlanner {
63 reference_robot,
64 robot_collision_detector,
65 step_length,
66 max_try,
67 num_smoothing,
68 }
69 }
70
71 fn is_feasible(
73 &self,
74 using_joints: &k::Chain<N>,
75 joint_positions: &[N],
76 objects: &Compound<N>,
77 ) -> bool {
78 match using_joints.set_joint_positions(joint_positions) {
79 Ok(()) => !self.robot_collision_detector.is_collision_detected(objects),
80 Err(err) => {
81 debug!("is_feasible: {err}");
82 false
83 }
84 }
85 }
86
87 fn is_feasible_with_self(&self, using_joints: &k::Chain<N>, joint_positions: &[N]) -> bool {
89 match using_joints.set_joint_positions(joint_positions) {
90 Ok(()) => !self.robot_collision_detector.is_self_collision_detected(),
91 Err(err) => {
92 debug!("is_feasible: {err}");
93 false
94 }
95 }
96 }
97
98 pub fn plan(
107 &self,
108 using_joint_names: &[String],
109 start_angles: &[N],
110 goal_angles: &[N],
111 objects: &Compound<N>,
112 ) -> Result<Vec<Vec<N>>> {
113 self.sync_joint_positions_with_reference();
114
115 let using_joints =
116 create_chain_from_joint_names(self.collision_check_robot(), using_joint_names)?;
117 let limits = using_joints.iter_joints().map(|j| j.limits).collect();
118 let step_length = self.step_length;
119 let max_try = self.max_try;
120 let current_angles = using_joints.joint_positions();
121
122 if !self.is_feasible(&using_joints, start_angles, objects) {
123 let collision_link_names = self.env_collision_link_names(objects);
124 using_joints.set_joint_positions(¤t_angles)?;
125 return Err(Error::Collision {
126 point: UnfeasibleTrajectoryPoint::Start,
127 collision_link_names,
128 });
129 } else if !self.is_feasible(&using_joints, goal_angles, objects) {
130 let collision_link_names = self.env_collision_link_names(objects);
131 using_joints.set_joint_positions(¤t_angles)?;
132 return Err(Error::Collision {
133 point: UnfeasibleTrajectoryPoint::Goal,
134 collision_link_names,
135 });
136 }
137
138 let mut path = match rrt::dual_rrt_connect(
139 start_angles,
140 goal_angles,
141 |angles: &[N]| self.is_feasible(&using_joints, angles, objects),
142 || generate_random_joint_positions_from_limits(&limits),
143 step_length,
144 max_try,
145 ) {
146 Ok(p) => p,
147 Err(error) => {
148 using_joints.set_joint_positions(¤t_angles)?;
149 return Err(Error::PathPlanFail(error));
150 }
151 };
152 let num_smoothing = self.num_smoothing;
153 rrt::smooth_path(
154 &mut path,
155 |angles: &[N]| self.is_feasible(&using_joints, angles, objects),
156 step_length,
157 num_smoothing,
158 );
159
160 using_joints.set_joint_positions(goal_angles)?;
163 Ok(path)
164 }
165
166 pub fn plan_avoid_self_collision(
174 &self,
175 using_joint_names: &[String],
176 start_angles: &[N],
177 goal_angles: &[N],
178 ) -> Result<Vec<Vec<N>>> {
179 self.sync_joint_positions_with_reference();
180
181 let using_joints =
182 create_chain_from_joint_names(self.collision_check_robot(), using_joint_names)?;
183 let limits = using_joints.iter_joints().map(|j| j.limits).collect();
184 let step_length = self.step_length;
185 let max_try = self.max_try;
186 let current_angles = using_joints.joint_positions();
187
188 if !self.is_feasible_with_self(&using_joints, start_angles) {
189 let collision_link_names = self.self_collision_link_pairs();
190 using_joints.set_joint_positions(¤t_angles)?;
191 return Err(Error::SelfCollision {
192 point: UnfeasibleTrajectoryPoint::Start,
193 collision_link_names,
194 });
195 } else if !self.is_feasible_with_self(&using_joints, goal_angles) {
196 let collision_link_names = self.self_collision_link_pairs();
197 using_joints.set_joint_positions(¤t_angles)?;
198 return Err(Error::SelfCollision {
199 point: UnfeasibleTrajectoryPoint::Goal,
200 collision_link_names,
201 });
202 }
203
204 let mut path = match rrt::dual_rrt_connect(
205 start_angles,
206 goal_angles,
207 |angles: &[N]| self.is_feasible_with_self(&using_joints, angles),
208 || generate_random_joint_positions_from_limits(&limits),
209 step_length,
210 max_try,
211 ) {
212 Ok(p) => p,
213 Err(error) => {
214 using_joints.set_joint_positions(¤t_angles)?;
215 return Err(Error::PathPlanFail(error));
216 }
217 };
218 let num_smoothing = self.num_smoothing;
219 rrt::smooth_path(
220 &mut path,
221 |angles: &[N]| self.is_feasible_with_self(&using_joints, angles),
222 step_length,
223 num_smoothing,
224 );
225 Ok(path)
226 }
227
228 pub fn sync_joint_positions_with_reference(&self) {
230 self.collision_check_robot()
231 .set_joint_positions_clamped(self.reference_robot.joint_positions().as_slice());
232 }
233
234 pub fn update_transforms(&self) -> Vec<na::Isometry3<N>> {
236 self.collision_check_robot().update_transforms()
237 }
238
239 pub fn joint_names(&self) -> Vec<String> {
241 self.collision_check_robot()
242 .iter_joints()
243 .map(|j| j.name.clone())
244 .collect()
245 }
246
247 pub fn collision_check_robot(&self) -> &k::Chain<N> {
249 &self.robot_collision_detector.robot
250 }
251
252 pub fn env_collision_link_names(&self, objects: &Compound<N>) -> Vec<String> {
255 self.robot_collision_detector
256 .env_collision_link_names(objects)
257 }
258
259 pub fn self_collision_link_pairs(&self) -> Vec<(String, String)> {
261 self.robot_collision_detector.self_collision_link_pairs()
262 }
263}
264
265pub struct JointPathPlannerBuilder<N>
267where
268 N: RealField + Copy + k::SubsetOf<f64>,
269{
270 reference_robot: Option<Arc<k::Chain<N>>>,
271 robot_collision_detector: RobotCollisionDetector<N>,
272 step_length: N,
273 max_try: usize,
274 num_smoothing: usize,
275 collision_check_margin: Option<N>,
276 self_collision_pairs: Vec<(String, String)>,
277}
278
279impl<N> JointPathPlannerBuilder<N>
280where
281 N: RealField + num_traits::Float + k::SubsetOf<f64>,
282{
283 pub fn new(robot_collision_detector: RobotCollisionDetector<N>) -> Self {
287 JointPathPlannerBuilder {
288 reference_robot: None,
289 robot_collision_detector,
290 step_length: na::convert(0.1),
291 max_try: 5000,
292 num_smoothing: 100,
293 collision_check_margin: None,
294 self_collision_pairs: vec![],
295 }
296 }
297
298 pub fn reference_robot(mut self, robot: Arc<k::Chain<N>>) -> Self {
299 self.reference_robot = Some(robot);
300 self
301 }
302
303 pub fn collision_check_margin(mut self, length: N) -> Self {
304 self.collision_check_margin = Some(length);
305 self
306 }
307
308 pub fn step_length(mut self, step_length: N) -> Self {
309 self.step_length = step_length;
310 self
311 }
312
313 pub fn max_try(mut self, max_try: usize) -> Self {
314 self.max_try = max_try;
315 self
316 }
317
318 pub fn num_smoothing(mut self, num_smoothing: usize) -> Self {
319 self.num_smoothing = num_smoothing;
320 self
321 }
322
323 pub fn self_collision_pairs(mut self, self_collision_pairs: Vec<(String, String)>) -> Self {
324 self.self_collision_pairs = self_collision_pairs;
325 self
326 }
327
328 pub fn finalize(mut self) -> Result<JointPathPlanner<N>> {
329 if let Some(margin) = self.collision_check_margin {
330 self.robot_collision_detector.collision_detector.prediction = margin;
331 }
332
333 match self.reference_robot {
334 Some(robot) => {
335 let mut planner = JointPathPlanner::new(
336 robot,
337 self.robot_collision_detector,
338 self.step_length,
339 self.max_try,
340 self.num_smoothing,
341 );
342 planner.robot_collision_detector.self_collision_pairs = self.self_collision_pairs;
343 Ok(planner)
344 }
345 None => Err(Error::ReferenceRobot("JointPathBuilder".to_owned())),
346 }
347 }
348
349 pub fn from_urdf_file<P>(file: P) -> Result<JointPathPlannerBuilder<N>>
351 where
352 P: AsRef<Path>,
353 {
354 let urdf_robot = urdf_rs::utils::read_urdf_or_xacro(file.as_ref())?;
355 Ok(JointPathPlannerBuilder::from_urdf_robot_with_base_dir(
356 urdf_robot,
357 file.as_ref().parent(),
358 ))
359 }
360
361 pub fn from_urdf_robot_with_base_dir(
363 urdf_robot: urdf_rs::Robot,
364 base_path: Option<&Path>,
365 ) -> JointPathPlannerBuilder<N> {
366 let robot = k::Chain::from(&urdf_robot);
367 let default_margin = na::convert(0.0);
368 let collision_detector = CollisionDetector::from_urdf_robot_with_base_dir(
369 &urdf_robot,
370 base_path,
371 default_margin,
372 );
373 let robot_collision_detector =
374 RobotCollisionDetector::new(robot, collision_detector, vec![]);
375 JointPathPlannerBuilder::new(robot_collision_detector)
376 }
377}
378
379#[derive(Clone, Serialize, Deserialize, Debug, JsonSchema)]
380#[serde(deny_unknown_fields)]
381pub struct JointPathPlannerConfig {
382 #[serde(default = "default_step_length")]
383 step_length: f64,
384 #[serde(default = "default_max_try")]
385 max_try: usize,
386 #[serde(default = "default_num_smoothing")]
387 num_smoothing: usize,
388 #[serde(default = "default_margin")]
389 margin: f64,
390}
391
392fn default_step_length() -> f64 {
393 0.1
394}
395
396fn default_max_try() -> usize {
397 5000
398}
399
400fn default_num_smoothing() -> usize {
401 100
402}
403
404fn default_margin() -> f64 {
405 0.001
406}
407
408impl Default for JointPathPlannerConfig {
409 fn default() -> Self {
410 Self {
411 step_length: default_step_length(),
412 max_try: default_max_try(),
413 num_smoothing: default_num_smoothing(),
414 margin: default_margin(),
415 }
416 }
417}
418
419pub fn create_joint_path_planner<P: AsRef<Path>>(
420 urdf_path: P,
421 self_collision_check_pairs: Vec<(String, String)>,
422 config: &JointPathPlannerConfig,
423 robot: Arc<k::Chain<f64>>,
424) -> JointPathPlanner<f64> {
425 JointPathPlannerBuilder::from_urdf_file(urdf_path)
426 .unwrap()
427 .step_length(config.step_length)
428 .max_try(config.max_try)
429 .num_smoothing(config.num_smoothing)
430 .collision_check_margin(config.margin)
431 .self_collision_pairs(self_collision_check_pairs)
432 .reference_robot(robot)
433 .finalize()
434 .unwrap()
435}
436
437#[cfg(test)]
438mod tests {
439 use super::*;
440 use crate::collision::create_all_collision_pairs;
441
442 #[test]
443 fn from_urdf() {
444 let urdf_path = Path::new("sample.urdf");
445 let _planner = JointPathPlannerBuilder::from_urdf_file(urdf_path)
446 .unwrap()
447 .collision_check_margin(0.01)
448 .finalize();
449 }
450
451 #[test]
452 fn test_create_joint_path_planner() {
453 let urdf_path = Path::new("sample.urdf");
454 let robot = k::Chain::from_urdf_file(urdf_path).unwrap();
455 let planner = create_joint_path_planner(
456 urdf_path,
457 create_all_collision_pairs(&robot),
458 &JointPathPlannerConfig::default(),
459 Arc::new(robot),
460 );
461
462 let l_shoulder_yaw_node = planner
463 .collision_check_robot()
464 .find("l_shoulder_yaw")
465 .unwrap();
466 let using_joints = k::SerialChain::from_end(l_shoulder_yaw_node);
467 let using_joint_names = using_joints
468 .iter_joints()
469 .map(|j| j.name.to_owned())
470 .collect::<Vec<String>>();
471
472 assert!(planner
473 .plan_avoid_self_collision(using_joint_names.as_slice(), &[0.0], &[0.0],)
474 .is_ok());
475 assert!(planner
477 .plan_avoid_self_collision(using_joint_names.as_slice(), &[1.57], &[0.0],)
478 .is_err());
479 assert!(planner
481 .plan_avoid_self_collision(using_joint_names.as_slice(), &[0.0], &[1.57],)
482 .is_err());
483 }
484
485 #[flaky_test::flaky_test]
489 fn test_plan_avoid_self_collision() {
490 let urdf_path = Path::new("sample.urdf");
491 let robot = k::Chain::from_urdf_file(urdf_path).unwrap();
492
493 k::SerialChain::from_end(robot.find("r_tool_fixed").unwrap())
495 .set_joint_positions_clamped(&[0.9, 0.0, 0.0, 0.0, 0.67, 0.0]);
496
497 let planner = create_joint_path_planner(
498 urdf_path,
499 create_all_collision_pairs(&robot),
500 &JointPathPlannerConfig::default(),
501 Arc::new(robot),
502 );
503
504 let l_tool_node = planner
505 .collision_check_robot()
506 .find("l_tool_fixed")
507 .unwrap();
508 let using_joints = k::SerialChain::from_end(l_tool_node);
509 let using_joint_names = using_joints
510 .iter_joints()
511 .map(|j| j.name.to_owned())
512 .collect::<Vec<String>>();
513
514 assert!(planner
516 .plan_avoid_self_collision(using_joint_names.as_slice(), &[0.0; 6], &[0.0; 6],)
517 .is_err());
518 assert!(planner
520 .plan_avoid_self_collision(
521 using_joint_names.as_slice(),
522 &[0.0, -0.3, 0.0, 0.0, 0.0, 0.0],
523 &[0.0, 0.3, 0.0, 0.0, 0.0, 0.0],
524 )
525 .is_ok());
526 }
527}