introduce more interfaces / Gripper forward planning now works

This commit is contained in:
v4hn 2017-03-01 15:19:30 +01:00
parent 651fd9a946
commit c77d9106a6
9 changed files with 240 additions and 30 deletions

View File

@ -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})

View File

@ -5,6 +5,7 @@
#include <moveit/macros/class_forward.h>
#include <vector>
#include <cassert>
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<InterfaceState*> begin;
std::vector<InterfaceState*> 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<InterfaceState*> begin;
std::vector<InterfaceState*> end;
};
}

View File

@ -9,6 +9,7 @@
#include <vector>
#include <list>
#include <tuple>
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<InterfaceState&, InterfaceState&> 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<InterfaceState> endings_;
planning_scene::PlanningSceneConstPtr scene_;
std::list<InterfaceState>::iterator it_beginnings_;
std::list<InterfaceState>::iterator it_endings_;
};
}

View File

@ -0,0 +1,39 @@
// copyright Michael 'v4hn' Goerner @ 2017
#pragma once
#include <moveit_task_constructor/subtask.h>
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_;
};
}

View File

@ -1,7 +1,9 @@
#include <moveit_task_constructor/subtask.h>
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();
}

View File

@ -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;
}

66
src/subtasks/gripper.cpp Normal file
View File

@ -0,0 +1,66 @@
#include <moveit_task_constructor/subtasks/gripper.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/move_group_interface/move_group_interface.h>
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<robot_trajectory::RobotTrajectory>(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;
}

View File

@ -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 ){

View File

@ -0,0 +1,35 @@
#include <ros/ros.h>
#include <moveit_task_constructor/task.h>
#include <moveit_task_constructor/subtasks/current_state.h>
#include <moveit_task_constructor/subtasks/gripper.h>
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<subtasks::CurrentState>("current state") );
{
auto gripper= std::make_shared<subtasks::Gripper>("close gripper");
gripper->setGroup("gripper");
gripper->setTo("closed");
t.addAfter( gripper );
}
t.plan();
{
auto gripper= std::make_shared<subtasks::Gripper>("close gripper");
gripper->setGroup("gripper");
gripper->setTo("closed");
t.addStart( gripper );
}
t.addAfter( std::make_shared<subtasks::CurrentState>("current state") );
return 0;
}