Skip to content

Commit

Permalink
added depalletizing to mtc demo (needs more tweeking)
Browse files Browse the repository at this point in the history
  • Loading branch information
@PetoAdam
PetoAdam committed Sep 10, 2024
1 parent 17ccfb9 commit d731811
Show file tree
Hide file tree
Showing 2 changed files with 177 additions and 102 deletions.
14 changes: 4 additions & 10 deletions ...it_task_constructor/include/kuka_moveit_task_constructor/kuka_moveit_task_constructor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,16 +7,8 @@
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif

namespace mtc = moveit::task_constructor;

Expand All @@ -27,12 +19,14 @@ class MTCTaskNode

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
void setupPlanningScene();
// Compose an MTC task from a series of stages.
mtc::Task createTask();
mtc::Task createDepalletizingTask();
bool doTask(mtc::Task& task);

private:
rclcpp::Node::SharedPtr node_;
void addPalletObjects();
void attachObject(const std::string& object_id);
void detachAndRemoveObject(const std::string& object_id);
};

#endif // KUKA_MOVEIT_TASK_CONSTRUCTOR_HPP
265 changes: 173 additions & 92 deletions ...ask_constructor_example/kuka_moveit_task_constructor/src/kuka_moveit_task_constructor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,21 +14,179 @@ MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)

void MTCTaskNode::setupPlanningScene()
{
moveit_msgs::msg::CollisionObject object;
object.id = "object";
object.header.frame_id = "world";
object.primitives.resize(1);
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
object.primitives[0].dimensions = { 0.1, 0.02 };

geometry_msgs::msg::Pose pose;
pose.position.x = 0.5;
pose.position.y = -0.25;
pose.orientation.w = 1.0;
object.pose = pose;
addPalletObjects(); // Setup the pallet objects in the planning scene
}

void MTCTaskNode::addPalletObjects()
{
moveit::planning_interface::PlanningSceneInterface psi;

for (int k = 0; k < 1; k++)
{
for (int j = 0; j < 1; j++)
{
for (int i = 0; i < 1; i++)
{
moveit_msgs::msg::CollisionObject pallet_object;
pallet_object.header.frame_id = "world";
pallet_object.id = "pallet_" + std::to_string(9 * k + 3 * j + i);

shape_msgs::msg::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.097;
primitive.dimensions[primitive.BOX_Y] = 0.097;
primitive.dimensions[primitive.BOX_Z] = 0.097;

geometry_msgs::msg::Pose pose;
pose.orientation.w = 1.0;
pose.position.x = 0.3 + i * 0.1;
pose.position.y = -0.1 + j * 0.1;
pose.position.z = 0.3 - 0.1 * k;

pallet_object.primitives.push_back(primitive);
pallet_object.primitive_poses.push_back(pose);
pallet_object.operation = pallet_object.ADD;

psi.applyCollisionObject(pallet_object);
std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Delay for visualization
}
}
}
}

void MTCTaskNode::attachObject(const std::string& object_id)
{
moveit_msgs::msg::AttachedCollisionObject attached_object;
attached_object.link_name = "flange";
attached_object.object.id = object_id;
attached_object.object.operation = attached_object.object.REMOVE;

moveit::planning_interface::PlanningSceneInterface psi;
psi.applyAttachedCollisionObject(attached_object);
}

void MTCTaskNode::detachAndRemoveObject(const std::string& object_id)
{
moveit_msgs::msg::AttachedCollisionObject attached_object;
attached_object.link_name = "flange";
attached_object.object.id = object_id;
attached_object.object.operation = attached_object.object.REMOVE;

moveit::planning_interface::PlanningSceneInterface psi;
psi.applyCollisionObject(object);
psi.applyAttachedCollisionObject(attached_object);
psi.removeCollisionObjects({object_id});
}

mtc::Task MTCTaskNode::createDepalletizingTask()
{
mtc::Task task;
task.stages()->setName("depalletizing task");
task.loadRobotModel(node_);

const auto& arm_group_name = "manipulator";
const auto& eef_frame = "flange";

task.setProperty("group", arm_group_name);
task.setProperty("ik_frame", eef_frame);

auto current_state_stage = std::make_unique<mtc::stages::CurrentState>("current state");
task.add(std::move(current_state_stage));

auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
sampling_planner->setTimeout(30.0);
auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();
auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
cartesian_planner->setMaxVelocityScalingFactor(0.2);
cartesian_planner->setMaxAccelerationScalingFactor(0.2);
cartesian_planner->setStepSize(.001);
cartesian_planner->setMinFraction(.02);

// Define start
auto start = std::make_unique<mtc::stages::MoveTo>("start", interpolation_planner);
start->properties().set("marker_ns", "start");
start->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });

// Define the joint positions for the goal
std::map<std::string, double> start_joint_positions = {
{"joint_1", 0.0},
{"joint_2", -1.07},
{"joint_3", 0.0},
{"joint_4", 0.0},
{"joint_5", 0.0},
{"joint_6", 0.0}
};
start->setGoal(start_joint_positions);
task.add(std::move(start));

// Define Pick Stage
auto pick_stage = std::make_unique<mtc::stages::MoveTo>("pick", sampling_planner);
pick_stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
pick_stage->properties().set("marker_ns", "pick");
geometry_msgs::msg::PoseStamped pick_pose;
pick_pose.header.frame_id = "world";
pick_pose.pose.position.x = 0.3;
pick_pose.pose.position.y = -0.1;
pick_pose.pose.position.z = 0.4;
pick_pose.pose.orientation.x = 0.0;
pick_pose.pose.orientation.y = 1.0;
pick_pose.pose.orientation.z = 0.0;
pick_pose.pose.orientation.w = 0.0;
pick_stage->setGoal(pick_pose);
task.add(std::move(pick_stage));

// Attach Object Stage
auto attach_stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");
attach_stage->attachObject("pallet_0", eef_frame);
task.add(std::move(attach_stage));

// Define Lift Stage
auto lift_stage = std::make_unique<mtc::stages::MoveTo>("lift", cartesian_planner);
lift_stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
lift_stage->properties().set("marker_ns", "lift");
geometry_msgs::msg::PoseStamped lift_pose;
lift_pose.header.frame_id = "world";
lift_pose.pose.position.x = 0.3;
lift_pose.pose.position.y = -0.1;
lift_pose.pose.position.z = 0.55;
lift_pose.pose.orientation.x = 0.0;
lift_pose.pose.orientation.y = 1.0;
lift_pose.pose.orientation.z = 0.0;
lift_pose.pose.orientation.w = 0.0;
lift_stage->setGoal(lift_pose);
task.add(std::move(lift_stage));

// Define Place Stage
auto place = std::make_unique<mtc::stages::MoveRelative>("place", sampling_planner);
place->properties().set("marker_ns", "place");
place->properties().set("link", eef_frame);
place->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
place->setMinMaxDistance(0.0, 0.2);
geometry_msgs::msg::Vector3Stamped vec_place;
vec_place.header.frame_id = "world";
vec_place.vector.x = -0.6;
//vec_place.vector.y = 0.6;
place->setDirection(vec_place);
//task.add(std::move(place));

// Define Lower Stage
auto lower = std::make_unique<mtc::stages::MoveRelative>("lower", cartesian_planner);
lower->properties().set("marker_ns", "lower");
lower->properties().set("link", eef_frame);
lower->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
lower->setMinMaxDistance(0.0, 0.2);
geometry_msgs::msg::Vector3Stamped vec_lower;
vec_lower.header.frame_id = "world";
vec_lower.vector.z = -0.2;
lower->setDirection(vec_lower);
task.add(std::move(lower));

// Detach Object Stage
auto detach_stage = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object");
detach_stage->detachObject("pallet_0", eef_frame);
task.add(std::move(detach_stage));

return task;
}

bool MTCTaskNode::doTask(mtc::Task& task)
Expand All @@ -49,7 +207,7 @@ bool MTCTaskNode::doTask(mtc::Task& task)
return false;
}

if (!task.plan(5))
if (!task.plan(20))
{
RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
return false;
Expand All @@ -66,83 +224,6 @@ bool MTCTaskNode::doTask(mtc::Task& task)
return true;
}

mtc::Task MTCTaskNode::createTask()
{
mtc::Task task;
task.stages()->setName("demo task");
task.loadRobotModel(node_);

const auto& arm_group_name = "manipulator";
const auto& eef_frame = "end_effector";

// Set task properties
task.setProperty("group", arm_group_name);
task.setProperty("ik_frame", eef_frame);

auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
mtc::Stage* current_state_ptr = stage_state_current.get();
task.add(std::move(stage_state_current));

auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();

auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
cartesian_planner->setMaxVelocityScalingFactor(0.2);
cartesian_planner->setMaxAccelerationScalingFactor(0.2);
cartesian_planner->setStepSize(.01);
cartesian_planner->setMinFraction(0.2);

// Set stage1 as MoveRelative
auto stage1 = std::make_unique<mtc::stages::MoveRelative>("move1", cartesian_planner);
stage1->properties().set("marker_ns", "move1");
stage1->properties().set("link", eef_frame);
stage1->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
stage1->setMinMaxDistance(0.0, 0.2);
geometry_msgs::msg::Vector3Stamped vec;
vec.header.frame_id = eef_frame;
vec.vector.z = 1.0;
stage1->setDirection(vec);
task.add(std::move(stage1));

// Set stage2 as MoveTo with cartesian goal
auto stage2 = std::make_unique<mtc::stages::MoveTo>("move2", cartesian_planner);
stage2->properties().set("marker_ns", "move2");
stage2->properties().set("link", eef_frame);
stage2->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });

// Set the goal pose for stage2
geometry_msgs::msg::PoseStamped goalPose;
goalPose.header.frame_id = "world";
goalPose.pose.position.x = 0.4;
goalPose.pose.position.y = 0.2;
goalPose.pose.position.z = 0.3;
goalPose.pose.orientation.x = 1.0;
goalPose.pose.orientation.y = 1.0;
goalPose.pose.orientation.z = 1.0;
goalPose.pose.orientation.w = 1.0;
stage2->setGoal(goalPose);
task.add(std::move(stage2));

// Set stage3 as MoveTo with joint positions
auto stage3 = std::make_unique<mtc::stages::MoveTo>("move3", interpolation_planner);
stage3->properties().set("marker_ns", "move3");
stage3->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });

// Define the joint positions for the goal
std::map<std::string, double> joint_positions = {
{"joint_1", 0.0},
{"joint_2", -0.5},
{"joint_3", 1.0},
{"joint_4", -1.5},
{"joint_5", 1.0},
{"joint_6", 0.5}
};
stage3->setGoal(joint_positions);
task.add(std::move(stage3));

return task;
}

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
Expand All @@ -160,7 +241,7 @@ int main(int argc, char** argv)
});

mtc_task_node->setupPlanningScene();
mtc::Task task = mtc_task_node->createTask();
mtc::Task task = mtc_task_node->createDepalletizingTask();
bool success = mtc_task_node->doTask(task);

spin_thread->join();
Expand Down

0 comments on commit d731811

Please sign in to comment.