#![allow(clippy::trivially_copy_pass_by_ref)]
use k::nalgebra as na;
use na::RealField;
use ncollide3d::shape::Compound;
use super::joint_path_planner::JointPathPlanner;
use crate::errors::*;
pub struct JointPathPlannerWithIk<T, I>
where
I: k::InverseKinematicsSolver<T>,
T: RealField + Copy + k::SubsetOf<f64>,
{
pub path_planner: JointPathPlanner<T>,
pub ik_solver: I,
}
impl<T, I> JointPathPlannerWithIk<T, I>
where
T: RealField + k::SubsetOf<f64> + num_traits::Float,
I: k::InverseKinematicsSolver<T>,
{
pub fn new(path_planner: JointPathPlanner<T>, ik_solver: I) -> Self {
Self {
path_planner,
ik_solver,
}
}
pub fn solve_ik(
&mut self,
arm: &k::SerialChain<T>,
target_pose: &na::Isometry3<T>,
) -> Result<()> {
Ok(self.ik_solver.solve(arm, target_pose)?)
}
pub fn solve_ik_with_constraints(
&mut self,
arm: &k::SerialChain<T>,
target_pose: &na::Isometry3<T>,
c: &k::Constraints,
) -> Result<()> {
Ok(self.ik_solver.solve_with_constraints(arm, target_pose, c)?)
}
pub fn colliding_link_names(&self, objects: &Compound<T>) -> Vec<String> {
self.path_planner.env_collision_link_names(objects)
}
pub fn plan_with_ik(
&mut self,
target_name: &str,
target_pose: &na::Isometry3<T>,
objects: &Compound<T>,
) -> Result<Vec<Vec<T>>> {
self.plan_with_ik_with_constraints(
target_name,
target_pose,
objects,
&k::Constraints::default(),
)
}
pub fn plan_with_ik_with_constraints(
&mut self,
target_name: &str,
target_pose: &na::Isometry3<T>,
objects: &Compound<T>,
constraints: &k::Constraints,
) -> Result<Vec<Vec<T>>> {
self.path_planner.sync_joint_positions_with_reference();
let end_link = self
.path_planner
.collision_check_robot()
.find(target_name)
.ok_or_else(|| Error::NotFound(target_name.to_owned()))?;
let arm = k::SerialChain::from_end(end_link);
let initial = arm.joint_positions();
let using_joint_names = arm
.iter_joints()
.map(|j| j.name.clone())
.collect::<Vec<String>>();
self.ik_solver
.solve_with_constraints(&arm, target_pose, constraints)?;
let goal = arm.joint_positions();
self.path_planner
.plan(using_joint_names.as_slice(), &initial, &goal, objects)
}
pub fn plan_joints<K>(
&mut self,
using_joint_names: &[String],
start_angles: &[T],
goal_angles: &[T],
objects: &Compound<T>,
) -> Result<Vec<Vec<T>>> {
self.path_planner
.plan(using_joint_names, start_angles, goal_angles, objects)
}
pub fn update_transforms(&self) -> Vec<na::Isometry3<T>> {
self.path_planner.update_transforms()
}
pub fn joint_names(&self) -> Vec<String> {
self.path_planner.joint_names()
}
}