Skip to content
Snippets Groups Projects
Commit 7491de87 authored by KingMaZito's avatar KingMaZito
Browse files

fixed mtc issue

parent 97f6ca66
No related branches found
No related tags found
No related merge requests found
Pipeline #14914 failed
/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
......@@ -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"/>
......
......@@ -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());
}
......
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"
......
......@@ -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();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment