first implementation of the Move subtask

This commit is contained in:
v4hn 2017-03-06 19:49:06 +01:00
parent a9c9a61710
commit ca6151cacb
3 changed files with 127 additions and 4 deletions

View 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_;
};
}

View File

@ -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");

View File

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