23 int main(
int argc,
char **argv)
29 auto fetch = std::make_shared<FetchRobot>();
30 fetch->initialize(
false);
31 const auto &ee =
fetch->getModel()->getEndEffectors()[0]->getLinkModelNames()[0];
35 scene->fromYAMLFile(
"package://robowflex_tesseract/scenes/table/scene.yaml");
38 scene->attachObject(*
fetch->getScratchState(),
"Can1");
41 auto planner = std::make_shared<TrajOptPlanner>(
fetch,
GROUP);
42 planner->initialize(
"torso_lift_link",
"gripper_link");
45 planner->options.num_waypoints = 10;
46 planner->options.joint_vel_coeffs = 20.0;
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);
56 const auto &rviz = std::make_shared<IO::RVIZHelper>(
fetch);
57 rviz->updateScene(
scene);
58 rviz->visualizeState(start_state);
65 auto result = planner->plan(
scene, start_state, goal_ee_pose, ee);
67 rviz->updateTrajectory(planner->getTrajectory());
69 rviz->visualizeState(planner->getTrajectory()->getLastWayPointPtr());
RAII-pattern for starting up ROS.
int main(int argc, char **argv)
static const std::string GROUP
#define RBX_INFO(fmt,...)
Output a info logging message.
Main namespace. Contains all library classes and functions.
Functions for loading and animating scenes in Blender.