Merge branch 'devel' of github.com:JeroenDM/elion into devel

elion_examples/config/ur5_pos_con.json

@@ -0,0 +1,36 @@
+{
+    "config": {
+        "planning_group": "manipulator",
+        "fixed_frame": "world",
+        "robot_description": "robot_description",
+        "planning_plugin_name": "ompl_interface/OMPLPlanner",
+        "allowed_planning_time": 10.0,
+        "planner_id": "RRTConnect"
+    },
+    "start": {
+        "type": "pose",
+        "xyz": [
+            0.7,
+            -0.6,
+            0.04
+        ],
+        "rpy": [
+            1.57079632679,
+            0,
+            0.78539816339
+        ]
+    },
+    "goal": {
+        "type": "pose",
+        "xyz": [
+            0.7,
+            0.36,
+            0.35
+        ],
+        "rpy": [
+            1.57079632679,
+            0,
+            3.14159265359
+        ]
+    }
+}

elion_examples/config/ur5_pos_con_2.json

@@ -0,0 +1,88 @@
+{
+    "config": {
+        "planning_group": "manipulator",
+        "fixed_frame": "world",
+        "robot_description": "robot_description",
+        "planning_plugin_name": "ompl_interface/OMPLPlanner",
+        "allowed_planning_time": 10.0,
+        "planner_id": "RRTConnect"
+    },
+    "start": {
+        "type": "joint_values",
+        "values": [
+            -0.0537845,
+            -0.551092,
+            1.01938,
+            -0.468291,
+            1.51701,
+            0
+        ]
+    },
+    "goal": {
+        "type": "joint_values",
+        "values": [
+            0.854276,
+            -0.697604,
+            0.485754,
+            0.21185,
+            0.0688774,
+            0
+        ]
+    },
+    "collision_objects": [
+        {
+            "type": "cuboid",
+            "name": "table",
+            "xyz": [
+                0.6,
+                0.36,
+                0.10
+            ],
+            "rpy": [
+                0.0,
+                0.0,
+                0.0
+            ],
+            "dims": [
+                0.5,
+                0.2,
+                0.3
+            ]
+        },
+        {
+            "type": "cuboid",
+            "name": "pillar",
+            "xyz": [
+                0.65,
+                -0.1,
+                0.3
+            ],
+            "rpy": [
+                0.0,
+                0.0,
+                0.0
+            ],
+            "dims": [
+                0.1,
+                0.1,
+                0.7
+            ]
+        }
+    ],
+    "constraints": [
+        {
+            "type": "angle_axis",
+            "link_name": "gripper_reference",
+            "rpy": [
+                1.57079632679,
+                0,
+                0
+            ],
+            "tolerance": [
+                0.1,
+                3.0,
+                0.1
+            ]
+        }
+    ]
+}

elion_examples/config/ur5_pos_con_3.json

@@ -0,0 +1,90 @@
+{
+    "config": {
+        "planning_group": "manipulator",
+        "fixed_frame": "world",
+        "robot_description": "robot_description",
+        "planning_plugin_name": "ompl_interface/OMPLPlanner",
+        "allowed_planning_time": 10.0,
+        "planner_id": "RRTConnect"
+    },
+    "start": {
+        "type": "joint_values",
+        "values": [
+            -0.0537845,
+            -0.551092,
+            1.01938,
+            -0.468291,
+            1.51701,
+            0
+        ]
+    },
+    "goal": {
+        "type": "pose",
+        "xyz": [
+            0.7,
+            0.36,
+            0.35
+        ],
+        "rpy": [
+            1.57079632679,
+            0,
+            3.14159265359
+        ]
+    },
+    "collision_objects": [
+        {
+            "type": "cuboid",
+            "name": "table",
+            "xyz": [
+                0.6,
+                0.36,
+                0.10
+            ],
+            "rpy": [
+                0.0,
+                0.0,
+                0.0
+            ],
+            "dims": [
+                0.5,
+                0.2,
+                0.3
+            ]
+        },
+        {
+            "type": "cuboid",
+            "name": "pillar",
+            "xyz": [
+                0.65,
+                -0.1,
+                0.3
+            ],
+            "rpy": [
+                0.0,
+                0.0,
+                0.0
+            ],
+            "dims": [
+                0.1,
+                0.1,
+                0.7
+            ]
+        }
+    ],
+    "constraints": [
+        {
+            "type": "angle_axis",
+            "link_name": "gripper_reference",
+            "rpy": [
+                1.57079632679,
+                0,
+                0
+            ],
+            "tolerance": [
+                0.1,
+                3.0,
+                0.1
+            ]
+        }
+    ]
+}

elion_examples/src/run_example.cpp

@@ -60,21 +60,64 @@ planning_interface::MotionPlanRequest createPTPProblem(
   return req;
 }
 
+planning_interface::MotionPlanRequest createPTPProblem(
+    const std::vector<double>& start, geometry_msgs::Pose& goal_pose,
+    robot_model::RobotModelPtr& robot_model,
+    const robot_state::JointModelGroup* joint_model_group,
+    moveit_visual_tools::MoveItVisualTools& mvt) {
+  planning_interface::MotionPlanRequest req;
+  req.group_name = joint_model_group->getName();
+
+  // fill out start state in request
+  robot_state::RobotState start_state(robot_model);
+  start_state.setJointGroupPositions(joint_model_group, start);
+  moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
+
+  // fill out goal state in request
+  geometry_msgs::PoseStamped goal_pose_stamped;
+  goal_pose_stamped.pose = goal_pose;
+  goal_pose_stamped.header.frame_id = "world";
+
+  moveit_msgs::Constraints goal =
+      kinematic_constraints::constructGoalConstraints(
+          "gripper_reference", goal_pose_stamped, 0.001, 0.001);
+  req.goal_constraints.clear();
+  req.goal_constraints.push_back(goal);
+
+  // I don't know I nice way to publish two robot states at once with MoveIt
+  // visual tools
+  // Therefore I just put a pause in between to show them both in sequence.
+  mvt.publishRobotState(start, joint_model_group, rviz_visual_tools::GREEN);
+  mvt.trigger();
+  mvt.publishAxisLabeled(goal_pose, "Goal Pose");
+  mvt.trigger();
+
+  return req;
+}
+
 planning_interface::MotionPlanRequest createPTPProblem(
     geometry_msgs::Pose& start_pose, geometry_msgs::Pose& goal_pose,
     robot_model::RobotModelPtr& robot_model,
     const robot_state::JointModelGroup* joint_model_group,
     moveit_visual_tools::MoveItVisualTools& mvt) {
+  mvt.publishAxis(start_pose);
+  mvt.publishAxis(goal_pose);
+  mvt.trigger();
   planning_interface::MotionPlanRequest req;
   req.group_name = joint_model_group->getName();
 
   // fill out start state in request
   robot_state::RobotState start_state(robot_model);
   start_state.setToDefaultValues();
+  start_state.setToRandomPositions();
   bool success = start_state.setFromIK(joint_model_group, start_pose, 10.0);
   ROS_INFO_STREAM("Start pose IK: " << (success ? "succeeded." : "failed."));
   moveit::core::robotStateToRobotStateMsg(start_state, req.start_state);
 
+  Eigen::VectorXd js(6);
+  start_state.copyJointGroupPositions(joint_model_group, js);
+  std::cout << "Start state: " << js.transpose() << std::endl;
+
   // fill out goal state in request
   robot_state::RobotState goal_state(start_state);
   goal_state.setToDefaultValues();
@@ -86,6 +129,9 @@ planning_interface::MotionPlanRequest createPTPProblem(
   req.goal_constraints.clear();
   req.goal_constraints.push_back(joint_goal);
 
+  goal_state.copyJointGroupPositions(joint_model_group, js);
+  std::cout << "Goal state: " << js.transpose() << std::endl;
+
   // I don't know I nice way to publish two robot states at once with MoveIt
   // visual tools
   // Therefore I just put a pause in between to show them both in sequence.
@@ -277,6 +323,11 @@ int main(int argc, char** argv) {
     auto goal_pose = elion::jsonToPoseMsg(root["goal"]);
     req1 = createPTPProblem(start_pose, goal_pose, robot_model,
                             joint_model_group, visual_tools);
+  } else if (start_type == "joint_values" && goal_type == "pose") {
+    std::vector<double> start{elion::jsonToVector(root["start"]["values"])};
+    auto goal_pose = elion::jsonToPoseMsg(root["goal"]);
+    req1 = createPTPProblem(start, goal_pose, robot_model, joint_model_group,
+                            visual_tools);
   } else {
     ROS_ERROR_STREAM("Unkown type of start or goal type: " << start_type << ", "
                                                            << goal_type);