diff --git a/config/config.yaml b/config/config.yaml index 48ae64d65812c1e8799d614911e26da7afea9511..8f88ed54dc19e176d0d8735a031dd0d6d0e0ef43 100644 --- a/config/config.yaml +++ b/config/config.yaml @@ -1,15 +1,15 @@ panda_mqtt_connector: server: "tcp://localhost:1883" - robot_speed_factor: .95 + robot_speed_factor: .5 robot_planning_mode: "fluid_path" # "fluid_path" or "cartesian_path" - default_trajectory: "square" # "square" or "circle", everything else is ignored + default_trajectory: "circle" # "square" or "circle", everything else is ignored topics: robotConfig: "robotconfig" dataConfig: "dataconfig" zone_size: 0.5 zones: - "1 1" - - "0 1" + - "-1 -1 1" parts: panda: Link0: "panda::panda_link0" diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp index beae1b83d32c2bf5e5f866603a8c54223f83aad9..cc19a23ed8dc67600d1030988f07c940d9a079d6 100644 --- a/src/TimedPlanner.cpp +++ b/src/TimedPlanner.cpp @@ -23,7 +23,6 @@ namespace PlannerState { const double default_velocity = 1.0; const std::string default_planning_mode = TrajectoryUtil::FLUID_PATH; - const bool useCircle = true; bool isLooping = true; @@ -106,6 +105,30 @@ bool loadWaypoints(std::vector<geometry_msgs::Pose> &waypoints, const ros::NodeH } } +static std::vector<moveit_msgs::RobotTrajectory> split(moveit::planning_interface::MoveGroupInterface &group, const moveit::planning_interface::MoveGroupInterface::Plan &plan) { + + std::vector<moveit_msgs::RobotTrajectory> trajectories; + + auto currentState = group.getCurrentState(); + auto robotModel = currentState->getRobotModel(); + + robot_trajectory::RobotTrajectory rt(robotModel, "panda_arm"); + rt.setRobotTrajectoryMsg(*currentState, plan.start_state_, plan.trajectory_); + + for (int wpIndex = 0; wpIndex < rt.getWayPointCount() - 1; ++wpIndex) { + robot_trajectory::RobotTrajectory wpTrajectory{robotModel, "panda_arm"}; + moveit::core::RobotState waypointState{robotModel}; + wpTrajectory.insertWayPoint(0, rt.getWayPoint(wpIndex), 0); + wpTrajectory.insertWayPoint(1, rt.getWayPoint(wpIndex+1), rt.getWayPointDurationFromPrevious(wpIndex+1)); + moveit_msgs::RobotTrajectory wpTrajectoryMsg{}; + wpTrajectory.getRobotTrajectoryMsg(wpTrajectoryMsg); + trajectories.push_back(wpTrajectoryMsg); + } + + return trajectories; + +} + void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::MoveGroupInterface &group) { bool newTrajectoryAvailable = false; @@ -129,24 +152,33 @@ void doMotion(const ros::NodeHandle &node_handle, moveit::planning_interface::Mo node_handle.getParam("new_trajectory_available", newTrajectoryAvailable); node_handle.getParam("loop_trajectory", PlannerState::isLooping); - - double velocity; - if (!node_handle.getParam("robot_speed_factor", velocity)) { - velocity = PlannerState::default_velocity; - } - std::string planning_mode; if (!node_handle.getParam("robot_planning_mode", planning_mode)) { planning_mode = PlannerState::default_planning_mode; } - ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode << " with velocity " << velocity); + ROS_INFO_STREAM("Planning the next trajectory in " << planning_mode); moveit::planning_interface::MoveGroupInterface::Plan plan; if (PlannerState::traj_util.computePathToPose(group, plan, PlannerState::raw_trajectory.at(i), planning_mode, 0.8, - 0.8, velocity)) { - group.execute(plan); + 0.8)) { + for (auto trajectory : split(group, plan)) { + double motionSpeedFactor; + if (!node_handle.getParam("robot_speed_factor", motionSpeedFactor)) { + motionSpeedFactor = PlannerState::default_velocity; + } + if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) { + ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1]."); + return; + } + ROS_INFO_STREAM("Moving to the next waypoint with speed factor " << motionSpeedFactor); + TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor); + moveit_msgs::MoveItErrorCodes errorCode = group.execute(trajectory); + if (errorCode.val != errorCode.SUCCESS) { + ROS_ERROR_STREAM("Error Code from executing segment: " << errorCode); + } + } } else { ROS_ERROR_STREAM("The planner was unable to compute a path to the pose with the given settings."); } diff --git a/src/util/TrajectoryUtil.cpp b/src/util/TrajectoryUtil.cpp index ad9e8c7277a99f330382267270859b424f0c4262..cf710476969a827c237b1d96b9ca844a3cde2b7d 100644 --- a/src/util/TrajectoryUtil.cpp +++ b/src/util/TrajectoryUtil.cpp @@ -11,7 +11,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose, const std::string& pathType, double maxVelocityFactor, - double maxAccelerationFactor, double motionSpeedFactor) { + double maxAccelerationFactor) { if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) { ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1]."); return false; @@ -20,10 +20,6 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte 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); @@ -67,14 +63,11 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte ROS_ERROR("Invalid type of path provided."); return false; } - - manipulateVelocity(plan, motionSpeedFactor); return true; } -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) { +void TrajectoryUtil::applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity) { + for (auto &point : trajectory.joint_trajectory.points) { point.time_from_start *= 1 / velocity; for (double &v : point.velocities) { v *= velocity; @@ -84,5 +77,4 @@ void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInt acceleration = sqrt(std::abs(acceleration)); } } - } diff --git a/src/util/TrajectoryUtil.h b/src/util/TrajectoryUtil.h index e978c7a3c1445132a2e043d694a25dd232fd7165..fef5dabc5a0c64472304c5045c95a534363b540b 100644 --- a/src/util/TrajectoryUtil.h +++ b/src/util/TrajectoryUtil.h @@ -25,11 +25,11 @@ public: moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose, const std::string &pathType, double maxVelocityFactor, - double maxAccelerationFactor, double motionSpeedFactor); + double maxAccelerationFactor); + static void applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity); private: - static void manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity); }; #endif //PANDA_SIMULATION_TRAJECTORYUTIL_H \ No newline at end of file