TrajectoryUtil.h

//
// Created by sebastian on 08.04.20.
//

#ifndef PANDA_SIMULATION_TRAJECTORYUTIL_H
#define PANDA_SIMULATION_TRAJECTORYUTIL_H

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>

class TrajectoryUtil {

public:

  constexpr static const char* const CARTESIAN_PATH = "cartesian_path";
  constexpr static const char* const FLUID_PATH = "fluid_path";

  static bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
                         moveit::planning_interface::MoveGroupInterface::Plan &plan,
                         geometry_msgs::Pose targetPose,
                         const std::string &pathType, double maxVelocityFactor,
                         double maxAccelerationFactor);

  static void applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity);

};

#endif //PANDA_SIMULATION_TRAJECTORYUTIL_H