InterpolationPlanner: implement simple IK-based solver for pose targets

This commit is contained in:
v4hn 2021-10-05 09:52:51 +02:00
parent ef86799f27
commit 86fe752d43
2 changed files with 38 additions and 10 deletions

View File

@ -40,6 +40,8 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <chrono>
namespace moveit {
namespace task_constructor {
namespace solvers {
@ -53,9 +55,9 @@ void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_mod
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const moveit_msgs::Constraints& /*path_constraints*/) {
const auto& props = properties();
// Get maximum joint distance
@ -95,12 +97,38 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
return true;
}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& /*from*/,
const moveit::core::LinkModel& /*link*/, const Eigen::Isometry3d& /*target_eigen*/,
const moveit::core::JointModelGroup* /*jmg*/, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& /*result*/,
const moveit_msgs::Constraints& /*path_constraints*/) {
throw std::runtime_error("Not yet implemented");
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now();
auto to{ from->diff() };
kinematic_constraints::KinematicConstraintSet constraints{ to->getRobotModel() };
constraints.add(path_constraints, from->getTransforms());
auto is_valid{ [&constraints, &to](moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* jmg,
const double* joint_values) -> bool {
robot_state->setJointGroupPositions(jmg, joint_values);
robot_state->update();
return to->isStateValid(*robot_state, constraints, jmg->getName());
} };
if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
// TODO(v4hn): planners need a way to add feedback to failing plans
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");
return false;
}
to->getCurrentStateNonConst().update();
timeout = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
if (timeout <= 0.0)
return false;
return plan(from, to, jmg, timeout, result, path_constraints);
}
} // namespace solvers
} // namespace task_constructor

View File

@ -70,7 +70,7 @@ TEST_F(PandaMoveTo, stateTarget) {
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, DISABLED_pointTarget) {
TEST_F(PandaMoveTo, pointTarget) {
move_to->setGoal([this]() {
RobotState state{ scene->getCurrentState() };
state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");
@ -83,7 +83,7 @@ TEST_F(PandaMoveTo, DISABLED_pointTarget) {
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, DISABLED_poseTarget) {
TEST_F(PandaMoveTo, poseTarget) {
move_to->setGoal([this]() {
RobotState state{ scene->getCurrentState() };
state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");