ur5_custom_planning.cpp File Reference

#include <robowflex_library/detail/ur5.h>
#include <robowflex_library/io/visualization.h>
#include <robowflex_library/log.h>
#include <robowflex_library/planning.h>
#include <robowflex_library/robot.h>
#include <robowflex_library/scene.h>
#include <robowflex_library/trajectory.h>
#include <robowflex_library/util.h>
#include <robowflex_tesseract/conversions.h>
#include <robowflex_tesseract/trajopt_planner.h>

Go to the source code of this file.

Classes
class	CustomTrajOptPlanner
struct	CustomTrajOptPlanner::CartCnt

Functions
int	main (int argc, char **argv)

Variables
static const std::string	GROUP = "manipulator"

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 86 of file ur5_custom_planning.cpp.

{
    // Startup ROS
    ROS ros(argc, argv);
 
    // Create the default UR5 robot.
    auto ur5 = std::make_shared<UR5Robot>();
    ur5->initialize();
 
    // Set start state and get the ee pose.
    ur5->setGroupState(GROUP, {0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0});
    const auto &ee = ur5->getModel()->getEndEffectors()[0]->getLinkModelNames()[0];
    RobotPose pose = ur5->getLinkTF(ee);
 
    // Create an RViz visualization helper.
    IO::RVIZHelper rviz(ur5);
    RBX_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)...");
    std::cin.get();
 
    // Create and visualize scene.
    auto scene = std::make_shared<Scene>(ur5);
    scene->getCurrentState() = *ur5->getScratchState();
    rviz.updateScene(scene);
 
    // Create a custom TrajOpt planner
    auto planner = std::make_shared<CustomTrajOptPlanner>(ur5, GROUP);
    planner->initialize(GROUP);
    planner->options.num_waypoints = 17;  // Set number of waypoints in trajectory.
 
    // Define desired motion of the end-effector.
    EigenSTL::vector_Vector3d directions = {
        {0.0, -0.3, 0.0}, {0.0, 0.0, -0.2}, {0.0, 0.3, 0.0}, {0.0, 0.0, 0.2}};
 
    // Define a Cartesian constraint for each location.
    for (size_t i = 0; i < directions.size(); ++i)
    {
        CustomTrajOptPlanner::CartCnt term;
        pose.translate(directions[i]);
        term.pose = pose;
        term.timestep = (i + 1) * 4;
        term.link = ee;
        term.pos_coeffs = 1.0;
        term.rot_coeffs = 1.0;
 
        planner->constraints_.push_back(term);
    }
 
    // Do planning with cartesian constraints.
    auto response = planner->plan(scene, ur5->getScratchState());
    if (!response.first)
    {
        RBX_INFO("Optimization did not converge");
        return 0;
    }
 
    // Publish the trajectory to a topic to display in RViz
    Trajectory trajectory(planner->getTrajectory());
    rviz.updateTrajectory(trajectory);
 
    // Visualize the final pose.
    scene->getCurrentState() = *ur5->getScratchState();
    rviz.updateScene(scene);
 
    RBX_INFO("Press enter to exit.");
    std::cin.get();
 
    return 0;
}

Variable Documentation

GROUP

const std::string GROUP = "manipulator"

static

Definition at line 20 of file ur5_custom_planning.cpp.