|
| 1 | +#include <example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp> |
| 2 | + |
| 3 | +namespace |
| 4 | +{ |
| 5 | +const auto kLogger = rclcpp::get_logger("ConvertMtcSolutionToJointTrajectory"); |
| 6 | + |
| 7 | +// Port names for input and output ports. |
| 8 | +constexpr auto kPortIDSolution = "solution"; |
| 9 | +constexpr auto kPortIDJointGroup = "joint_group"; |
| 10 | +constexpr auto kPortIDJointTrajectory = "joint_trajectory"; |
| 11 | +constexpr auto kPortIDVelocityScalingFactor = "velocity_scaling_factor"; |
| 12 | +constexpr auto kPortIDAccelerationScalingFactor = "acceleration_scaling_factor"; |
| 13 | +constexpr auto kPortIDSamplingRate = "sampling_rate"; |
| 14 | +} // namespace |
| 15 | + |
| 16 | +namespace example_behaviors |
| 17 | +{ |
| 18 | +ConvertMtcSolutionToJointTrajectory::ConvertMtcSolutionToJointTrajectory( |
| 19 | + const std::string& name, const BT::NodeConfiguration& config, |
| 20 | + const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) |
| 21 | + : moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>(name, config, shared_resources) |
| 22 | +{ |
| 23 | +} |
| 24 | + |
| 25 | +BT::PortsList ConvertMtcSolutionToJointTrajectory::providedPorts() |
| 26 | +{ |
| 27 | + return { |
| 28 | + BT::InputPort<moveit_task_constructor_msgs::msg::Solution>(kPortIDSolution, "{mtc_solution}", |
| 29 | + "MoveIt Task Constructor solution."), |
| 30 | + BT::InputPort<std::string>(kPortIDJointGroup, "manipulator", |
| 31 | + "Joint group name used in the MTC solution."), |
| 32 | + BT::InputPort<double>(kPortIDVelocityScalingFactor, 1.0, |
| 33 | + "Velocity scaling factor for trajectory generation."), |
| 34 | + BT::InputPort<double>(kPortIDAccelerationScalingFactor, 1.0, |
| 35 | + "Acceleration scaling factor for trajectory generation."), |
| 36 | + BT::InputPort<int>(kPortIDSamplingRate, 100, |
| 37 | + "Sampling rate for trajectory generation."), |
| 38 | + BT::OutputPort<trajectory_msgs::msg::JointTrajectory>(kPortIDJointTrajectory, "{joint_trajectory_msg}", |
| 39 | + "Resulting joint trajectory."), |
| 40 | + }; |
| 41 | +} |
| 42 | + |
| 43 | +BT::KeyValueVector ConvertMtcSolutionToJointTrajectory::metadata() |
| 44 | +{ |
| 45 | + return { { "subcategory", "Motion Planning" }, |
| 46 | + { "description", "Extracts joint space trajectories from an MTC solution and adds time parameterization using TOTG." } }; |
| 47 | +} |
| 48 | + |
| 49 | +BT::NodeStatus ConvertMtcSolutionToJointTrajectory::tick() |
| 50 | +{ |
| 51 | + using namespace moveit_studio::behaviors; |
| 52 | + |
| 53 | + // Load data from the behavior input ports. |
| 54 | + const auto solution = getInput<moveit_task_constructor_msgs::msg::Solution>(kPortIDSolution); |
| 55 | + const auto joint_group_name = getInput<std::string>(kPortIDJointGroup); |
| 56 | + const auto velocity_scaling_factor = getInput<double>(kPortIDVelocityScalingFactor); |
| 57 | + const auto acceleration_scaling_factor = getInput<double>(kPortIDAccelerationScalingFactor); |
| 58 | + const auto sampling_rate = getInput<int>(kPortIDSamplingRate); |
| 59 | + |
| 60 | + // Check that the required input data port was set |
| 61 | + if (const auto error = maybe_error(solution, joint_group_name, velocity_scaling_factor, acceleration_scaling_factor, sampling_rate); error) |
| 62 | + { |
| 63 | + spdlog::error("Failed to get required value from input data port: {}", error.value()); |
| 64 | + return BT::NodeStatus::FAILURE; |
| 65 | + } |
| 66 | + // Initialize the robot model loader if this is the first time this Behavior has been run. |
| 67 | + if (robot_model_loader_ == nullptr) |
| 68 | + { |
| 69 | + robot_model_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(shared_resources_->node); |
| 70 | + } |
| 71 | + |
| 72 | + // Get the robot model from the robot model loader. |
| 73 | + const auto robot_model = robot_model_loader_->getModel(); |
| 74 | + if (robot_model == nullptr) |
| 75 | + { |
| 76 | + // Return as a failure case if the robot model loader has no robot model. |
| 77 | + spdlog::error("Failed to load robot model."); |
| 78 | + return BT::NodeStatus::FAILURE; |
| 79 | + } |
| 80 | + |
| 81 | + // Get the JointModelGroup using the joint group name from the input port |
| 82 | + auto joint_model_group = robot_model->getJointModelGroup(joint_group_name.value()); |
| 83 | + if (!joint_model_group) |
| 84 | + { |
| 85 | + spdlog::error("Failed to get JointModelGroup '{}'.", joint_group_name.value()); |
| 86 | + return BT::NodeStatus::FAILURE; |
| 87 | + } |
| 88 | + |
| 89 | + // Extract joint positions from the MTC solution |
| 90 | + const auto& waypoints = extractJointPositions(solution.value()); |
| 91 | + |
| 92 | + // Use trajectory_utils.hpp to create a trajectory |
| 93 | + auto trajectory_result = createTrajectoryFromWaypoints( |
| 94 | + *joint_model_group, waypoints, velocity_scaling_factor.value(), acceleration_scaling_factor.value(), sampling_rate.value()); |
| 95 | + |
| 96 | + if (!trajectory_result) |
| 97 | + { |
| 98 | + spdlog::error("Trajectory generation failed: {}", trajectory_result.error()); |
| 99 | + return BT::NodeStatus::FAILURE; |
| 100 | + } |
| 101 | + |
| 102 | + // Set the output port with the resulting joint trajectory |
| 103 | + setOutput(kPortIDJointTrajectory, trajectory_result.value()); |
| 104 | + |
| 105 | + spdlog::info("Successfully converted MTC Solution to JointTrajectory with {} points.", |
| 106 | + trajectory_result.value().points.size()); |
| 107 | + |
| 108 | + return BT::NodeStatus::SUCCESS; |
| 109 | +} |
| 110 | + |
| 111 | +const std::vector<Eigen::VectorXd>& ConvertMtcSolutionToJointTrajectory::extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution) |
| 112 | +{ |
| 113 | + static std::vector<Eigen::VectorXd> waypoints; |
| 114 | + waypoints.clear(); |
| 115 | + for (size_t sub_traj_idx = 0; sub_traj_idx < solution.sub_trajectory.size(); ++sub_traj_idx) |
| 116 | + { |
| 117 | + const auto& sub_traj = solution.sub_trajectory[sub_traj_idx]; |
| 118 | + const auto& joint_traj = sub_traj.trajectory.joint_trajectory; |
| 119 | + |
| 120 | + if (joint_traj.points.empty()) |
| 121 | + { |
| 122 | + spdlog::warn("Sub-trajectory {} has no points.", sub_traj_idx); |
| 123 | + continue; |
| 124 | + } |
| 125 | + |
| 126 | + spdlog::info("Processing sub-trajectory {} with {} points.", |
| 127 | + sub_traj_idx, joint_traj.points.size()); |
| 128 | + |
| 129 | + for (const auto& point : joint_traj.points) |
| 130 | + { |
| 131 | + Eigen::VectorXd positions(point.positions.size()); |
| 132 | + for (size_t i = 0; i < point.positions.size(); ++i) |
| 133 | + { |
| 134 | + positions[i] = point.positions[i]; |
| 135 | + } |
| 136 | + waypoints.push_back(positions); |
| 137 | + } |
| 138 | + } |
| 139 | + return waypoints; |
| 140 | +} |
| 141 | + |
| 142 | +} // namespace example_behaviors |
0 commit comments