Robowflex  v0.1
Making MoveIt Easy
fetch_tabletop_goalpose.cpp
Go to the documentation of this file.
1 /* Author: Carlos Quintero Pena*/
2 
6 #include <robowflex_library/io.h>
11 
12 using namespace robowflex;
13 
14 /* \file fetch_tabletop_goalpose.cpp
15  * A simple script that shows how to use TrajOpt to plan in a manipulation task. The scene and request are
16  * loaded from yaml files. Instead of providing a goal state, a goal pose for the end effector is encoded as a
17  * cartesian constraint. An RVIZ Helper object is used to visualize the start/end states and the computed
18  * trajectory.
19  */
20 
21 static const std::string GROUP = "arm";
22 
23 int main(int argc, char **argv)
24 {
25  // Startup ROS
26  ROS ros(argc, argv);
27 
28  // Create the default Fetch robot.
29  auto fetch = std::make_shared<FetchRobot>();
30  fetch->initialize(false);
31  const auto &ee = fetch->getModel()->getEndEffectors()[0]->getLinkModelNames()[0];
32 
33  // Load tabletop scene.
34  auto scene = std::make_shared<Scene>(fetch);
35  scene->fromYAMLFile("package://robowflex_tesseract/scenes/table/scene.yaml");
36 
37  // Attach object to end effector.
38  scene->attachObject(*fetch->getScratchState(), "Can1");
39 
40  // Create a TrajOpt planner for Fetch.
41  auto planner = std::make_shared<TrajOptPlanner>(fetch, GROUP);
42  planner->initialize("torso_lift_link", "gripper_link");
43 
44  // Set planner parameters.
45  planner->options.num_waypoints = 10; // Select number of waypoints in trajectory.
46  planner->options.joint_vel_coeffs = 20.0; // Set weights for velocity costs.
47 
48  // Load request.
49  const auto &request = std::make_shared<MotionRequestBuilder>(fetch);
50  request->fromYAMLFile("package://robowflex_tesseract/scenes/table/request.yaml");
51  const auto &start_state = request->getStartConfiguration();
52  const auto &goal_state = request->getGoalConfiguration();
53  const auto &goal_ee_pose = goal_state->getFrameTransform(ee);
54 
55  // RVIZ helper.
56  const auto &rviz = std::make_shared<IO::RVIZHelper>(fetch);
57  rviz->updateScene(scene);
58  rviz->visualizeState(start_state);
59 
60  RBX_INFO("Visualizing start state");
61  RBX_INFO("Press Enter to continue");
62  std::cin.ignore();
63 
64  // Do motion planning using a goal pose for the end effector.
65  auto result = planner->plan(scene, start_state, goal_ee_pose, ee);
66  if (result.first)
67  rviz->updateTrajectory(planner->getTrajectory());
68 
69  rviz->visualizeState(planner->getTrajectory()->getLastWayPointPtr());
70 
71  RBX_INFO("Visualizing end state");
72  RBX_INFO("Press Enter to exit");
73  std::cin.ignore();
74 
75  return 0;
76 }
RAII-pattern for starting up ROS.
Definition: util.h:52
int main(int argc, char **argv)
static const std::string GROUP
#define RBX_INFO(fmt,...)
Output a info logging message.
Definition: log.h:118
Main namespace. Contains all library classes and functions.
Definition: scene.cpp:25
Functions for loading and animating scenes in Blender.