Skip to content
Snippets Groups Projects
Select Git revision
  • master default protected
1 result

TrajectoryUtil.cpp

Blame
  • TrajectoryUtil.cpp 4.31 KiB
    //
    // Created by sebastian on 08.04.20.
    //
    
    #include "TrajectoryUtil.h"
    
    const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path";
    const std::string TrajectoryUtil::FLUID_PATH = "fluid_path";
    
    bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group,
                                           moveit::planning_interface::MoveGroupInterface::Plan &plan,
                                           geometry_msgs::Pose targetPose,
                                           const std::string& pathType, double maxVelocityFactor,
                                           double maxAccelerationFactor, double motionSpeedFactor) {
      if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) {
        ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1].");
        return false;
      }
      if (maxAccelerationFactor <= 0 || maxAccelerationFactor > 1.0) {
        ROS_ERROR_STREAM("Invalid maximum acceleration scaling factor " << maxAccelerationFactor << ". Must be in (0,1].");
        return false;
      }
      if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) {
        ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1].");
        return false;
      }
    
      group.setStartStateToCurrentState();
      group.setPlanningTime(10.0);
      group.setPoseTarget(targetPose);
      group.setMaxVelocityScalingFactor(maxVelocityFactor);
      group.setMaxAccelerationScalingFactor(maxAccelerationFactor);
    
      if (pathType == CARTESIAN_PATH) {
        std::vector<geometry_msgs::Pose> single_traj{targetPose};
    
        moveit_msgs::RobotTrajectory trajectory_msg;
        double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, true);
        ROS_INFO("Planned cartesian plan (%.2f%% achieved)", fraction * 100.0);
    
        robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
        rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
        trajectory_processing::IterativeParabolicTimeParameterization iptp{};
    
        ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s");
    
        if (iptp.computeTimeStamps(rt)) {
          ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s");
        }
    
        if (iptp.computeTimeStamps(rt, maxVelocityFactor)) {
          ROS_INFO("Computed time stamps.");
          rt.getRobotTrajectoryMsg(trajectory_msg);
          plan.trajectory_ = trajectory_msg;
    
          ROS_ERROR_STREAM("After second time parameterization, the trajectory takes " << rt.getDuration() << "s");
        } else {
          ROS_ERROR("Computation of cartesian path has failed.");
          return false;
        }
      } else if (pathType == FLUID_PATH) {
        if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) {
          ROS_ERROR("Computation of fluid path has failed.");
        }
      } else {
        ROS_ERROR("Invalid type of path provided.");
        return false;
      }
    
      manipulateVelocity(plan, motionSpeedFactor);