diff --git a/config/config.yaml b/config/config.yaml index a18ba849e3714314ed726f9e2780839dc1cae3ae..1c84f392d70e116356398ac6185f8bd6233310d0 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,12 +1,14 @@ panda_mqtt_connector: server: "tcp://localhost:1883" + robot_speed_factor: .15 + robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path" topics: robotConfig: "robotconfig" dataConfig: "dataconfig" zone_size: 0.5 zones: - "1 1" - - "-1 -1 1" + - "0 1" parts: panda: Link0: "panda::panda_link0" diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp index 7bb35446c5d1f5bf5a827cbf46bbae61a95a2c4f..9404808bc650c3584d0014c9a77330cedca68a7a 100644 --- a/src/TimedPlanner.cpp +++ b/src/TimedPlanner.cpp @@ -24,7 +24,6 @@ namespace PlannerState { const double default_velocity = 1.0; const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH; - bool isInitialized = false; bool isLooping = true; } @@ -176,7 +175,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo } // wake up if we have trajectory - while (PlannerState::raw_trajectory.size() == 0) { + while (PlannerState::raw_trajectory.empty()) { if (newTrajectoryAvailable) { updateRawTrajectory(node_handle, &group); i = 0; @@ -185,7 +184,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo } double velocity = 0.0; - std::string planning_mode = ""; + std::string planning_mode; if (!node_handle.getParam("robot_speed_factor", velocity)) { velocity = PlannerState::default_velocity; @@ -198,7 +197,7 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity); moveit::planning_interface::MoveGroupInterface::Plan plan; - PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, velocity); + PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, 0.8, velocity); group.execute(plan); // make sure the robot moves in an infinite circle diff --git a/src/util/TrajectoryUtil.cpp b/src/util/TrajectoryUtil.cpp index c3423a9b0a6ff2be7c8bbcf40aea1c152ed7e713..88c70d7600fb4d8a1f08ec36883f2fc80f1a012b 100644 --- a/src/util/TrajectoryUtil.cpp +++ b/src/util/TrajectoryUtil.cpp @@ -8,118 +8,104 @@ 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, - std::string pathType, double velocity) -{ - if(velocity < 0.1 || velocity > 1.0){ - ROS_ERROR("Invalid velocity configured. "); - return false; - } - - if (pathType.compare(CARTESIAN_PATH) == 0) - { - if(!computeCartesianPath(group, plan, targetPose)) - { - return false; - }else{ - manipulateVelocity(velocity, group, plan, CARTESIAN_PATH); - return true; - } - } - - if(pathType.compare(FLUID_PATH) == 0) - { - group.setStartStateToCurrentState(); - group.setPlanningTime(10.0); - - group.setPoseTarget(targetPose); - manipulateVelocity(velocity, group, plan, FLUID_PATH); - return (group.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - } - - ROS_ERROR("Invalid type of path provided."); - + 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; -} - -bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose) -{ - std::vector<double> x_values; - std::vector<double> y_values; - std::vector<double> z_values; - - // check if no trajectory is configured - if(!node_handle.getParam("trajectory_pos_x", x_values) || - !node_handle.getParam("trajectory_pos_y", y_values) || - !node_handle.getParam("trajectory_pos_z", z_values) ) - { - return false; - }else{ - for(int i = 0; i < x_values.size(); i++) - { - geometry_msgs::Pose pose = initialPose; - pose.position.x = x_values.at(i); - pose.position.y = y_values.at(i); - pose.position.z = z_values.at(i); - waypoints.push_back(pose); - } - - node_handle.setParam("new_trajectory_available", false); - return true; - } -} + } + 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; + } -bool TrajectoryUtil::computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group, - moveit::planning_interface::MoveGroupInterface::Plan &plan, - const geometry_msgs::Pose &targetPose) { + 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}; - group.setStartStateToCurrentState(); - group.setPlanningTime(10.0); moveit_msgs::RobotTrajectory trajectory_msg; - double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, false); + 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; + trajectory_processing::IterativeParabolicTimeParameterization iptp{}; - bool success = iptp.computeTimeStamps(rt); - ROS_INFO("Computed time stamp %s", success ? "SUCCEDED" : "FAILED"); + ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s"); - if(!success){ - return false; + if (iptp.computeTimeStamps(rt)) { + ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s"); } - rt.getRobotTrajectoryMsg(trajectory_msg); - ROS_INFO("Planned cartesian plan (%.2f%% acheived)", fraction * 100.0); - plan.trajectory_ = trajectory_msg; + 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); +} + +bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, + geometry_msgs::Pose &initialPose) { + std::vector<double> x_values; + std::vector<double> y_values; + std::vector<double> z_values; + + // check if no trajectory is configured + if (!node_handle.getParam("trajectory_pos_x", x_values) || + !node_handle.getParam("trajectory_pos_y", y_values) || + !node_handle.getParam("trajectory_pos_z", z_values)) { + return false; + } else { + for (int i = 0; i < x_values.size(); i++) { + geometry_msgs::Pose pose = initialPose; + pose.position.x = x_values.at(i); + pose.position.y = y_values.at(i); + pose.position.z = z_values.at(i); + waypoints.push_back(pose); + } + + node_handle.setParam("new_trajectory_available", false); return true; + } } -void TrajectoryUtil::manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group, - moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType) -{ - if(pathType.compare(FLUID_PATH)) - { - group.setMaxVelocityScalingFactor(velocity); +void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) { + ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << "joints"); + for (auto &point : plan.trajectory_.joint_trajectory.points) { + point.time_from_start *= 1 / velocity; + for (double &v : point.velocities) { + v *= velocity; } - if(pathType.compare(CARTESIAN_PATH)) - { - for (int i = 0; i < plan.trajectory_.joint_trajectory.points.size(); i++) { - plan.trajectory_.joint_trajectory.points[i].time_from_start.operator*=((1 / velocity)); - for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).velocities.size(); j++) { - if (plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) != 0.0) { - plan.trajectory_.joint_trajectory.points.at(i).velocities.at(j) *= velocity; - } - } - - for (int j = 0; j < plan.trajectory_.joint_trajectory.points.at(i).accelerations.size(); j++) { - plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j) = - sqrt(std::abs(plan.trajectory_.joint_trajectory.points.at(i).accelerations.at(j))); - } - } + for (double &acceleration : point.accelerations) { + acceleration = sqrt(std::abs(acceleration)); } + } + } diff --git a/src/util/TrajectoryUtil.h b/src/util/TrajectoryUtil.h index 0569c2200ee077eb6395ce614a22f4fa19a989a6..15ea8fbf601612887990c8d730f2acfce030615f 100644 --- a/src/util/TrajectoryUtil.h +++ b/src/util/TrajectoryUtil.h @@ -18,23 +18,21 @@ class TrajectoryUtil { public: - static const std::string CARTESIAN_PATH; - static const std::string FLUID_PATH; + static const std::string CARTESIAN_PATH; + static const std::string FLUID_PATH; - bool computePathToPose(moveit::planning_interface::MoveGroupInterface &group, - moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose, - std::string pathType, double velocity); + 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, double motionSpeedFactor); - bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose); + bool initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, + geometry_msgs::Pose &initialPose); private: - bool computeCartesianPath(moveit::planning_interface::MoveGroupInterface &group, - moveit::planning_interface::MoveGroupInterface::Plan &plan, - const geometry_msgs::Pose &targetPose); - - void manipulateVelocity(double velocity, moveit::planning_interface::MoveGroupInterface &group, - moveit::planning_interface::MoveGroupInterface::Plan &plan, std::string pathType); + void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity); }; #endif //PANDA_SIMULATION_TRAJECTORYUTIL_H \ No newline at end of file