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
|
roscpp
|
||||||
moveit_core
|
moveit_core
|
||||||
moveit_ros_planning
|
moveit_ros_planning
|
||||||
|
moveit_ros_planning_interface
|
||||||
moveit_msgs
|
moveit_msgs
|
||||||
)
|
)
|
||||||
|
|
||||||
@ -23,6 +24,7 @@ add_compile_options(-std=c++14)
|
|||||||
add_library(${PROJECT_NAME}_subtasks
|
add_library(${PROJECT_NAME}_subtasks
|
||||||
src/subtasks/move.cpp
|
src/subtasks/move.cpp
|
||||||
src/subtasks/current_state.cpp
|
src/subtasks/current_state.cpp
|
||||||
|
src/subtasks/gripper.cpp
|
||||||
)
|
)
|
||||||
target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES})
|
target_link_libraries(${PROJECT_NAME}_subtasks ${catkin_LIBRARIES})
|
||||||
|
|
||||||
@ -31,8 +33,11 @@ add_library(${PROJECT_NAME}
|
|||||||
src/task.cpp
|
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)
|
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})
|
target_link_libraries(test_plan_current_state ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||||
|
|
||||||
add_executable(plan_pick src/plan_pick.cpp)
|
add_executable(test_plan_gripper src/test/test_plan_gripper.cpp)
|
||||||
target_link_libraries(plan_pick ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
target_link_libraries(test_plan_gripper ${PROJECT_NAME}_subtasks ${PROJECT_NAME})
|
||||||
|
|||||||
@ -5,6 +5,7 @@
|
|||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
#include <cassert>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
@ -19,12 +20,6 @@ namespace moveit::task_constructor {
|
|||||||
MOVEIT_CLASS_FORWARD(SubTrajectory);
|
MOVEIT_CLASS_FORWARD(SubTrajectory);
|
||||||
MOVEIT_CLASS_FORWARD(InterfaceState);
|
MOVEIT_CLASS_FORWARD(InterfaceState);
|
||||||
|
|
||||||
struct SubTrajectory {
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
|
||||||
std::vector<InterfaceState*> begin;
|
|
||||||
std::vector<InterfaceState*> end;
|
|
||||||
};
|
|
||||||
|
|
||||||
struct InterfaceState {
|
struct InterfaceState {
|
||||||
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
|
InterfaceState(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* previous, SubTrajectory* next)
|
||||||
: state(ps),
|
: state(ps),
|
||||||
@ -38,4 +33,20 @@ struct InterfaceState {
|
|||||||
planning_scene::PlanningSceneConstPtr state;
|
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 <vector>
|
||||||
#include <list>
|
#include <list>
|
||||||
|
#include <tuple>
|
||||||
|
|
||||||
namespace planning_scene {
|
namespace planning_scene {
|
||||||
MOVEIT_CLASS_FORWARD(PlanningScene);
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
@ -36,12 +37,15 @@ public:
|
|||||||
void addSuccessor(SubTaskPtr);
|
void addSuccessor(SubTaskPtr);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void sendForward();
|
InterfaceState& fetchStateBeginning();
|
||||||
void sendBackward();
|
InterfaceState& fetchStateEnding();
|
||||||
void sendBothWays(robot_trajectory::RobotTrajectoryPtr&, planning_scene::PlanningSceneConstPtr&);
|
std::pair<InterfaceState&, InterfaceState&> fetchStatePair();
|
||||||
|
|
||||||
void connectBegin(InterfaceState&, SubTrajectory*);
|
SubTrajectory& addTrajectory(robot_trajectory::RobotTrajectoryPtr);
|
||||||
void connectEnd(InterfaceState&, SubTrajectory*);
|
|
||||||
|
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* newBegin(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
||||||
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
InterfaceState* newEnd(planning_scene::PlanningSceneConstPtr, SubTrajectory*);
|
||||||
@ -58,6 +62,9 @@ protected:
|
|||||||
std::list<InterfaceState> endings_;
|
std::list<InterfaceState> endings_;
|
||||||
|
|
||||||
planning_scene::PlanningSceneConstPtr scene_;
|
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>
|
#include <moveit_task_constructor/subtask.h>
|
||||||
|
|
||||||
moveit::task_constructor::SubTask::SubTask(std::string name)
|
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){
|
void moveit::task_constructor::SubTask::addPredecessor(SubTaskPtr prev_task){
|
||||||
@ -37,29 +39,71 @@ moveit::task_constructor::SubTask::setPlanningScene(planning_scene::PlanningScen
|
|||||||
scene_= scene;
|
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
|
void
|
||||||
moveit::task_constructor::SubTask::sendBothWays(robot_trajectory::RobotTrajectoryPtr& traj, planning_scene::PlanningSceneConstPtr& ps){
|
moveit::task_constructor::SubTask::sendForward(moveit::task_constructor::SubTrajectory& traj, planning_scene::PlanningSceneConstPtr ps){
|
||||||
trajectories_.emplace_back();
|
std::cout << "sending state forward to " << successors_.size() << " successors" << std::endl;
|
||||||
SubTrajectory& subtraj= trajectories_.back();
|
traj.end.reserve(successors_.size());
|
||||||
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());
|
|
||||||
for( SubTaskPtr succ : successors_ )
|
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::InterfaceState*
|
||||||
moveit::task_constructor::SubTask::newBegin(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_end){
|
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();
|
return &beginnings_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
moveit::task_constructor::InterfaceState*
|
moveit::task_constructor::InterfaceState*
|
||||||
moveit::task_constructor::SubTask::newEnd(planning_scene::PlanningSceneConstPtr ps, SubTrajectory* old_beginning){
|
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();
|
return &endings_.back();
|
||||||
}
|
}
|
||||||
|
|||||||
@ -15,10 +15,13 @@ bool
|
|||||||
moveit::task_constructor::subtasks::CurrentState::compute(){
|
moveit::task_constructor::subtasks::CurrentState::compute(){
|
||||||
ran_= true;
|
ran_= true;
|
||||||
|
|
||||||
|
assert( scene_ );
|
||||||
|
|
||||||
// empty trajectory ref -> this node only produces states
|
// empty trajectory ref -> this node only produces states
|
||||||
robot_trajectory::RobotTrajectoryPtr traj;
|
robot_trajectory::RobotTrajectoryPtr traj;
|
||||||
|
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(traj);
|
||||||
|
|
||||||
sendBothWays(traj, scene_);
|
sendBothWays(trajectory, scene_);
|
||||||
|
|
||||||
return true;
|
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");
|
throw Exception("Task failed to aquire current PlanningScene");
|
||||||
}
|
}
|
||||||
|
|
||||||
planning_scene::PlanningScenePtr ps(new planning_scene::PlanningScene(rml_->getModel()));
|
scene_.reset(new planning_scene::PlanningScene(rml_->getModel()));
|
||||||
ps->setPlanningSceneMsg(res.scene);
|
scene_->setPlanningSceneMsg(res.scene);
|
||||||
}
|
}
|
||||||
|
|
||||||
void moveit::task_constructor::Task::addStart( SubTaskPtr subtask ){
|
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