mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
InterpolationPlanner: implement simple IK-based solver for pose targets
This commit is contained in:
parent
ef86799f27
commit
86fe752d43
@ -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
|
||||
|
@ -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");
|
||||
|
Loading…
Reference in New Issue
Block a user