From 7a13d79bc412481a33e2e12c34445397c6f20070 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 Nov 2017 11:22:29 +0100 Subject: [PATCH] example setting solution markers --- core/demo/plan_pick_pa10.cpp | 6 ++-- core/demo/plan_pick_trixi.cpp | 12 +++++-- core/demo/plan_pick_ur5.cpp | 12 +++++-- core/src/stages/compute_ik.cpp | 43 +++++++++++++++++++++---- core/src/stages/generate_grasp_pose.cpp | 32 +++++++++++++++++- 5 files changed, 91 insertions(+), 14 deletions(-) diff --git a/core/demo/plan_pick_pa10.cpp b/core/demo/plan_pick_pa10.cpp index 972aa86e..89675e43 100644 --- a/core/demo/plan_pick_pa10.cpp +++ b/core/demo/plan_pick_pa10.cpp @@ -82,7 +82,7 @@ int main(int argc, char** argv){ gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)* Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()), "lh_tool_frame"); - gengrasp->setAngleDelta(-.2); + gengrasp->setAngleDelta(M_PI / 10.); auto ik = std::make_unique("compute ik", std::move(gengrasp)); ik->properties().configureInitFrom(Stage::PARENT); @@ -113,7 +113,9 @@ int main(int argc, char** argv){ try { t.plan(); - t.publishAllSolutions(); + std::cout << "waiting for \n"; + char ch; + std::cin >> ch; } catch (const InitStageException &e) { std::cerr << e; diff --git a/core/demo/plan_pick_trixi.cpp b/core/demo/plan_pick_trixi.cpp index 14d880cc..25f9dbbd 100644 --- a/core/demo/plan_pick_trixi.cpp +++ b/core/demo/plan_pick_trixi.cpp @@ -110,8 +110,16 @@ int main(int argc, char** argv){ t.add(std::move(move)); } - t.plan(); - t.publishAllSolutions(); + try { + t.plan(); + std::cout << "waiting for \n"; + char ch; + std::cin >> ch; + } + catch (const InitStageException &e) { + std::cerr << e; + return EINVAL; + } return 0; } diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 36b52579..4b504838 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -110,8 +110,16 @@ int main(int argc, char** argv){ t.add(std::move(move)); } - t.plan(); - t.publishAllSolutions(); + try { + t.plan(); + std::cout << "waiting for \n"; + char ch; + std::cin >> ch; + } + catch (const InitStageException &e) { + std::cerr << e; + return EINVAL; + } return 0; } diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 119f4257..8ae8fbda 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -3,6 +3,9 @@ // #include +#include +#include + #include #include #include @@ -11,6 +14,7 @@ #include #include #include +#include #include namespace moveit { namespace task_constructor { namespace stages { @@ -213,14 +217,39 @@ void ComputeIK::onNewSolution(const SolutionBase &s) remaining_time -= std::chrono::duration(now - start_time).count(); start_time = now; - if (succeeded) { - // create a new scene for each solution as they will have different robot states - planning_scene::PlanningScenePtr scene = s.start()->scene()->diff(); - robot_state::RobotState& robot_state = scene->getCurrentStateNonConst(); - robot_state.setJointGroupPositions(jmg, ik_solutions.back().data()); + planning_scene::PlanningSceneConstPtr scene = s.start()->scene(); + std::unique_ptr solution(new SubTrajectory()); - spawn(InterfaceState(scene), s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); - } else if (max_ik_solutions == 1) + // include markers from original solution + std::copy(s.markers().begin(), s.markers().end(), std::back_inserter(solution->markers())); + + // frame at target pose + target_pose_msg.header.frame_id = scene->getPlanningFrame(); + rviz_marker_tools::appendFrame(solution->markers(), target_pose_msg, 0.1, "ik frame"); + + if (succeeded) { + solution->setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data())); + // create a new scene for each solution as they will have different robot states + planning_scene::PlanningScenePtr new_scene = s.start()->scene()->diff(); + robot_state::RobotState& robot_state = new_scene->getCurrentStateNonConst(); + robot_state.setJointGroupPositions(jmg, ik_solutions.back().data()); + scene = new_scene; + + // robot model + robot_state.updateLinkTransforms(); + auto appender = [&solution](visualization_msgs::Marker& marker, const std::string& name) { + marker.ns = "ik solution"; + marker.color.a *= 0.5; + solution->markers().push_back(marker); + }; + generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames()); + } else { + solution->setCost(std::numeric_limits::infinity()); + } + + spawn(InterfaceState(scene), std::move(solution)); + + if (!succeeded && max_ik_solutions == 1) break; // first and only attempt failed } } diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp index 0086e788..70e4f02e 100644 --- a/core/src/stages/generate_grasp_pose.cpp +++ b/core/src/stages/generate_grasp_pose.cpp @@ -35,6 +35,9 @@ /* Authors: Robert Haschke, Michael Goerner */ #include +#include +#include +#include #include @@ -143,7 +146,34 @@ bool GenerateGraspPose::compute(){ tf::poseEigenToMsg(link_pose, goal_pose_msg.pose); state.properties().set("target_pose", goal_pose_msg); - spawn(std::move(state), 0.0); + SubTrajectory trajectory; + trajectory.setCost(0.0); + trajectory.setName(std::to_string(current_angle_)); + + // add an arrow marker + visualization_msgs::Marker m; + m.header.frame_id = scene_->getPlanningFrame(); + m.ns = "grasp pose"; + rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); + double scale = 0.1; + rviz_marker_tools::makeArrow(m, scale); + tf::poseEigenToMsg(grasp_pose * grasp2tool * + // arrow should point along z (instead of x) + Eigen::AngleAxisd(-M_PI / 2.0, Eigen::Vector3d::UnitY()) * + // arrow tip at goal_pose + Eigen::Translation3d(-scale, 0, 0), m.pose); + trajectory.markers().push_back(m); + + // add end-effector marker + robot_state.updateStateWithLinkAt(link_name, link_pose); + auto appender = [&trajectory](visualization_msgs::Marker& marker, const std::string& name) { + marker.ns = "grasp eef"; + marker.color.a *= 0.5; + trajectory.markers().push_back(marker); + }; + generateVisualMarkers(robot_state, appender, jmg->getLinkModelNames()); + + spawn(std::move(state), std::move(trajectory)); return true; }