From 9e71753046ed09075355ec56b66f8edaa4edd544 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 2 Feb 2023 17:34:20 +0100
Subject: [PATCH] limit the movit controller to not move more than 40cm behind
the table
---
src/moveit_sorting_controller.cpp | 10 ++++++++++
1 file changed, 10 insertions(+)
diff --git a/src/moveit_sorting_controller.cpp b/src/moveit_sorting_controller.cpp
index 1a4ae2c..e5ba294 100644
--- a/src/moveit_sorting_controller.cpp
+++ b/src/moveit_sorting_controller.cpp
@@ -37,6 +37,16 @@ int main(int argc, char** argv)
MoveItRobotArmController controller{ n, NODE_NAME, robotName };
+ // we can only set an absolute workspace here, because we do not have the scene yet!
+ geometry_msgs::Point minPoint, maxPoint;
+ minPoint.x = -.58; // we need 58 free space, i.e. ~40 cm behind the table (if the robot is at -22)
+ minPoint.y = -1;
+ minPoint.z = 0; // the robot can not reach under itself, i.e., into the table
+ maxPoint.x = 1;
+ maxPoint.y = 1;
+ maxPoint.z = 1.5;
+ controller.setWorkspace(minPoint, maxPoint, true);
+
auto mqttServer = getPrivateParameter<std::string>("mqtt_server", "localhost:1883");
std::unique_ptr<MqttConnection> mqtt_connection =
std::make_unique<MqttConnection>(mqttServer, ros::this_node::getName());
--
GitLab