From c77d9106a60cbc628f68768c93d817715e55effa Mon Sep 17 00:00:00 2001 From: v4hn Date: Wed, 1 Mar 2017 15:19:30 +0100 Subject: [PATCH] introduce more interfaces / Gripper forward planning now works --- CMakeLists.txt | 9 ++- include/moveit_task_constructor/storage.h | 23 ++++-- include/moveit_task_constructor/subtask.h | 17 +++-- .../subtasks/gripper.h | 39 ++++++++++ src/subtask.cpp | 72 +++++++++++++++---- src/subtasks/current_state.cpp | 5 +- src/subtasks/gripper.cpp | 66 +++++++++++++++++ src/task.cpp | 4 +- src/test/test_plan_gripper.cpp | 35 +++++++++ 9 files changed, 240 insertions(+), 30 deletions(-) create mode 100644 include/moveit_task_constructor/subtasks/gripper.h create mode 100644 src/subtasks/gripper.cpp create mode 100644 src/test/test_plan_gripper.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e6c2fa4..4b7bff41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp moveit_core moveit_ros_planning + moveit_ros_planning_interface moveit_msgs ) @@ -23,6 +24,7 @@ add_compile_options(-std=c++14) add_library(${PROJECT_NAME}_subtasks src/subtasks/move.cpp src/subtasks/current_state.cpp + src/subtasks/gripper.cpp ) target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES}) @@ -31,8 +33,11 @@ add_library(${PROJECT_NAME} src/task.cpp ) +add_executable(plan_pick src/plan_pick.cpp) +target_link_libraries(plan_pick ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) + add_executable(test_plan_current_state src/test/test_plan_current_state.cpp) target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) -add_executable(plan_pick src/plan_pick.cpp) -target_link_libraries(plan_pick ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) +add_executable(test_plan_gripper src/test/test_plan_gripper.cpp) +target_link_libraries(test_plan_gripper ${PROJECT_NAME}_subtasks ${PROJECT_NAME}) diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h index 0233e66a..92edc3ed 100644 --- a/include/moveit_task_constructor/storage.h +++ b/include/moveit_task_constructor/storage.h @@ -5,6 +5,7 @@ #include #include +#include namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); @@ -19,12 +20,6 @@ namespace moveit::task_constructor { MOVEIT_CLASS_FORWARD(SubTrajectory); MOVEIT_CLASS_FORWARD(InterfaceState); -struct SubTrajectory { - robot_trajectory::RobotTrajectoryPtr trajectory; - std::vector begin; - std::vector end; -}; - struct InterfaceState { InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next) : state(ps), @@ -38,4 +33,20 @@ struct InterfaceState { planning_scene::PlanningSceneConstPtr state; }; +struct SubTrajectory { + SubTrajectory(robot_trajectory::RobotTrajectoryPtr traj) + : trajectory(traj) + {} + + void connectToBeginning(InterfaceState& state){ + begin.push_back(&state); + assert(state.next_trajectory == NULL); + state.next_trajectory= this; + } + + robot_trajectory::RobotTrajectoryPtr trajectory; + std::vector begin; + std::vector end; +}; + } diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h index 7b78a912..16cf917c 100644 --- a/include/moveit_task_constructor/subtask.h +++ b/include/moveit_task_constructor/subtask.h @@ -9,6 +9,7 @@ #include #include +#include namespace planning_scene { MOVEIT_CLASS_FORWARD(PlanningScene); @@ -36,12 +37,15 @@ public: void addSuccessor(SubTaskPtr); protected: - void sendForward(); - void sendBackward(); - void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningSceneConstPtr&); + InterfaceState& fetchStateBeginning(); + InterfaceState& fetchStateEnding(); + std::pair fetchStatePair(); - void connectBegin(InterfaceState&, SubTrajectory*); - void connectEnd(InterfaceState&, SubTrajectory*); + SubTrajectory& addTrajectory(robot_trajectory::RobotTrajectoryPtr); + + void sendForward(SubTrajectory&, planning_scene::PlanningSceneConstPtr); + void sendBackward(SubTrajectory&, planning_scene::PlanningSceneConstPtr); + void sendBothWays(SubTrajectory&, planning_scene::PlanningSceneConstPtr); InterfaceState* newBegin(planning_scene::PlanningSceneConstPtr, SubTrajectory*); InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*); @@ -58,6 +62,9 @@ protected: std::list endings_; planning_scene::PlanningSceneConstPtr scene_; + + std::list::iterator it_beginnings_; + std::list::iterator it_endings_; }; } diff --git a/include/moveit_task_constructor/subtasks/gripper.h b/include/moveit_task_constructor/subtasks/gripper.h new file mode 100644 index 00000000..dc45c870 --- /dev/null +++ b/include/moveit_task_constructor/subtasks/gripper.h @@ -0,0 +1,39 @@ +// copyright Michael 'v4hn' Goerner @ 2017 + +#pragma once + +#include + +namespace moveit::planning_interface { +MOVEIT_CLASS_FORWARD(MoveGroupInterface); +} + +namespace moveit::task_constructor::subtasks { + +class Gripper : public SubTask { +public: + Gripper(std::string name); + + virtual bool canCompute(); + + virtual bool compute(); + + void setGroup(std::string group); + + void setFrom(std::string named_target); + void setTo(std::string named_target); + + void graspObject(std::string object); + +protected: + std::string group_; + + std::string from_named_target_; + std::string to_named_target_; + + std::string object_; + + moveit::planning_interface::MoveGroupInterfacePtr mgi_; +}; + +} diff --git a/src/subtask.cpp b/src/subtask.cpp index ee6fa66c..f28df973 100644 --- a/src/subtask.cpp +++ b/src/subtask.cpp @@ -1,7 +1,9 @@ #include moveit::task_constructor::SubTask::SubTask(std::string name) - : name_(name) + : name_(name), + it_beginnings_(beginnings_.begin()), + it_endings_(endings_.begin()) {}; void moveit::task_constructor::SubTask::addPredecessor(SubTaskPtr prev_task){ @@ -37,29 +39,71 @@ moveit::task_constructor::SubTask::setPlanningScene(planning_scene::PlanningScen scene_= scene; } +moveit::task_constructor::InterfaceState& +moveit::task_constructor::SubTask::fetchStateBeginning(){ + if(it_beginnings_ == beginnings_.end()) + throw std::runtime_error("now new state for beginning available"); + + moveit::task_constructor::InterfaceState& state= *it_beginnings_; + ++it_beginnings_; + + return state; +} + +moveit::task_constructor::SubTrajectory& +moveit::task_constructor::SubTask::addTrajectory(robot_trajectory::RobotTrajectoryPtr trajectory){ + trajectories_.emplace_back(trajectory); + return trajectories_.back(); +} + void -moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajectoryPtr& traj, planning_scene::PlanningSceneConstPtr& ps){ - trajectories_.emplace_back(); - SubTrajectory& subtraj= trajectories_.back(); - subtraj.trajectory= traj; - - subtraj.begin.reserve(predecessors_.size()); - for( SubTask* pred : predecessors_ ) - subtraj.begin.push_back( pred->newEnd(ps, &subtraj) ); - - subtraj.end.resize(successors_.size()); +moveit::task_constructor::SubTask::sendForward(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){ + std::cout << "sending state forward to " << successors_.size() << " successors" << std::endl; + traj.end.reserve(successors_.size()); for( SubTaskPtr succ : successors_ ) - subtraj.end.push_back( succ->newBegin(ps, &subtraj) ); + traj.end.push_back( succ->newBegin(ps, &traj) ); +} + +void +moveit::task_constructor::SubTask::sendBackward(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){ + std::cout << "sending state backward to " << predecessors_.size() << " successors" << std::endl; + traj.begin.reserve(successors_.size()); + for( SubTask* pred : predecessors_ ) + traj.begin.push_back( pred->newEnd(ps, &traj) ); +} + +void +moveit::task_constructor::SubTask::sendBothWays(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){ + std::cout << "sending state both ways" << std::endl; + if( predecessors_.size() > 0 ) + sendBackward(traj, ps); + if( successors_.size() > 0 ) + sendForward(traj, ps); } moveit::task_constructor::InterfaceState* moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){ - beginnings_.push_back( InterfaceState(ps, old_end, NULL)); + assert( bool(ps) ); + + beginnings_.push_back( InterfaceState(ps, old_end, NULL) ); + + // we just appended a state to the list, but the iterator doesn't see it anymore + // so let's point it at the new one + if( it_beginnings_ == beginnings_.end() ) + --it_beginnings_; + return &beginnings_.back(); } moveit::task_constructor::InterfaceState* moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){ - endings_.push_back( InterfaceState(ps, NULL, old_beginning)); + assert( bool(ps) ); + endings_.push_back( InterfaceState(ps, NULL, old_beginning) ); + + // we just appended a state to the list, but the iterator doesn't see it anymore + // so let's point it at the new one + if( it_endings_ == endings_.end() ) + --it_endings_; + return &endings_.back(); } diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp index 4a46c520..6eab547b 100644 --- a/src/subtasks/current_state.cpp +++ b/src/subtasks/current_state.cpp @@ -15,10 +15,13 @@ bool moveit::task_constructor::subtasks::CurrentState::compute(){ ran_= true; + assert( scene_ ); + // empty trajectory ref -> this node only produces states robot_trajectory::RobotTrajectoryPtr traj; + moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(traj); - sendBothWays(traj, scene_); + sendBothWays(trajectory, scene_); return true; } diff --git a/src/subtasks/gripper.cpp b/src/subtasks/gripper.cpp new file mode 100644 index 00000000..d2f318a7 --- /dev/null +++ b/src/subtasks/gripper.cpp @@ -0,0 +1,66 @@ +#include + +#include +#include + +#include + +moveit::task_constructor::subtasks::Gripper::Gripper(std::string name) +: moveit::task_constructor::SubTask::SubTask(name) +{} + +void +moveit::task_constructor::subtasks::Gripper::setGroup(std::string group){ + group_= group; + mgi_.reset(new moveit::planning_interface::MoveGroupInterface(group)); +} + +void +moveit::task_constructor::subtasks::Gripper::setFrom(std::string named_target){ + from_named_target_= named_target; +} + +void +moveit::task_constructor::subtasks::Gripper::setTo(std::string named_target){ + to_named_target_= named_target; +} + +bool +moveit::task_constructor::subtasks::Gripper::canCompute(){ + return true; +} + +bool +moveit::task_constructor::subtasks::Gripper::compute(){ + InterfaceState& start= fetchStateBeginning(); + + moveit_msgs::RobotState state; + moveit::core::robotStateToRobotStateMsg(start.state->getCurrentState(), state); + mgi_->setStartState(state); + + mgi_->setNamedTarget(to_named_target_); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + + if(!mgi_->plan(plan)) + return false; + + // construct trajectory + auto traj= std::make_shared(scene_->getRobotModel(), group_); + traj->setRobotTrajectoryMsg(start.state->getCurrentState(), plan.trajectory_); + + // construct end state + robot_state::RobotState end_state(start.state->getCurrentState()); + end_state.setVariablePositions( + plan.trajectory_.joint_trajectory.joint_names, + plan.trajectory_.joint_trajectory.points.back().positions); + planning_scene::PlanningScenePtr end_scene(start.state->diff()); + end_scene->setCurrentState(end_state); + + // finish subtask + moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(traj); + trajectory.connectToBeginning(start); + sendForward(trajectory, end_scene); + + return true; +} diff --git a/src/task.cpp b/src/task.cpp index a7e97714..55d4ede1 100644 --- a/src/task.cpp +++ b/src/task.cpp @@ -35,8 +35,8 @@ moveit::task_constructor::Task::Task() throw Exception("Task failed to aquire current PlanningScene"); } - planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(rml_->getModel())); - ps->setPlanningSceneMsg(res.scene); + scene_.reset(new planning_scene::PlanningScene(rml_->getModel())); + scene_->setPlanningSceneMsg(res.scene); } void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){ diff --git a/src/test/test_plan_gripper.cpp b/src/test/test_plan_gripper.cpp new file mode 100644 index 00000000..33e8d74d --- /dev/null +++ b/src/test/test_plan_gripper.cpp @@ -0,0 +1,35 @@ +#include + +#include + +#include +#include + +using namespace moveit::task_constructor; + +int main(int argc, char** argv){ + ros::init(argc, argv, "test_plan_gripper"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + Task t; + + t.addStart( std::make_shared("current state") ); + { + auto gripper= std::make_shared("close gripper"); + gripper->setGroup("gripper"); + gripper->setTo("closed"); + t.addAfter( gripper ); + } + t.plan(); + + + { + auto gripper= std::make_shared("close gripper"); + gripper->setGroup("gripper"); + gripper->setTo("closed"); + t.addStart( gripper ); + } + t.addAfter( std::make_shared("current state") ); + return 0; +}