diff --git a/CMakeLists.txt b/CMakeLists.txt index ea97967..4f45c84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,9 +43,12 @@ include_directories(include) add_library( moveit_drake SHARED # KTOPT - src/ktopt_planner_manager.cpp src/ktopt_planning_context.cpp + src/ktopt_planner_manager.cpp + src/ktopt_planning_context.cpp # TOPPRA - src/add_toppra_time_parameterization.cpp) + src/add_toppra_time_parameterization.cpp + # Conversions + src/conversions.cpp) ament_target_dependencies(moveit_drake rclcpp pluginlib moveit_core moveit_msgs EIGEN3) diff --git a/demo/src/pipeline_testbench_main.cpp b/demo/src/pipeline_testbench_main.cpp index 6b030b9..3af307f 100644 --- a/demo/src/pipeline_testbench_main.cpp +++ b/demo/src/pipeline_testbench_main.cpp @@ -23,7 +23,6 @@ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("pipeline_testbench"); const std::string PLANNING_GROUP = "panda_arm"; -const std::vector CONTROLLERS(1, "panda_arm_controller"); const std::vector SENSED_SCENE_NAMES = { "bookshelf_small_panda//scene_sensed0001.yaml", "bookshelf_tall_panda//scene_sensed0001.yaml", "bookshelf_thin_panda//scene_sensed0001.yaml", "cage_panda//scene_sensed0001.yaml", @@ -355,7 +354,7 @@ int main(int argc, char** argv) pipeline_testbench::Demo demo(node); - for (const auto& scene_name : ALL_SCENE_NAMES) + for (const auto& scene_name : SCENE_NAMES) { if (!demo.loadPlanningSceneAndQuery(scene_name)) { diff --git a/include/ktopt_interface/ktopt_planning_context.hpp b/include/ktopt_interface/ktopt_planning_context.hpp index 4fc0060..4041621 100644 --- a/include/ktopt_interface/ktopt_planning_context.hpp +++ b/include/ktopt_interface/ktopt_planning_context.hpp @@ -76,9 +76,6 @@ class KTOptPlanningContext : public planning_interface::PlanningContext void clear() override; void setRobotDescription(std::string robot_description); - VectorXd toDrakePositions(const moveit::core::RobotState& state, const Joints& joints); - void setJointPositions(const VectorXd& values, const Joints& joints, moveit::core::RobotState& state); - void setJointVelocities(const VectorXd& values, const Joints& joints, moveit::core::RobotState& state); void transcribePlanningScene(const planning_scene::PlanningScene& planning_scene); private: diff --git a/include/moveit/drake/conversions.hpp b/include/moveit/drake/conversions.hpp index 623f588..4065da4 100644 --- a/include/moveit/drake/conversions.hpp +++ b/include/moveit/drake/conversions.hpp @@ -48,13 +48,6 @@ #include namespace moveit::drake { - -using ::drake::geometry::SceneGraph; -using ::drake::multibody::AddMultibodyPlantSceneGraph; -using ::drake::multibody::MultibodyPlant; -using ::drake::multibody::Parser; -using ::drake::systems::DiagramBuilder; - /** * @brief Create a Piecewise Polynomial from a moveit trajectory (see * https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_polynomial.html) @@ -65,58 +58,40 @@ using ::drake::systems::DiagramBuilder; */ [[nodiscard]] ::drake::trajectories::PiecewisePolynomial getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajectory, - const moveit::core::JointModelGroup* group) -{ - std::vector breaks; - breaks.reserve(robot_trajectory.getWayPointCount()); - std::vector samples; - samples.reserve(robot_trajectory.getWayPointCount()); - - // Create samples & breaks - for (std::size_t i = 0; i < robot_trajectory.getWayPointCount(); ++i) - { - const auto& state = robot_trajectory.getWayPoint(i); - Eigen::VectorXd position(state.getVariableCount()); - state.copyJointGroupPositions(group, position); - samples.emplace_back(position); - breaks.emplace_back(robot_trajectory.getWayPointDurationFromStart(i)); - } - - // Create a piecewise polynomial trajectory - return ::drake::trajectories::PiecewisePolynomial::FirstOrderHold(breaks, samples); -} + const moveit::core::JointModelGroup* group); /** * @brief Create a moveit trajectory from a piecewise polynomial. Assumes that the piecewise polynomial describes a * joint trajectory for every active joint of the given trajectory. * - * @param piecewise_polynomial Drake trajectory + * @param drake_trajectory Drake trajectory * @param delta_t Time step size - * @param output_trajectory MoveIt trajectory to be populated based on the piecewise polynomial + * @param moveit_trajectory MoveIt trajectory to be populated based on the piecewise polynomial */ -void getRobotTrajectory(const ::drake::trajectories::PiecewisePolynomial& piecewise_polynomial, - const double delta_t, std::shared_ptr<::robot_trajectory::RobotTrajectory>& output_trajectory) -{ - // Get the start and end times of the piecewise polynomial - double t_prev = 0.0; - const auto num_pts = static_cast(std::ceil(piecewise_polynomial.end_time() / delta_t) + 1); +void getRobotTrajectory(const ::drake::trajectories::Trajectory& drake_trajectory, const double delta_t, + std::shared_ptr<::robot_trajectory::RobotTrajectory>& moveit_trajectory); - for (unsigned int i = 0; i < num_pts; ++i) - { - const auto t_scale = static_cast(i) / static_cast(num_pts - 1); - const auto t = std::min(t_scale, 1.0) * piecewise_polynomial.end_time(); - const auto pos_val = piecewise_polynomial.value(t); - const auto vel_val = piecewise_polynomial.EvalDerivative(t); - const auto waypoint = std::make_shared(output_trajectory->getRobotModel()); - const auto active_joints = output_trajectory->getRobotModel()->getActiveJointModels(); - for (size_t joint_index = 0; joint_index < active_joints.size(); ++joint_index) - { - waypoint->setJointPositions(active_joints[joint_index], &pos_val(joint_index)); - waypoint->setJointVelocities(active_joints[joint_index], &vel_val(joint_index)); - } +/** + * @brief Get a joint positions vector for a MultibodyPlant from a MoveIt robot state + * + * @param moveit_state MoveIt Robot state + * @param group_name Name of the joint group + * @param plant Drake Multibody Plant + * @return Vector with drake joint positions + */ +[[nodiscard]] Eigen::VectorXd getJointPositionVector(const moveit::core::RobotState& moveit_state, + const std::string& group_name, + const ::drake::multibody::MultibodyPlant& plant); - output_trajectory->addSuffixWayPoint(waypoint, t - t_prev); - t_prev = t; - } -} +/** + * @brief Get a joint velocities vector for a MultibodyPlant from a MoveIt robot state + * + * @param moveit_state MoveIt Robot state + * @param group_name Name of the joint group + * @param plant Drake Multibody Plant + * @return Vector with drake joint velocities + */ +[[nodiscard]] Eigen::VectorXd getJointVelocityVector(const moveit::core::RobotState& moveit_state, + const std::string& group_name, + const ::drake::multibody::MultibodyPlant& plant); } // namespace moveit::drake diff --git a/src/add_toppra_time_parameterization.cpp b/src/add_toppra_time_parameterization.cpp index 6d70fbe..e3cb9fd 100644 --- a/src/add_toppra_time_parameterization.cpp +++ b/src/add_toppra_time_parameterization.cpp @@ -91,7 +91,7 @@ class AddToppraTimeParameterization : public planning_interface::PlanningRespons // TODO(sjahr) Replace with subscribed robot description const char* ModelUrl = "package://drake_models/franka_description/" - "urdf/panda_arm.urdf"; + "urdf/panda_arm_hand.urdf"; const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl); Parser(&plant, &scene_graph).AddModels(urdf); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0")); @@ -136,15 +136,9 @@ class AddToppraTimeParameterization : public planning_interface::PlanningRespons ///////////////////////////////////// auto& plant = dynamic_cast&>(diagram_->GetSubsystemByName("plant")); auto& plant_context = diagram_->GetMutableSubsystemContext(plant, diagram_context_.get()); - const auto& current_state = planning_scene->getCurrentState(); - Eigen::VectorXd q_pos = Eigen::VectorXd::Zero(joint_model_group->getActiveVariableCount()); - Eigen::VectorXd q_vel = Eigen::VectorXd::Zero(joint_model_group->getActiveVariableCount()); - Eigen::VectorXd q = Eigen::VectorXd::Zero(2 * joint_model_group->getActiveVariableCount()); - - current_state.copyJointGroupPositions(joint_model_group, q_pos); - current_state.copyJointGroupVelocities(joint_model_group, q_vel); - q << q_pos; - q << q_vel; + Eigen::VectorXd q = Eigen::VectorXd::Zero(plant.num_positions() + plant.num_velocities()); + q << moveit::drake::getJointPositionVector(res.trajectory->getWayPoint(0), joint_model_group->getName(), plant); + q << moveit::drake::getJointVelocityVector(res.trajectory->getWayPoint(0), joint_model_group->getName(), plant); plant.SetPositionsAndVelocities(&plant_context, q); // Create drake::trajectories::Trajectory from moveit trajectory diff --git a/src/conversions.cpp b/src/conversions.cpp new file mode 100644 index 0000000..f1f55ac --- /dev/null +++ b/src/conversions.cpp @@ -0,0 +1,127 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + */ + +#include +namespace moveit::drake +{ +using ::drake::geometry::SceneGraph; +using ::drake::multibody::AddMultibodyPlantSceneGraph; +using ::drake::multibody::MultibodyPlant; +using ::drake::multibody::Parser; +using ::drake::systems::DiagramBuilder; + +[[nodiscard]] ::drake::trajectories::PiecewisePolynomial +getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajectory, + const moveit::core::JointModelGroup* group) +{ + std::vector breaks; + breaks.reserve(robot_trajectory.getWayPointCount()); + std::vector samples; + samples.reserve(robot_trajectory.getWayPointCount()); + + // Create samples & breaks + for (std::size_t i = 0; i < robot_trajectory.getWayPointCount(); ++i) + { + const auto& state = robot_trajectory.getWayPoint(i); + Eigen::VectorXd position(state.getVariableCount()); + state.copyJointGroupPositions(group, position); + samples.emplace_back(position); + breaks.emplace_back(robot_trajectory.getWayPointDurationFromStart(i)); + } + + // Create a piecewise polynomial trajectory + return ::drake::trajectories::PiecewisePolynomial::FirstOrderHold(breaks, samples); +} + +void getRobotTrajectory(const ::drake::trajectories::Trajectory& drake_trajectory, const double delta_t, + std::shared_ptr<::robot_trajectory::RobotTrajectory>& moveit_trajectory) +{ + // Reset output trajectory + moveit_trajectory->clear(); + + // Get the start and end times of the piecewise polynomial + double t_prev = 0.0; + const auto num_pts = static_cast(std::ceil(drake_trajectory.end_time() / delta_t) + 1); + + for (unsigned int i = 0; i < num_pts; ++i) + { + const auto t_scale = static_cast(i) / static_cast(num_pts - 1); + const auto t = std::min(t_scale, 1.0) * drake_trajectory.end_time(); + const auto pos_val = drake_trajectory.value(t); + const auto vel_val = drake_trajectory.EvalDerivative(t); + const auto waypoint = std::make_shared(moveit_trajectory->getRobotModel()); + const auto active_joints = moveit_trajectory->getGroup()->getActiveJointModels(); + for (size_t joint_index = 0; joint_index < active_joints.size(); joint_index++) + { + waypoint->setJointPositions(active_joints[joint_index], &pos_val(joint_index)); + waypoint->setJointVelocities(active_joints[joint_index], &vel_val(joint_index)); + } + + moveit_trajectory->addSuffixWayPoint(waypoint, t - t_prev); + t_prev = t; + } +} + +[[nodiscard]] Eigen::VectorXd getJointPositionVector(const moveit::core::RobotState& moveit_state, + const std::string& group_name, const MultibodyPlant& plant) +{ + Eigen::VectorXd joint_positions = Eigen::VectorXd::Zero(plant.num_positions()); + + const auto& joint_model_group = moveit_state.getRobotModel()->getJointModelGroup(group_name); + for (const auto& joint_model : joint_model_group->getActiveJointModels()) + { + const auto& joint_name = joint_model->getName(); + const auto& joint_index = plant.GetJointByName(joint_name).ordinal(); + joint_positions(joint_index) = moveit_state.getVariablePosition(joint_name); + } + return joint_positions; +} + +[[nodiscard]] Eigen::VectorXd getJointVelocityVector(const moveit::core::RobotState& moveit_state, + const std::string& group_name, const MultibodyPlant& plant) +{ + Eigen::VectorXd joint_velocities = Eigen::VectorXd::Zero(plant.num_velocities()); + const auto& joint_model_group = moveit_state.getRobotModel()->getJointModelGroup(group_name); + for (const auto& joint_model : joint_model_group->getActiveJointModels()) + { + const auto& joint_name = joint_model->getName(); + const auto& joint_index = plant.GetJointByName(joint_name).ordinal(); + joint_velocities(joint_index) = moveit_state.getVariableVelocity(joint_name); + } + return joint_velocities; +} +} // namespace moveit::drake diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp index 11d8a17..617a173 100644 --- a/src/ktopt_planning_context.cpp +++ b/src/ktopt_planning_context.cpp @@ -1,5 +1,6 @@ #include +#include #include #include #include @@ -24,7 +25,7 @@ KTOptPlanningContext::KTOptPlanningContext(const std::string& name, const std::s // Do some drake initialization may be } -void KTOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) +void KTOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& /*res*/) { RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse&) is not implemented!"); @@ -35,27 +36,23 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) { RCLCPP_INFO(getLogger(), "Setting up and solving optimization problem ..."); // preliminary house keeping - const auto time_start = std::chrono::steady_clock::now(); res.planner_id = std::string("ktopt"); res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; // dome drake related scope initialisations const auto& plant = dynamic_cast&>(diagram_->GetSubsystemByName("plant")); - const auto& scene_graph = dynamic_cast&>(diagram_->GetSubsystemByName("scene_graph")); // Retrieve motion plan request const auto& req = getMotionPlanRequest(); const moveit::core::RobotState start_state(*getPlanningScene()->getCurrentStateUpdated(req.start_state)); - const auto group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); + const auto joint_model_group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName()); RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName()); - const auto& joints = group->getActiveJointModels(); + const auto& joints = joint_model_group->getActiveJointModels(); // q represents the complete state (joint positions and velocities) - const auto q_p = toDrakePositions(start_state, joints); - VectorXd q_v = VectorXd::Zero(joints.size()); - VectorXd q = VectorXd::Zero(2 * joints.size()); - q << q_p; - q << q_v; + VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities()); + q << moveit::drake::getJointPositionVector(start_state, getGroupName(), plant); + q << moveit::drake::getJointVelocityVector(start_state, getGroupName(), plant); // drake accepts a VectorX auto& plant_context = diagram_->GetMutableSubsystemContext(plant, diagram_context_.get()); @@ -88,11 +85,15 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) // Constraints // Add constraints on start joint configuration and velocity - trajopt.AddPathPositionConstraint(toDrakePositions(start_state, joints), toDrakePositions(start_state, joints), 0.0); - trajopt.AddPathVelocityConstraint(VectorXd::Zero(joints.size()), VectorXd::Zero(joints.size()), 0.0); + trajopt.AddPathPositionConstraint(moveit::drake::getJointPositionVector(start_state, getGroupName(), plant), + moveit::drake::getJointPositionVector(start_state, getGroupName(), plant), 0.0); + trajopt.AddPathVelocityConstraint(moveit::drake::getJointVelocityVector(start_state, getGroupName(), plant), + moveit::drake::getJointVelocityVector(start_state, getGroupName(), plant), 0.0); // Add constraint on end joint configuration and velocity - trajopt.AddPathPositionConstraint(toDrakePositions(goal_state, joints), toDrakePositions(goal_state, joints), 1.0); - trajopt.AddPathVelocityConstraint(VectorXd::Zero(joints.size()), VectorXd::Zero(joints.size()), 1.0); + trajopt.AddPathPositionConstraint(moveit::drake::getJointPositionVector(goal_state, getGroupName(), plant), + moveit::drake::getJointPositionVector(goal_state, getGroupName(), plant), 1.0); + trajopt.AddPathVelocityConstraint(moveit::drake::getJointVelocityVector(goal_state, getGroupName(), plant), + moveit::drake::getJointVelocityVector(goal_state, getGroupName(), plant), 1.0); // TODO: Add constraints on joint position/velocity/acceleration // trajopt.AddPositionBounds( @@ -132,33 +133,22 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) // package up the resulting trajectory auto traj = trajopt.ReconstructTrajectory(collision_free_result); - res.trajectory = std::make_shared(start_state.getRobotModel(), group); - res.trajectory->clear(); - const auto& active_joints = res.trajectory->getGroup() ? res.trajectory->getGroup()->getActiveJointModels() : - res.trajectory->getRobotModel()->getActiveJointModels(); + res.trajectory = std::make_shared(start_state.getRobotModel(), joint_model_group); - // sanity check - assert(traj.rows() == active_joints.size()); + moveit::drake::getRobotTrajectory(traj, params_.trajectory_time_step, res.trajectory); + // Visualize the trajectory with Meshcat visualizer_->StartRecording(); double t_prev = 0.0; const auto num_pts = static_cast(std::ceil(traj.end_time() / params_.trajectory_time_step) + 1); - for (int i = 0; i < num_pts; ++i) + for (unsigned int i = 0; i < num_pts; ++i) { const auto t_scale = static_cast(i) / static_cast(num_pts - 1); const auto t = std::min(t_scale, 1.0) * traj.end_time(); - const auto pos_val = traj.value(t); - const auto vel_val = traj.EvalDerivative(t); - const auto waypoint = std::make_shared(start_state); - setJointPositions(pos_val, active_joints, *waypoint); - setJointVelocities(vel_val, active_joints, *waypoint); - res.trajectory->addSuffixWayPoint(waypoint, t - t_prev); - t_prev = t; - - plant.SetPositions(&plant_context, pos_val); + plant.SetPositions(&plant_context, traj.value(t)); auto& vis_context = visualizer_->GetMyContextFromRoot(*diagram_context_); visualizer_->ForcedPublish(vis_context); - + t_prev = t; // Without these sleeps, the visualizer won't give you time to load your browser std::this_thread::sleep_for(std::chrono::milliseconds(static_cast(params_.trajectory_time_step * 10000.0))); } @@ -192,7 +182,7 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description) // HACK: For now loading directly from drake's package map const char* ModelUrl = "package://drake_models/franka_description/" - "urdf/panda_arm.urdf"; + "urdf/panda_arm_hand.urdf"; const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl); auto robot_instance = Parser(&plant, &scene_graph).AddModels(urdf); plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0")); @@ -237,7 +227,6 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin { RCLCPP_ERROR_STREAM(getLogger(), "caught exception ... " << e.what()); } - auto& plant = dynamic_cast&>(builder->GetMutableSubsystemByName("plant")); auto& scene_graph = dynamic_cast&>(builder->GetMutableSubsystemByName("scene_graph")); for (const auto& object : planning_scene.getWorld()->getObjectIds()) { @@ -252,12 +241,11 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin RCLCPP_WARN(getLogger(), "Octomap not supported for now ... "); continue; } - for (int i = 0; i < collision_object->shapes_.size(); ++i) + for (size_t i = 0; i < collision_object->shapes_.size(); ++i) { std::string shape_name = object + std::to_string(i); const auto& shape = collision_object->shapes_[i]; const auto& pose = collision_object->pose_; - const auto& shape_pose = collision_object->shape_poses_[i]; const auto& shape_type = collision_object->shapes_[i]->type; switch (shape_type) @@ -330,36 +318,6 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin } } -VectorXd KTOptPlanningContext::toDrakePositions(const moveit::core::RobotState& state, const Joints& joints) -{ - VectorXd q = VectorXd::Zero(joints.size()); - for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index) - { - q[joint_index] = *state.getJointPositions(joints[joint_index]); - } - return q; -} - -void KTOptPlanningContext::setJointPositions(const VectorXd& values, const Joints& joints, - moveit::core::RobotState& state) -{ - for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index) - { - state.setJointPositions(joints[joint_index], &values[joint_index]); - } - return; -} - -void KTOptPlanningContext::setJointVelocities(const VectorXd& values, const Joints& joints, - moveit::core::RobotState& state) -{ - for (size_t joint_index = 0; joint_index < joints.size(); ++joint_index) - { - state.setJointVelocities(joints[joint_index], &values[joint_index]); - } - return; -} - void KTOptPlanningContext::clear() { RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::clear() is not implemented!");