From 992b2386f76450ddf0af5d46d54e819b8c2ca121 Mon Sep 17 00:00:00 2001
From: mrceki <105711013+mrceki@users.noreply.github.com>
Date: Wed, 8 Mar 2023 10:51:29 +0300
Subject: [PATCH] pick & place parameters updated

---
 air_moveit_config/config/air_config.yaml  | 18 +++++++++---------
 air_moveit_config/src/pick_place_demo.cpp |  8 +++++++-
 air_moveit_config/src/pick_place_task.cpp |  6 +++---
 3 files changed, 19 insertions(+), 13 deletions(-)

diff --git a/air_moveit_config/config/air_config.yaml b/air_moveit_config/config/air_config.yaml
index 0349e2b..c878a9f 100644
--- a/air_moveit_config/config/air_config.yaml
+++ b/air_moveit_config/config/air_config.yaml
@@ -1,5 +1,5 @@
 # Total planning attempts
-max_solutions: 10
+max_solutions: 1000
 
 
 # Planning group and link names
@@ -10,8 +10,8 @@ hand_frame: "qbhand_base_link"
 
 # Poses
 hand_open_pose: "open"
-hand_close_pose: "closed"
-arm_home_pose: "home"
+hand_close_pose: "full_closed"
+arm_home_pose: "up"
 
 # Scene frames
 world_frame: "world"
@@ -21,21 +21,21 @@ surface_link: "table"
 
 # Collision object for picking
 # CYLINDER object specifications
-object_name: "object"
-object_dimensions: [0.25, 0.02] # [height, radius]
+object_name: "sphere"
+object_dimensions: [0.05, 0.01] # [height, radius]
 object_pose: [0.55, -0.25, 0.7, 0, 0, 0]
 
 # Table model
-spawn_table: true
+spawn_table: True
 table_name: "table"
 table_dimensions: [0.4, 0.5, 0.2]  # [length, width, height]
 table_pose: [0.4, -0.21, 0.69, 0, 0, 0]
 
 # Gripper grasp frame transform [x,y,z,r,p,y]
-grasp_frame_transform: [0.0, 0.04, 0.13, -1.571, -1.571, 1.571]
+grasp_frame_transform: [0.0, 0.07, 0.13, 1.571, 0, 0]
 
 # Place pose [x,y,z,r,p,y]
-place_pose: [0.45, -0.1, 0.7, 0, 0, 0]
+place_pose: [0.59, -0.25, 0.82, 0, 0, 0]
 place_surface_offset: 0.0005 # place offset from table
 
 # Valid distance range when approaching an object for picking
@@ -44,4 +44,4 @@ approach_object_max_dist: 0.25
 
 # Valid height range when lifting an object after pick
 lift_object_min_dist: 0.00
-lift_object_max_dist: 0.25
+lift_object_max_dist: 0.15
diff --git a/air_moveit_config/src/pick_place_demo.cpp b/air_moveit_config/src/pick_place_demo.cpp
index a14c425..a03f3cd 100644
--- a/air_moveit_config/src/pick_place_demo.cpp
+++ b/air_moveit_config/src/pick_place_demo.cpp
@@ -50,7 +50,7 @@ int main(int argc, char** argv) {
 	ros::AsyncSpinner spinner(1);
 	spinner.start();
 
-	moveit_task_constructor_demo::setupDemoScene(pnh);
+	//moveit_task_constructor_demo::setupDemoScene(pnh);
 
 	// Construct and run pick/place task
 	moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh);
@@ -58,17 +58,23 @@ int main(int argc, char** argv) {
 		ROS_INFO_NAMED(LOGNAME, "Initialization failed");
 		return 1;
 	}
+	else{
+		system("rosnode kill air_collision_publisher");
+	}	
 
 	if (pick_place_task.plan()) {
 		ROS_INFO_NAMED(LOGNAME, "Planning succeded");
 		if (pnh.param("execute", false)) {
 			pick_place_task.execute();
 			ROS_INFO_NAMED(LOGNAME, "Execution complete");
+			system("rosrun air_moveit_config add_sphere");
 		} else {
 			ROS_INFO_NAMED(LOGNAME, "Execution disabled");
+			system("rosrun air_moveit_config add_sphere");
 		}
 	} else {
 		ROS_INFO_NAMED(LOGNAME, "Planning failed");
+		system("rosrun air_moveit_config add_sphere");
 	}
 
 	// Keep introspection alive
diff --git a/air_moveit_config/src/pick_place_task.cpp b/air_moveit_config/src/pick_place_task.cpp
index 3ef7168..5526cf0 100644
--- a/air_moveit_config/src/pick_place_task.cpp
+++ b/air_moveit_config/src/pick_place_task.cpp
@@ -247,7 +247,7 @@ bool PickPlaceTask::init() {
 			// Set hand forward direction
 			geometry_msgs::Vector3Stamped vec;
 			vec.header.frame_id = hand_frame_;
-			vec.vector.z = 1.0;
+			vec.vector.y = 1.0;
 			stage->setDirection(vec);
 			grasp->insert(std::move(stage));
 		}
@@ -448,12 +448,12 @@ bool PickPlaceTask::init() {
 		{
 			auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
 			stage->properties().configureInitFrom(Stage::PARENT, { "group" });
-			stage->setMinMaxDistance(.12, .25);
+				stage->setMinMaxDistance(.00, .05);
 			stage->setIKFrame(hand_frame_);
 			stage->properties().set("marker_ns", "retreat");
 			geometry_msgs::Vector3Stamped vec;
 			vec.header.frame_id = hand_frame_;
-			vec.vector.z = -1.0;
+			vec.vector.z = -0.05;
 			stage->setDirection(vec);
 			place->insert(std::move(stage));
 		}