From 7491de873313e77d76f1f376060090324b57133d Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Sun, 30 Oct 2022 17:16:40 +0100
Subject: [PATCH] fixed mtc issue
---
.catkin_tools/profiles/default/devel_collisions.txt | 2 +-
src/ceti_double/config/panda.srdf | 4 ++--
src/mtc/include/impl/moveit_robot.h | 1 +
src/mtc/mtc_task_file/dummy.yaml | 5 +++--
src/mtc/src/impl/map_loader.cpp | 4 ++--
5 files changed, 9 insertions(+), 7 deletions(-)
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 935b46d..fe7274b 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
/home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 12632
+/home/matteo/reachability/devel/./cmake.lock 12715
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/ceti_double/config/panda.srdf b/src/ceti_double/config/panda.srdf
index 42aab71..c186de1 100644
--- a/src/ceti_double/config/panda.srdf
+++ b/src/ceti_double/config/panda.srdf
@@ -64,8 +64,8 @@
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
- <end_effector name="hand_1" parent_link="panda_1_link8" group="hand_1"/>
- <end_effector name="hand_2" parent_link="panda_2_link8" group="hand_2"/>
+ <end_effector name="hand_1" parent_link="panda_1_link8" group="hand_1" parent_group="panda_arm1"/>
+ <end_effector name="hand_2" parent_link="panda_2_link8" group="hand_2" parent_group="panda_arm2"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint1" type="fixed" parent_frame="world" child_link="base_1"/>
<virtual_joint name="virtual_joint2" type="fixed" parent_frame="world" child_link="base_2"/>
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
index 2b3aa8c..15d12a1 100644
--- a/src/mtc/include/impl/moveit_robot.h
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -12,6 +12,7 @@ class Moveit_robot : public Robot{
public:
Moveit_robot(std::string name, tf2::Transform tf) : Robot(name, tf){
mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
+ ROS_INFO("planning frame: %s", mgi_->getPlanningFrame().c_str());
}
diff --git a/src/mtc/mtc_task_file/dummy.yaml b/src/mtc/mtc_task_file/dummy.yaml
index 8bd37d8..1c90201 100644
--- a/src/mtc/mtc_task_file/dummy.yaml
+++ b/src/mtc/mtc_task_file/dummy.yaml
@@ -1,3 +1,4 @@
+
planners:
- id: "cartesian"
type: "CartesianPath"
@@ -160,7 +161,7 @@ task:
set:
monitored_stage: "pick_up"
pose:
- point: {x: 0.1, y: 0.7, z: 0.9305}
+ point: {x: 0.1, y: 0.3, z: 0.9305}
orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
- type: "MoveTo"
planner: "sampling"
@@ -201,7 +202,7 @@ task:
planner: "sampling"
propertiesConfigureInitFrom:
source: "PARENT"
- value:
+ values:
- "group"
set:
goal: "ready"
diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp
index b1c2e40..0fc66eb 100644
--- a/src/mtc/src/impl/map_loader.cpp
+++ b/src/mtc/src/impl/map_loader.cpp
@@ -325,7 +325,7 @@ void Map_loader::write_task(){
stage_in_stage["stage"]["type"] = "GeneratePose";
stage_in_stage["stage"]["set"]["monitored_stage"] = "pick_up";
- stage_in_stage["stage"]["set"]["pose"]["point"] = YAML::Load("{ x: 0.1, y: 0.7, z: 0.9305 }"); // Hier objekt
+ stage_in_stage["stage"]["set"]["pose"]["point"] = YAML::Load("{ x: 0.1, y: 0.3, z: 0.9305 }"); // Hier objekt
stage_in_stage["stage"]["set"]["pose"]["orientation"] = YAML::Load("{ x: 0.0, y: 0.0, z: 0.0, w: 1.0}");
stage["stages"].push_back(stage_in_stage);
stage_in_stage.reset();
@@ -368,7 +368,7 @@ void Map_loader::write_task(){
stage_in_stage["type"] = "MoveTo";
stage_in_stage["planner"] = "sampling";
stage_in_stage["propertiesConfigureInitFrom"]["source"]= "PARENT";
- stage_in_stage["propertiesConfigureInitFrom"]["value"].push_back("group");
+ stage_in_stage["propertiesConfigureInitFrom"]["values"].push_back("group");
stage_in_stage["set"]["goal"] = "ready";
stage["stages"].push_back(stage_in_stage);
stage_in_stage.reset();
--
GitLab