From 0918ad68978f2b20498b0fd8f6212ac7c521d0d8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 14 Dec 2017 09:15:29 +0100 Subject: [PATCH] implement IK as separate stage ... wrapping another stage --- core/demo/plan_pick_pa10.cpp | 8 +- core/demo/plan_pick_trixi.cpp | 8 +- core/demo/plan_pick_ur5.cpp | 8 +- .../task_constructor/stages/compute_ik.h | 42 +++++ .../stages/generate_grasp_pose.h | 26 +-- .../moveit/task_constructor/stages/move.h | 4 +- core/src/stages/CMakeLists.txt | 2 + core/src/stages/compute_ik.cpp | 174 ++++++++++++++++++ core/src/stages/generate_grasp_pose.cpp | 149 +++------------ core/test/test_plan_generate_grasp_pose.cpp | 2 - 10 files changed, 270 insertions(+), 153 deletions(-) create mode 100644 core/include/moveit/task_constructor/stages/compute_ik.h create mode 100644 core/src/stages/compute_ik.cpp diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index adc1e8bf..ddc5d4db 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -82,8 +83,11 @@ int main(int argc, char** argv){ Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), "lh_tool_frame"); gengrasp->setAngleDelta(-.2); - gengrasp->setMaxIKSolutions(1); - t.add(std::move(gengrasp)); + + auto ik = std::make_unique("compute ik", std::move(gengrasp)); + ik->properties().configureInitFrom(Stage::PARENT); + ik->setMaxIKSolutions(1); + t.add(std::move(ik)); } { diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index b385799c..e5eee7ef 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include @@ -76,17 +77,20 @@ int main(int argc, char** argv){ { auto gengrasp= std::make_unique("generate grasp pose"); gengrasp->setEndEffector("left_gripper"); - //gengrasp->setGroup("arm"); gengrasp->setGripperGraspPose("open"); gengrasp->setObject("object"); gengrasp->setGraspFrame(Eigen::Translation3d(), "l_gripper_tool_frame"); gengrasp->setAngleDelta(.2); - t.add(std::move(gengrasp)); + + auto ik = std::make_unique("compute ik", std::move(gengrasp)); + ik->setEndEffector("left_gripper"); + t.add(std::move(ik)); } { auto move= std::make_unique("grasp"); move->setEndEffector("left_gripper"); + move->setAttachLink("l_gripper_tool_frame"); move->setTo("closed"); move->graspObject("object"); t.add(std::move(move)); diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index e961cf7c..9a9489ad 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include @@ -81,8 +82,11 @@ int main(int argc, char** argv){ gengrasp->setObject("object"); gengrasp->setGraspFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0"); gengrasp->setAngleDelta(-.2); - gengrasp->setMaxIKSolutions(8); - t.add(std::move(gengrasp)); + + auto ik = std::make_unique("compute ik", std::move(gengrasp)); + ik->properties().configureInitFrom(Stage::PARENT); + ik->setMaxIKSolutions(8); + t.add(std::move(ik)); } { diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h new file mode 100644 index 00000000..ff8eb3fa --- /dev/null +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -0,0 +1,42 @@ +// +// Created by llach on 24.11.17. +// +// copyright Michael 'v4hn' Goerner @ 2017 + +#pragma once + +#include +#include +#include + +namespace moveit { +namespace core { MOVEIT_CLASS_FORWARD(RobotState) } +} + +namespace moveit { namespace task_constructor { namespace stages { + +class ComputeIK : public Wrapper { +public: + ComputeIK(const std::string &name, pointer &&child = Stage::pointer()); + + void onNewSolution(const SolutionBase &s) override; + + void setTimeout(double timeout); + void setEndEffector(const std::string& eef); + + void setTargetPose(const geometry_msgs::PoseStamped &pose); + void setTargetPose(const Eigen::Affine3d& pose, const std::string& link = ""); + template + void setTargetPose(const T& p, const std::string& link = "") { + Eigen::Affine3d pose; pose = p; + setTargetPose(pose, link); + } + void setMaxIKSolutions(uint32_t n); + void setIgnoreCollisions(bool flag); + +protected: + bool tried_current_state_as_seed_ = false; + std::vector< std::vector > previous_solutions_; +}; + +} } } diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h index 88f6bf80..cf65f509 100644 --- a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h +++ b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h @@ -51,21 +51,11 @@ public: bool canCompute() const override; bool compute() override; - void setEndEffector(std::string eef); - - void setGroup(std::string group_name); - - void setGripperGraspPose(std::string pose_name); - - void setObject(std::string object); - void setTimeout(double timeout); - void setAngleDelta(double delta); - - void setMaxIKSolutions(uint32_t n); - - void setIgnoreCollisions(bool flag); + void setEndEffector(const std::string &eef); + void setGripperGraspPose(const std::string &pose_name); + void setObject(const std::string &object); void setGraspFrame(const geometry_msgs::TransformStamped &transform); void setGraspFrame(const Eigen::Affine3d& transform, const std::string& link = ""); @@ -74,17 +64,11 @@ public: Eigen::Affine3d transform; transform = t; setGraspFrame(transform, link); } + void setAngleDelta(double delta); protected: - planning_scene::PlanningSceneConstPtr scene_; - - /* temp values */ - + planning_scene::PlanningScenePtr scene_; double current_angle_ = 0.0; - - bool tried_current_state_as_seed_ = false; - - std::vector< std::vector > previous_solutions_; }; } } } diff --git a/core/include/moveit/task_constructor/stages/move.h b/core/include/moveit/task_constructor/stages/move.h index 3869ed78..2a303828 100644 --- a/core/include/moveit/task_constructor/stages/move.h +++ b/core/include/moveit/task_constructor/stages/move.h @@ -49,8 +49,8 @@ public: void init(const planning_scene::PlanningSceneConstPtr &scene); bool compute(const InterfaceState &from, const InterfaceState &to); - void setGroup(const std::__cxx11::string &group); - void setPlannerId(const std::__cxx11::string &planner); + void setGroup(const std::string &group); + void setPlannerId(const std::string &planner); void setTimeout(double timeout); protected: diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 284a3dd5..cfb89072 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(${PROJECT_NAME}_stages ${PROJECT_INCLUDE}/stages/cartesian_position_motion.h + ${PROJECT_INCLUDE}/stages/compute_ik.h ${PROJECT_INCLUDE}/stages/current_state.h ${PROJECT_INCLUDE}/stages/generate_grasp_pose.h ${PROJECT_INCLUDE}/stages/gripper.h @@ -7,6 +8,7 @@ add_library(${PROJECT_NAME}_stages cartesian_position_motion.cpp current_state.cpp + compute_ik.cpp generate_grasp_pose.cpp gripper.cpp move.cpp diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp new file mode 100644 index 00000000..f71cbafe --- /dev/null +++ b/core/src/stages/compute_ik.cpp @@ -0,0 +1,174 @@ +// +// Created by llach on 24.11.17. +// + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace stages { + +ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) + : Wrapper(name, std::move(child)) +{ + auto& p = properties(); + p.declare("timeout", 1.0); + p.declare("eef", "name of end-effector group"); + p.declare("group", "", "name of active group (derived from eef if not provided)"); + p.declare("default_pose", "", "default joint pose of active group (defines cost of IK)"); + p.declare("max_ik_solutions", 1); + p.declare("ignore_collisions", false); + + // target_pose is read from the interface + p.declare("target_pose"); + p.configureInitFrom(Stage::INTERFACE, {"target_pose"}); +} + +void ComputeIK::setTimeout(double timeout){ + setProperty("timeout", timeout); +} + +void ComputeIK::setEndEffector(const std::string &eef){ + setProperty("eef", eef); +} + +void ComputeIK::setTargetPose(const geometry_msgs::PoseStamped &pose) +{ + setProperty("target_pose", pose); +} + +void ComputeIK::setTargetPose(const Eigen::Affine3d &pose, const std::string &link) +{ + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = link; + tf::poseEigenToMsg(pose, pose_msg.pose); + setTargetPose(pose_msg); +} + + +void ComputeIK::setMaxIKSolutions(uint32_t n){ + setProperty("max_ik_solutions", n); +} + +void ComputeIK::setIgnoreCollisions(bool flag) +{ + setProperty("ignore_collisions", flag); +} + +namespace { +bool isValid(planning_scene::PlanningSceneConstPtr scene, + bool ignore_collisions, + std::vector< std::vector >* found_solutions, + robot_state::RobotState* state, + const robot_model::JointModelGroup* jmg, + const double* joint_positions){ + for(const auto& sol : *found_solutions ){ + if( jmg->distance(joint_positions, sol.data()) < 0.1 ) + return false; // to close at already found solutions + } + + state->setJointGroupPositions(jmg, joint_positions); + state->update(); + found_solutions->emplace_back(); + state->copyJointGroupPositions(jmg, found_solutions->back()); + + if (ignore_collisions) + return true; + + if (scene->isStateColliding(const_cast(*state), jmg->getName())) + return false; + + return true; +} + +} // anonymous namespace + +void ComputeIK::onNewSolution(const SolutionBase &s) +{ + assert(s.start() && s.end()); + assert(s.start()->scene() == s.end()->scene()); // wrapped child should be a generator + planning_scene::PlanningScenePtr scene = s.start()->scene()->diff(); + + // enforced initialization from interface ensures that new target_pose is read + properties().performInitFrom(INTERFACE, s.start()->properties(), true); + const auto& props = properties(); + + const std::string& eef = props.get("eef"); + assert(scene->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf"); + + robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); + const auto& robot_model = robot_state.getRobotModel(); + + const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef); + const std::string& link_name = jmg->getEndEffectorParentGroup().second; + const std::string& group = props.get("group"); + jmg = group.empty() ? robot_model->getJointModelGroup(jmg->getEndEffectorParentGroup().first) + : robot_model->getJointModelGroup(group); + + // compute target pose w.r.t. link_name + geometry_msgs::PoseStamped target_pose = props.get("target_pose"); + Eigen::Affine3d target_pose_eigen; + tf::poseMsgToEigen(target_pose.pose, target_pose_eigen); + if (!target_pose.header.frame_id.empty() && target_pose.header.frame_id != link_name) { + const Eigen::Affine3d& ref_pose = scene->getFrameTransform(props.get(target_pose.header.frame_id)); + if(ref_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + ROS_WARN("requested reference frame '%s' for target pose does not exist", target_pose.header.frame_id.c_str()); + const Eigen::Affine3d& link_pose = scene->getFrameTransform(link_name); + if(link_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) + ROS_WARN("requested link frame '%s' does not exist", link_name.c_str()); + + // transform target pose such that the link frame will reach there + target_pose_eigen = target_pose_eigen * ref_pose.inverse() * link_pose; + } + + // determine joint values of robot pose to compare IK solution with for costs + std::vector compare_pose; + const std::string &compare_pose_name = props.get("default_pose"); + if (!compare_pose_name.empty()) { + robot_state::RobotState compare_state(robot_state); + compare_state.setToDefaultValues(jmg, compare_pose_name); + compare_state.copyJointGroupPositions(jmg, compare_pose); + } else + robot_state.copyJointGroupPositions(jmg, compare_pose); + + const moveit::core::GroupStateValidityCallbackFn is_valid = + std::bind(&isValid, + scene, + props.get("ignore_collisions"), + &previous_solutions_, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3); + + uint32_t max_ik_solutions = props.get("max_ik_solutions"); + tried_current_state_as_seed_= false; + previous_solutions_.clear(); + + double remaining_time = props.get("timeout"); + auto start_time = std::chrono::steady_clock::now(); + while (previous_solutions_.size() < max_ik_solutions && remaining_time > 0) { + if(tried_current_state_as_seed_) + robot_state.setToRandomPositions(jmg); + tried_current_state_as_seed_= true; + + bool succeeded = robot_state.setFromIK(jmg, target_pose_eigen, link_name, 1, remaining_time, is_valid); + + auto now = std::chrono::steady_clock::now(); + remaining_time -= std::chrono::duration(now - start_time).count(); + start_time = now; + + if (succeeded) + spawn(InterfaceState(scene), s.cost() + jmg->distance(previous_solutions_.back().data(), compare_pose.data())); + else if (max_ik_solutions == 1) + break; // first and only attempt failed + } +} + +} } } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 9f139612..31f0d560 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -35,22 +35,11 @@ /* Authors: Michael Goerner */ #include -#include - -#include -#include #include -#include -#include -#include - -#include #include - -#include -#include +#include namespace moveit { namespace task_constructor { namespace stages { @@ -58,125 +47,74 @@ GenerateGraspPose::GenerateGraspPose(std::string name) : Generator(name) { auto& p = properties(); - p.declare("group", "name of planning group"); p.declare("eef", "name of end-effector group"); + p.declare("eef_named_pose"); p.declare("object"); - p.declare("eef_grasp_pose"); - p.declare("timeout", 0.1); - p.declare("max_ik_solutions", 1); p.declare("grasp_frame", geometry_msgs::TransformStamped(), "robot frame to use for grasping"); p.declare("angle_delta", 0.1, "angular steps (rad)"); - p.declare("ignore_collisions", false); } void GenerateGraspPose::init(const planning_scene::PlanningSceneConstPtr &scene) { Generator::init(scene); - scene_ = scene; + scene_ = scene->diff(); } -void GenerateGraspPose::setGroup(std::string group){ - setProperty("group", group); -} - -void GenerateGraspPose::setEndEffector(std::string eef){ +void GenerateGraspPose::setEndEffector(const std::string &eef){ setProperty("eef", eef); } -void GenerateGraspPose::setGripperGraspPose(std::string pose_name){ - setProperty("eef_grasp_pose", pose_name); +void GenerateGraspPose::setGripperGraspPose(const std::string &pose_name){ + setProperty("eef_named_pose", pose_name); } -void GenerateGraspPose::setObject(std::string object){ +void GenerateGraspPose::setObject(const std::string &object){ setProperty("object", object); } void GenerateGraspPose::setGraspFrame(const geometry_msgs::TransformStamped &frame){ setProperty("grasp_frame", frame); } + void GenerateGraspPose::setGraspFrame(const Eigen::Affine3d &transform, const std::string &link) { geometry_msgs::TransformStamped frame; frame.header.frame_id = link; + frame.child_frame_id = "grasp_frame"; tf::transformEigenToMsg(transform, frame.transform); setGraspFrame(frame); } -void GenerateGraspPose::setTimeout(double timeout){ - setProperty("timeout", timeout); -} - void GenerateGraspPose::setAngleDelta(double delta){ setProperty("angle_delta", delta); } -void GenerateGraspPose::setMaxIKSolutions(uint32_t n){ - setProperty("max_ik_solutions", n); -} - -void GenerateGraspPose::setIgnoreCollisions(bool flag) -{ - setProperty("ignore_collisions", flag); -} - bool GenerateGraspPose::canCompute() const{ return current_angle_ < 2*M_PI && current_angle_ > -2*M_PI; } -namespace { - bool isValid(planning_scene::PlanningSceneConstPtr scene, - bool ignore_collisions, - std::vector< std::vector >* old_solutions, - robot_state::RobotState* state, - const robot_model::JointModelGroup* jmg, - const double* joint_positions){ - for( std::vector sol : *old_solutions ){ - if( jmg->distance(joint_positions, sol.data()) < 0.1 ){ - return false; - } - } - - if(ignore_collisions) - return true; - - state->setJointGroupPositions(jmg, joint_positions); - state->update(); - if( scene->isStateColliding(const_cast(*state), jmg->getName()) ){ - old_solutions->emplace_back(); - state->copyJointGroupPositions(jmg, old_solutions->back()); - return false; - } - return true; - } -} - bool GenerateGraspPose::compute(){ const auto& props = properties(); - double remaining_time = props.get("timeout"); - const std::string& eef = props.get("eef"); - const std::string& group = props.get("group"); assert(scene_->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf"); + const moveit::core::JointModelGroup* jmg = scene_->getRobotModel()->getEndEffector(eef); - planning_scene::PlanningScenePtr grasp_scene = scene_->diff(); - robot_state::RobotState &grasp_state = grasp_scene->getCurrentStateNonConst(); - - const moveit::core::JointModelGroup* jmg_eef= grasp_state.getRobotModel()->getEndEffector(eef); - - const moveit::core::JointModelGroup* jmg_active= group.empty() - ? grasp_state.getJointModelGroup(jmg_eef->getEndEffectorParentGroup().first) - : grasp_state.getJointModelGroup(group); + robot_state::RobotState &robot_state = scene_->getCurrentStateNonConst(); + const std::string& eef_named_pose = props.get("eef_named_pose"); + if(!eef_named_pose.empty()){ + robot_state.setToDefaultValues(jmg , eef_named_pose); + } geometry_msgs::TransformStamped grasp_frame = props.get("grasp_frame"); - const std::string &link_name = jmg_eef->getEndEffectorParentGroup().second; - if (grasp_frame.header.frame_id.empty()) - grasp_frame.header.frame_id = link_name; + const std::string &link_name = jmg ->getEndEffectorParentGroup().second; + if (grasp_frame.header.frame_id.empty()) // if no frame_id is given + grasp_frame.header.frame_id = link_name; // interpret the transform w.r.t. eef link frame Eigen::Affine3d grasp_pose; tf::transformMsgToEigen(grasp_frame.transform, grasp_pose); if (grasp_frame.header.frame_id != link_name) { - // convert grasp_pose to transform relative to link (instead of frame_id) + // convert grasp_pose into transform w.r.t. link (instead of frame_id) const Eigen::Affine3d link_pose = scene_->getFrameTransform(link_name); if(link_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) throw std::runtime_error("requested link does not exist or could not be retrieved"); @@ -187,56 +125,23 @@ bool GenerateGraspPose::compute(){ } grasp_pose = grasp_pose.inverse(); // invert once - const std::string& eef_grasp_pose = props.get("eef_grasp_pose"); - if(!eef_grasp_pose.empty()){ - grasp_state.setToDefaultValues(jmg_eef, eef_grasp_pose); - } - - const moveit::core::GroupStateValidityCallbackFn is_valid= - std::bind( - &isValid, - scene_, - props.get("ignore_collisions"), - &previous_solutions_, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3); - const Eigen::Affine3d object_pose = scene_->getFrameTransform(props.get("object")); if(object_pose.matrix().cwiseEqual(Eigen::Affine3d::Identity().matrix()).all()) throw std::runtime_error("requested object does not exist or could not be retrieved"); - uint32_t max_ik_solutions = props.get("max_ik_solutions"); while( canCompute() ){ - if( remaining_time <= 0.0 || (max_ik_solutions != 0 && previous_solutions_.size() >= max_ik_solutions)){ - std::cout << "computed angle " << current_angle_ - << " with " << previous_solutions_.size() - << " cached ik solutions" - << " and " << remaining_time << "s left" << std::endl; - current_angle_+= props.get("angle_delta"); - remaining_time = props.get("timeout"); - tried_current_state_as_seed_= false; - previous_solutions_.clear(); - continue; - } - // rotate object pose about z-axis Eigen::Affine3d goal_pose = object_pose * Eigen::AngleAxisd(current_angle_, Eigen::Vector3d::UnitZ()) * grasp_pose; + current_angle_ += props.get("angle_delta"); - if(tried_current_state_as_seed_) - grasp_state.setToRandomPositions(jmg_active); - tried_current_state_as_seed_= true; + InterfaceState state(scene_); + geometry_msgs::PoseStamped goal_pose_msg; + goal_pose_msg.header.frame_id = link_name; + tf::poseEigenToMsg(goal_pose, goal_pose_msg.pose); + state.properties().set("target_pose", goal_pose_msg); - auto now= std::chrono::steady_clock::now(); - bool succeeded= grasp_state.setFromIK(jmg_active, goal_pose, link_name, 1, remaining_time, is_valid); - remaining_time-= std::chrono::duration(std::chrono::steady_clock::now()- now).count(); - - if(succeeded) { - previous_solutions_.emplace_back(); - grasp_state.copyJointGroupPositions(jmg_active, previous_solutions_.back()); - spawn(InterfaceState(grasp_scene)); - return true; - } + spawn(std::move(state)); + return true; } return false; diff --git a/core/test/test_plan_generate_grasp_pose.cpp b/core/test/test_plan_generate_grasp_pose.cpp index 069a7f60..236729d9 100644 --- a/core/test/test_plan_generate_grasp_pose.cpp +++ b/core/test/test_plan_generate_grasp_pose.cpp @@ -39,9 +39,7 @@ int main(int argc, char** argv){ auto st= std::make_unique("generate grasp candidates"); st->setEndEffector("gripper"); - //st->setGroup("arm"); st->setObject("object"); - st->setTimeout(0.5); st->setAngleDelta(0.1); st->setGraspFrame(Eigen::Translation3d(.03,0,0));