mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
first implementation of the Move subtask
This commit is contained in:
parent
a9c9a61710
commit
ca6151cacb
44
include/moveit_task_constructor/subtasks/move.h
Normal file
44
include/moveit_task_constructor/subtasks/move.h
Normal file
@ -0,0 +1,44 @@
|
||||
// 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 Move : public SubTask {
|
||||
public:
|
||||
Move(std::string name);
|
||||
|
||||
virtual bool canCompute();
|
||||
|
||||
virtual bool compute();
|
||||
|
||||
void setGroup(std::string group);
|
||||
void setLink(std::string link);
|
||||
|
||||
void setPlannerId(std::string planner);
|
||||
void setTimeout(double timeout);
|
||||
|
||||
void setFrom(std::string named_target);
|
||||
void setTo(std::string named_target);
|
||||
|
||||
protected:
|
||||
std::string group_;
|
||||
std::string link_;
|
||||
|
||||
double timeout_;
|
||||
|
||||
std::string planner_id_;
|
||||
|
||||
std::string from_named_target_;
|
||||
std::string to_named_target_;
|
||||
|
||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
||||
};
|
||||
|
||||
}
|
||||
@ -2,6 +2,7 @@
|
||||
|
||||
#include <moveit_task_constructor/subtasks/current_state.h>
|
||||
#include <moveit_task_constructor/subtasks/gripper.h>
|
||||
#include <moveit_task_constructor/subtasks/move.h>
|
||||
#include <moveit_task_constructor/subtasks/generate_grasp_pose.h>
|
||||
#include <moveit_task_constructor/subtasks/cartesian_position_motion.h>
|
||||
|
||||
@ -25,15 +26,13 @@ int main(int argc, char** argv){
|
||||
t.addAfter(move);
|
||||
}
|
||||
|
||||
/*
|
||||
{
|
||||
auto move= std::make_shared<subtask::Move>("move to pre-grasp");
|
||||
auto move= std::make_shared<subtasks::Move>("move to pre-grasp");
|
||||
move->setGroup("arm");
|
||||
move->setPlannerId("RRTConnectkConfigDefault");
|
||||
move->setPlanningTime(5.0);
|
||||
move->setTimeout(8.0);
|
||||
t.addAfter(move);
|
||||
}
|
||||
*/
|
||||
|
||||
{
|
||||
auto move= std::make_shared<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
||||
|
||||
@ -0,0 +1,80 @@
|
||||
#include <moveit_task_constructor/subtasks/move.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_state/conversions.h>
|
||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
||||
|
||||
#include <moveit/move_group_interface/move_group_interface.h>
|
||||
|
||||
#include <moveit_msgs/MotionPlanRequest.h>
|
||||
#include <moveit_msgs/MotionPlanResponse.h>
|
||||
|
||||
moveit::task_constructor::subtasks::Move::Move(std::string name)
|
||||
: moveit::task_constructor::SubTask::SubTask(name),
|
||||
timeout_(5.0)
|
||||
{}
|
||||
|
||||
void
|
||||
moveit::task_constructor::subtasks::Move::setGroup(std::string group){
|
||||
group_= group;
|
||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_);
|
||||
}
|
||||
|
||||
void
|
||||
moveit::task_constructor::subtasks::Move::setLink(std::string link){
|
||||
link_= link;
|
||||
}
|
||||
|
||||
void
|
||||
moveit::task_constructor::subtasks::Move::setPlannerId(std::string planner){
|
||||
planner_id_= planner;
|
||||
}
|
||||
|
||||
void
|
||||
moveit::task_constructor::subtasks::Move::setTimeout(double timeout){
|
||||
timeout_= timeout;
|
||||
}
|
||||
|
||||
/* TODO: implement this in compute
|
||||
void
|
||||
moveit::task_constructor::subtasks::Move::setFrom(std::string named_target){
|
||||
from_named_target_= named_target;
|
||||
}
|
||||
|
||||
void
|
||||
moveit::task_constructor::subtasks::Move::setTo(std::string named_target){
|
||||
to_named_target_= named_target;
|
||||
}
|
||||
*/
|
||||
|
||||
bool
|
||||
moveit::task_constructor::subtasks::Move::canCompute(){
|
||||
return hasStatePair();
|
||||
}
|
||||
|
||||
bool
|
||||
moveit::task_constructor::subtasks::Move::compute(){
|
||||
assert( scene_->getRobotModel() );
|
||||
|
||||
std::pair<InterfaceState&, InterfaceState&> state_pair= fetchStatePair();
|
||||
|
||||
mgi_->setJointValueTarget(state_pair.second.state->getCurrentState());
|
||||
if( !planner_id_.empty() )
|
||||
mgi_->setPlannerId(planner_id_);
|
||||
mgi_->setPlanningTime(timeout_);
|
||||
|
||||
::planning_interface::MotionPlanRequest req;
|
||||
mgi_->constructMotionPlanRequest(req);
|
||||
|
||||
ros::Duration(4.0).sleep();
|
||||
::planning_interface::MotionPlanResponse res;
|
||||
if(!planner_->generatePlan(state_pair.first.state, req, res))
|
||||
return false;
|
||||
|
||||
// finish subtask
|
||||
moveit::task_constructor::SubTrajectory& trajectory= addTrajectory(res.trajectory_);
|
||||
trajectory.connectToBeginning(state_pair.first);
|
||||
trajectory.connectToEnding(state_pair.second);
|
||||
|
||||
return true;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user