diff --git a/CMakeLists.txt b/CMakeLists.txt index f019d117..595c0ad7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,28 @@ find_package(catkin REQUIRED) find_package(moveit_core REQUIRED) catkin_package( + INCLUDE_DIRS include ) -add_executable(test_plan_pick src/test_plan_pick.cpp) +include_directories( + include + ${moveit_core_INCLUDE_DIRS} +) + +add_compile_options(-std=c++14) + +add_library(${PROJECT_NAME}_subtasks + src/subtasks/move.cpp + src/subtasks/current_state.cpp +) + +add_library(${PROJECT_NAME} + src/subtask.cpp + src/task.cpp +) + +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}) diff --git a/include/moveit_task_constructor/storage.h b/include/moveit_task_constructor/storage.h new file mode 100644 index 00000000..1aa4532f --- /dev/null +++ b/include/moveit_task_constructor/storage.h @@ -0,0 +1,24 @@ +// copyright Michael 'v4hn' Goerner @ 2017 + +#pragma once + +#include + +namespace moveit::task_constructor { + +MOVEIT_CLASS_FORWARD(SubTrajectory); +MOVEIT_CLASS_FORWARD(InterfaceState); + +struct SubTrajectory { + robot_trajectory::RobotTrajectoryPtr trajectory; + std::vector next; + std::vector prev; +}; + +struct InterfaceState { + SubTrajectory* connection; + + planning_scene::PlanningSceneConstPtr state; +}; + +} diff --git a/include/moveit_task_constructor/subtask.h b/include/moveit_task_constructor/subtask.h new file mode 100644 index 00000000..cb671fc4 --- /dev/null +++ b/include/moveit_task_constructor/subtask.h @@ -0,0 +1,45 @@ +// copyright Michael 'v4hn' Goerner @ 2017 + +#pragma once + +#include + +#include +#include + +#include +#include + +namespace moveit::task_constructor { + + +MOVEIT_CLASS_FORWARD(SubTask); + +class SubTask { +public: + SubTask(std::string name); + + virtual bool canCompute() = 0; + virtual bool compute() = 0; + + const std::vector& getOut(); + const std::vector& getIn(); + const std::list& getTrajectories(); + + void addPredecessor(SubTaskConstPtr); + void addSuccessor(SubTaskConstPtr); + +protected: + const std::string name_; + + // maintain raw pointers to predecessors to avoid pointer circles + std::vector predecessors_; + std::vector successors_; + + std::list trajectories_; + + std::shared_ptr< std::vector > ins_; + std::shared_ptr< std::vector > outs_; +}; + +} diff --git a/include/moveit_task_constructor/subtasks/current_state.h b/include/moveit_task_constructor/subtasks/current_state.h new file mode 100644 index 00000000..10d3a072 --- /dev/null +++ b/include/moveit_task_constructor/subtasks/current_state.h @@ -0,0 +1,18 @@ +// copyright Michael 'v4hn' Goerner @ 2017 + +#pragma once + +#include + +namespace moveit::task_constructor::subtasks { + +class CurrentState : public SubTask { +public: + using SubTask::SubTask; + + virtual bool canCompute(); + + virtual bool compute(); +}; + +} diff --git a/include/moveit_task_constructor/task.h b/include/moveit_task_constructor/task.h new file mode 100644 index 00000000..ce1bdbdf --- /dev/null +++ b/include/moveit_task_constructor/task.h @@ -0,0 +1,27 @@ +// copyright Michael 'v4hn' Goerner @ 2017 + +#pragma once + +#include + +#include + +namespace moveit::task_constructor { + +MOVEIT_CLASS_FORWARD(SubTask); + +MOVEIT_CLASS_FORWARD(Task); +class Task { +public: + Task(); + + void addStart( SubTaskPtr ); + void addAfter( SubTaskPtr ); + + bool plan(); + +protected: + std::vector subtasks_; +}; + +} diff --git a/src/plan_pick.cpp b/src/plan_pick.cpp new file mode 100644 index 00000000..8314ac70 --- /dev/null +++ b/src/plan_pick.cpp @@ -0,0 +1,59 @@ +#include +#include + +#include + +using namespace moveit::task_constructor; + +int main(){ +/* + Task t; + + t.addStart( std::make_shared("current state") ); + + { + auto move= std::make_shared("open gripper"); + move->setGroup("gripper"); + move->setTo("open"); + t.addAfter(move); + } + + { + auto move= std::make_shared("move to pre-grasp"); + move->setGroup("arm"); + move->setPlannerId("RRTConnectkConfigDefault"); + move->setPlanningTime(5.0); + t.addAfter(move); + } + + { + auto move= std::make_shared("proceed to grasp pose"); + move->minMaxDistance(3.0, 10.0); + move->towards("object", 0.0, 0.0, 0.0); + t.addAfter(move); + } + + { + auto grasps= std::make_shared("generate grasp pose"); + } + + { + auto move= std::make_shared("grasp"); + move->setGroup("gripper"); + move->setTo("closed"); + move->graspObject("object"); + t.addAfter(move); + } + + { + auto move= std::make_shared("Lift Object"); + move->setGroup("arm"); + move->minMaxDistance(3.0, 5.0); + move->along("world", 0.0, 0.0, 1.0); + t.addAfter(move); + } + + t.plan(); +*/ + return 0; +} diff --git a/src/subtask.cpp b/src/subtask.cpp new file mode 100644 index 00000000..392325d2 --- /dev/null +++ b/src/subtask.cpp @@ -0,0 +1,30 @@ +#include + +moveit::task_constructor::SubTask::SubTask(std::string name) + : name_(name), + ins_(new std::vector), + outs_(new std::vector) +{}; + +void moveit::task_constructor::SubTask::addPredecessor(SubTaskConstPtr prev_task){ + predecessors_.push_back( prev_task.get() ); +} + +void moveit::task_constructor::SubTask::addSuccessor(SubTaskConstPtr next_task){ + successors_.push_back( next_task ); +} + +const std::vector& +moveit::task_constructor::SubTask::getIn(){ + return *ins_; +} + +const std::vector& +moveit::task_constructor::SubTask::getOut(){ + return *outs_; +} + +const std::list& +moveit::task_constructor::SubTask::getTrajectories(){ + return trajectories_; +} diff --git a/src/subtasks/current_state.cpp b/src/subtasks/current_state.cpp new file mode 100644 index 00000000..b20bd36b --- /dev/null +++ b/src/subtasks/current_state.cpp @@ -0,0 +1,11 @@ +#include + +bool +moveit::task_constructor::subtasks::CurrentState::canCompute(){ + return true; +} + +bool +moveit::task_constructor::subtasks::CurrentState::compute(){ + return false; +} diff --git a/src/subtasks/move.cpp b/src/subtasks/move.cpp new file mode 100644 index 00000000..e69de29b diff --git a/src/task.cpp b/src/task.cpp new file mode 100644 index 00000000..6ba84a51 --- /dev/null +++ b/src/task.cpp @@ -0,0 +1,20 @@ +#include +#include + +moveit::task_constructor::Task::Task() +{} + +void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){ + subtasks_.clear(); + subtasks_.push_back( subtask ); +} + +void moveit::task_constructor::Task::addAfter( SubTaskPtr subtask ){ + subtask->addPredecessor( subtasks_.back() ); + subtasks_.back()->addSuccessor( subtask ); + subtasks_.push_back( subtask ); +} + +bool moveit::task_constructor::Task::plan(){ + return false; +} diff --git a/src/test/test_plan_current_state.cpp b/src/test/test_plan_current_state.cpp new file mode 100644 index 00000000..962f0a54 --- /dev/null +++ b/src/test/test_plan_current_state.cpp @@ -0,0 +1,17 @@ +#include +#include + +#include + +using namespace moveit::task_constructor; + +int main(){ + + Task t; + + t.addStart( std::make_shared("current state") ); + + t.plan(); + + return 0; +} diff --git a/src/test_plan_pick.cpp b/src/test_plan_pick.cpp deleted file mode 100644 index ade45485..00000000 --- a/src/test_plan_pick.cpp +++ /dev/null @@ -1,3 +0,0 @@ -int main(){ - return 0; -}