mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
example setting solution markers
This commit is contained in:
parent
97bb6ef2c6
commit
7a13d79bc4
@ -82,7 +82,7 @@ int main(int argc, char** argv){
|
|||||||
gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)*
|
gengrasp->setToolToGraspTF(Eigen::Translation3d(0,0,.05)*
|
||||||
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
Eigen::AngleAxisd(-0.5*M_PI, Eigen::Vector3d::UnitY()),
|
||||||
"lh_tool_frame");
|
"lh_tool_frame");
|
||||||
gengrasp->setAngleDelta(-.2);
|
gengrasp->setAngleDelta(M_PI / 10.);
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
||||||
ik->properties().configureInitFrom(Stage::PARENT);
|
ik->properties().configureInitFrom(Stage::PARENT);
|
||||||
@ -113,7 +113,9 @@ int main(int argc, char** argv){
|
|||||||
|
|
||||||
try {
|
try {
|
||||||
t.plan();
|
t.plan();
|
||||||
t.publishAllSolutions();
|
std::cout << "waiting for <enter>\n";
|
||||||
|
char ch;
|
||||||
|
std::cin >> ch;
|
||||||
}
|
}
|
||||||
catch (const InitStageException &e) {
|
catch (const InitStageException &e) {
|
||||||
std::cerr << e;
|
std::cerr << e;
|
||||||
|
|||||||
@ -110,8 +110,16 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
try {
|
||||||
t.plan();
|
t.plan();
|
||||||
t.publishAllSolutions();
|
std::cout << "waiting for <enter>\n";
|
||||||
|
char ch;
|
||||||
|
std::cin >> ch;
|
||||||
|
}
|
||||||
|
catch (const InitStageException &e) {
|
||||||
|
std::cerr << e;
|
||||||
|
return EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -110,8 +110,16 @@ int main(int argc, char** argv){
|
|||||||
t.add(std::move(move));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
try {
|
||||||
t.plan();
|
t.plan();
|
||||||
t.publishAllSolutions();
|
std::cout << "waiting for <enter>\n";
|
||||||
|
char ch;
|
||||||
|
std::cin >> ch;
|
||||||
|
}
|
||||||
|
catch (const InitStageException &e) {
|
||||||
|
std::cerr << e;
|
||||||
|
return EINVAL;
|
||||||
|
}
|
||||||
|
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -3,6 +3,9 @@
|
|||||||
//
|
//
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||||
|
#include <moveit/task_constructor/storage.h>
|
||||||
|
#include <moveit/task_constructor/marker_tools.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/robot_state/conversions.h>
|
#include <moveit/robot_state/conversions.h>
|
||||||
#include <moveit/robot_state/robot_state.h>
|
#include <moveit/robot_state/robot_state.h>
|
||||||
@ -11,6 +14,7 @@
|
|||||||
#include <eigen_conversions/eigen_msg.h>
|
#include <eigen_conversions/eigen_msg.h>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
|
#include <iterator>
|
||||||
#include <ros/console.h>
|
#include <ros/console.h>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
@ -213,14 +217,39 @@ void ComputeIK::onNewSolution(const SolutionBase &s)
|
|||||||
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
remaining_time -= std::chrono::duration<double>(now - start_time).count();
|
||||||
start_time = now;
|
start_time = now;
|
||||||
|
|
||||||
if (succeeded) {
|
planning_scene::PlanningSceneConstPtr scene = s.start()->scene();
|
||||||
// create a new scene for each solution as they will have different robot states
|
std::unique_ptr<SolutionBase> solution(new SubTrajectory());
|
||||||
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
|
|
||||||
robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
|
|
||||||
robot_state.setJointGroupPositions(jmg, ik_solutions.back().data());
|
|
||||||
|
|
||||||
spawn(InterfaceState(scene), s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
|
// include markers from original solution
|
||||||
} else if (max_ik_solutions == 1)
|
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<double>::infinity());
|
||||||
|
}
|
||||||
|
|
||||||
|
spawn(InterfaceState(scene), std::move(solution));
|
||||||
|
|
||||||
|
if (!succeeded && max_ik_solutions == 1)
|
||||||
break; // first and only attempt failed
|
break; // first and only attempt failed
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@ -35,6 +35,9 @@
|
|||||||
/* Authors: Robert Haschke, Michael Goerner */
|
/* Authors: Robert Haschke, Michael Goerner */
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
|
#include <moveit/task_constructor/storage.h>
|
||||||
|
#include <moveit/task_constructor/marker_tools.h>
|
||||||
|
#include <rviz_marker_tools/marker_creation.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
@ -143,7 +146,34 @@ bool GenerateGraspPose::compute(){
|
|||||||
tf::poseEigenToMsg(link_pose, goal_pose_msg.pose);
|
tf::poseEigenToMsg(link_pose, goal_pose_msg.pose);
|
||||||
state.properties().set("target_pose", goal_pose_msg);
|
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user