Go to the source code of this file.
|
int | main (int argc, char **argv) |
|
◆ main()
int main |
( |
int |
argc, |
|
|
char ** |
argv |
|
) |
| |
Definition at line 17 of file ur5_pool.cpp.
23 auto ur5 = std::make_shared<UR5Robot>();
27 auto scene = std::make_shared<Scene>(ur5);
30 auto planner = std::make_shared<PoolPlanner>(ur5);
35 request.setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0});
38 pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313});
39 Eigen::Quaterniond orn{0, 0, 1, 0};
41 request.setGoalRegion(
"ee_link",
"world",
43 orn, {0.01, 0.01, 0.01}
48 if (res.
error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
52 auto job1 = planner->submit(
scene, request.getRequest());
53 auto job2 = planner->submit(
scene, request.getRequest());
54 auto job3 = planner->submit(
scene, request.getRequest());
55 auto job4 = planner->submit(
scene, request.getRequest());
56 auto job5 = planner->submit(
scene, request.getRequest());
57 auto job6 = planner->submit(
scene, request.getRequest());
58 auto job7 = planner->submit(
scene, request.getRequest());
59 auto job8 = planner->submit(
scene, request.getRequest());
A helper class to build motion planning requests for a robowflex::Planner.
Convenience class for the default motion planning pipeline for UR5.
RAII-pattern for starting up ROS.
std::shared_ptr< dart::dynamics::SphereShape > makeSphere(double radius)
Create a sphere.
std::decay< decltype(std::declval< moveit::core::Transforms >().getTransform("")) >::type RobotPose
A pose (point in SE(3)) used in various functions. Defined from what MoveIt! uses.
Functions for loading and animating scenes in Blender.
moveit_msgs::MoveItErrorCodes error_code_