mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
introduce more interfaces / Gripper forward planning now works
This commit is contained in:
parent
651fd9a946
commit
c77d9106a6
@ -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})
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -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_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
39
include/moveit_task_constructor/subtasks/gripper.h
Normal file
39
include/moveit_task_constructor/subtasks/gripper.h
Normal 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_;
|
||||
};
|
||||
|
||||
}
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -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
66
src/subtasks/gripper.cpp
Normal 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;
|
||||
}
|
||||
@ -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 ){
|
||||
|
||||
35
src/test/test_plan_gripper.cpp
Normal file
35
src/test/test_plan_gripper.cpp
Normal 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;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user