add two demos for cost computation aspects

Author: v4hn

demo/CMakeLists.txt

@@ -27,6 +27,12 @@ target_link_libraries(modular ${catkin_LIBRARIES})
 add_executable(pick_place_demo src/pick_place_demo.cpp src/pick_place_task.cpp)
 target_link_libraries(pick_place_demo ${catkin_LIBRARIES})
 
+add_executable(alternative_path_costs src/alternative_path_costs.cpp)
+target_link_libraries(alternative_path_costs ${catkin_LIBRARIES})
+
+add_executable(ik_clearance_cost src/ik_clearance_cost.cpp)
+target_link_libraries(ik_clearance_cost ${catkin_LIBRARIES})
+
 install(TARGETS cartesian modular pick_place_demo
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

demo/config/mtc.rviz

@@ -17,6 +17,7 @@ Panels:
   - Class: moveit_task_constructor/Motion Planning Tasks
     Global Settings:
       Task View Settings:
+        Show Computation Times: true
         Task Expansion: All Expanded
     Name: Motion Planning Tasks
     Tasks View:

demo/src/alternative_path_costs.cpp

@@ -0,0 +1,82 @@
+#include <ros/ros.h>
+
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <moveit/task_constructor/task.h>
+#include <moveit/task_constructor/container.h>
+
+#include <moveit/task_constructor/solvers/pipeline_planner.h>
+
+#include <moveit/task_constructor/stages/connect.h>
+#include <moveit/task_constructor/stages/fixed_state.h>
+
+#include <moveit/task_constructor/cost_terms.h>
+
+using namespace moveit::task_constructor;
+
+/* FixedState - Connect - FixedState */
+int main(int argc, char** argv) {
+	ros::init(argc, argv, "mtc_tutorial");
+
+	Task t;
+	t.stages()->setName("alternative path costs");
+	t.loadRobotModel();
+
+	assert(t.getRobotModel()->getName() == "panda");
+
+	auto scene{ std::make_shared<planning_scene::PlanningScene>(t.getRobotModel()) };
+	auto& robot_state{ scene->getCurrentStateNonConst() };
+	robot_state.setToDefaultValues();
+	robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended");
+
+	auto initial{ std::make_unique<stages::FixedState>("start") };
+	initial->setState(scene);
+	t.add(std::move(initial));
+
+	auto pipeline{ std::make_shared<solvers::PipelinePlanner>() };
+
+	auto alternatives{ std::make_unique<Alternatives>("connect") };
+	{
+		auto connect{ std::make_unique<stages::Connect>(
+			 "path length", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) };
+		connect->setCostTerm(std::make_unique<cost::PathLength>());  // This is the default for Connect, specified for
+		                                                             // demonstration purposes
+		alternatives->add(std::move(connect));
+	}
+	{
+		auto connect{ std::make_unique<stages::Connect>(
+			 "trajectory duration", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) };
+		connect->setCostTerm(std::make_unique<cost::TrajectoryDuration>());
+		alternatives->add(std::move(connect));
+	}
+	{
+		auto connect{ std::make_unique<stages::Connect>(
+			 "eef motion", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) };
+		connect->setCostTerm(std::make_unique<cost::LinkMotion>("panda_hand"));
+		alternatives->add(std::move(connect));
+	}
+	{
+		auto connect{ std::make_unique<stages::Connect>(
+			 "elbow motion", stages::Connect::GroupPlannerVector{ { "panda_arm", pipeline } }) };
+		connect->setCostTerm(std::make_unique<cost::LinkMotion>("panda_link4"));
+		alternatives->add(std::move(connect));
+	}
+
+	t.add(std::move(alternatives));
+
+	auto goal_scene{ scene->diff() };
+	goal_scene->getCurrentStateNonConst().setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "ready");
+	auto goal = std::make_unique<stages::FixedState>("goal");
+	goal->setState(goal_scene);
+	t.add(std::move(goal));
+
+	try {
+		t.plan(0);
+	} catch (const InitStageException& e) {
+		std::cout << e << std::endl;
+	}
+
+	ros::spin();
+
+	return 0;
+}

demo/src/ik_clearance_cost.cpp

@@ -0,0 +1,74 @@
+#include <ros/ros.h>
+
+#include <moveit/planning_scene/planning_scene.h>
+
+#include <moveit/task_constructor/task.h>
+
+#include <moveit/task_constructor/stages/fixed_state.h>
+#include <moveit/task_constructor/stages/compute_ik.h>
+
+#include <moveit/task_constructor/cost_terms.h>
+
+using namespace moveit::task_constructor;
+
+/* ComputeIK(FixedState) */
+int main(int argc, char** argv) {
+	ros::init(argc, argv, "mtc_tutorial");
+	ros::NodeHandle nh{ "~" };
+
+	ros::AsyncSpinner spinner{ 1 };
+	spinner.start();
+
+	Task t;
+	t.stages()->setName("clearance IK");
+
+	// announce new task (in case previous run was restarted)
+	ros::Duration(0.5).sleep();
+
+	t.loadRobotModel();
+	assert(t.getRobotModel()->getName() == "panda");
+
+	auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
+	auto& robot_state = scene->getCurrentStateNonConst();
+	robot_state.setToDefaultValues();
+	robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended");
+
+	moveit_msgs::CollisionObject co;
+	co.id = "obstacle";
+	co.primitives.emplace_back();
+	co.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
+	co.primitives[0].dimensions.resize(1);
+	co.primitives[0].dimensions[0] = 0.1;
+	co.header.frame_id = "world";
+	co.primitive_poses.emplace_back();
+	co.primitive_poses[0].orientation.w = 1.0;
+	co.primitive_poses[0].position.z = 0.85;
+	scene->processCollisionObjectMsg(co);
+
+	auto initial = std::make_unique<stages::FixedState>();
+	initial->setState(scene);
+
+	auto ik = std::make_unique<stages::ComputeIK>();
+	ik->insert(std::move(initial));
+	ik->setGroup("panda_arm");
+	ik->setTargetPose(Eigen::Translation3d(.3, 0, .35) * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()));
+	ik->setTimeout(1.0);
+	ik->setMaxIKSolutions(100);
+
+	auto cl_cost{ std::make_unique<cost::Clearance>() };
+	cl_cost->cumulative = nh.param("cumulative", false);  // sum up pairwise distances?
+	cl_cost->with_world = nh.param("with_world", true);  // consider distance to world objects?
+	ik->setCostTerm(std::move(cl_cost));
+
+	t.add(std::move(ik));
+
+	try {
+		t.plan(0);
+	} catch (const InitStageException& e) {
+		std::cout << e << std::endl;
+	}
+
+	ros::waitForShutdown();
+
+	return 0;
+}

demo/src/pick_place_demo.cpp

@@ -99,8 +99,8 @@ moveit_msgs::CollisionObject createObject() {
 }
 
 int main(int argc, char** argv) {
-	ROS_INFO_NAMED(LOGNAME, "Init moveit_task_constructor_demo");
-	ros::init(argc, argv, "moveit_task_constructor_demo");
+	ROS_INFO_NAMED(LOGNAME, "Init node");
+	ros::init(argc, argv, "mtc_tutorial");
 	ros::NodeHandle nh;
 	ros::AsyncSpinner spinner(1);
 	spinner.start();