mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
- allow hierarchical organization of stages (serially for now) - validate correctness of tree (at composition time, i.e. runtime) - derive Task from SerialContainer - fix pimpl_func(), PRIVATE_CLASS declaration in "public" section to allow access in tests
114 lines
3.0 KiB
C++
114 lines
3.0 KiB
C++
#include <moveit_task_constructor/task.h>
|
|
#include <moveit_task_constructor/debug.h>
|
|
|
|
#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>
|
|
|
|
#include <ros/ros.h>
|
|
|
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
|
|
|
using namespace moveit::task_constructor;
|
|
|
|
void spawnObject(){
|
|
moveit::planning_interface::PlanningSceneInterface psi;
|
|
|
|
moveit_msgs::CollisionObject o;
|
|
o.id= "object";
|
|
o.header.frame_id= "table_top";
|
|
o.primitive_poses.resize(1);
|
|
o.primitive_poses[0].position.x= -0.2;
|
|
o.primitive_poses[0].position.y= 0.13;
|
|
o.primitive_poses[0].position.z= 0.12;
|
|
o.primitive_poses[0].orientation.w= 1.0;
|
|
o.primitives.resize(1);
|
|
o.primitives[0].type= shape_msgs::SolidPrimitive::CYLINDER;
|
|
o.primitives[0].dimensions.resize(2);
|
|
o.primitives[0].dimensions[0]= 0.23;
|
|
o.primitives[0].dimensions[1]= 0.03;
|
|
psi.applyCollisionObject(o);
|
|
}
|
|
|
|
int main(int argc, char** argv){
|
|
ros::init(argc, argv, "plan_pick");
|
|
ros::AsyncSpinner spinner(1);
|
|
spinner.start();
|
|
|
|
spawnObject();
|
|
|
|
Task t;
|
|
|
|
t.add(std::make_unique<subtasks::CurrentState>("current state"));
|
|
|
|
{
|
|
auto move = std::make_unique<subtasks::Gripper>("open gripper");
|
|
move->setEndEffector("gripper");
|
|
move->setTo("open");
|
|
t.add(std::move(std::move(move)));
|
|
}
|
|
|
|
{
|
|
auto move = std::make_unique<subtasks::Move>("move to pre-grasp");
|
|
move->setGroup("arm");
|
|
move->setPlannerId("RRTConnectkConfigDefault");
|
|
move->setTimeout(8.0);
|
|
t.add(std::move(std::move(move)));
|
|
}
|
|
|
|
{
|
|
auto move = std::make_unique<subtasks::CartesianPositionMotion>("proceed to grasp pose");
|
|
move->setGroup("arm");
|
|
move->setLink("s_model_tool0");
|
|
move->setMinMaxDistance(.03, 0.1);
|
|
move->setCartesianStepSize(0.02);
|
|
|
|
geometry_msgs::PointStamped target;
|
|
target.header.frame_id= "object";
|
|
move->towards(target);
|
|
t.add(std::move(std::move(move)));
|
|
}
|
|
|
|
{
|
|
auto gengrasp = std::make_unique<subtasks::GenerateGraspPose>("generate grasp pose");
|
|
gengrasp->setEndEffector("gripper");
|
|
//gengrasp->setGroup("arm");
|
|
gengrasp->setGripperGraspPose("open");
|
|
gengrasp->setObject("object");
|
|
gengrasp->setGraspOffset(.03);
|
|
gengrasp->setAngleDelta(-.2);
|
|
gengrasp->setMaxIKSolutions(8);
|
|
t.add(std::move(std::move(gengrasp)));
|
|
}
|
|
|
|
{
|
|
auto move = std::make_unique<subtasks::Gripper>("grasp");
|
|
move->setEndEffector("gripper");
|
|
move->setTo("closed");
|
|
move->graspObject("object");
|
|
t.add(std::move(std::move(move)));
|
|
}
|
|
|
|
{
|
|
auto move = std::make_unique<subtasks::CartesianPositionMotion>("lift object");
|
|
move->setGroup("arm");
|
|
move->setLink("s_model_tool0");
|
|
move->setMinMaxDistance(0.03, 0.05);
|
|
move->setCartesianStepSize(0.01);
|
|
|
|
geometry_msgs::Vector3Stamped direction;
|
|
direction.header.frame_id= "world";
|
|
direction.vector.z= 1.0;
|
|
move->along(direction);
|
|
t.add(std::move(std::move(move)));
|
|
}
|
|
|
|
t.plan();
|
|
|
|
publishAllPlans(t);
|
|
|
|
return 0;
|
|
}
|