Include panda arm hand

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
+  # Conversations
+  src/conversions.cpp)
 
 ament_target_dependencies(moveit_drake rclcpp pluginlib moveit_core moveit_msgs
                           EIGEN3)

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<std::string> CONTROLLERS(1, "panda_arm_controller");
 const std::vector<std::string> 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))
     {

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:

include/moveit/drake/conversions.hpp

@@ -48,13 +48,6 @@
 #include <drake/common/trajectories/piecewise_polynomial.h>
 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<double>
 getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajectory,
-                       const moveit::core::JointModelGroup* group)
-{
-  std::vector<double> breaks;
-  breaks.reserve(robot_trajectory.getWayPointCount());
-  std::vector<Eigen::MatrixXd> 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<double>::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<double>& 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<size_t>(std::ceil(piecewise_polynomial.end_time() / delta_t) + 1);
+void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& 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<double>(i) / static_cast<double>(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<moveit::core::RobotState>(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 getJointPositions(const moveit::core::RobotState& moveit_state,
+                                                const std::string& group_name,
+                                                const ::drake::multibody::MultibodyPlant<double>& 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 getJointVelocities(const moveit::core::RobotState& moveit_state,
+                                                 const std::string& group_name,
+                                                 const ::drake::multibody::MultibodyPlant<double>& plant);
 }  // namespace moveit::drake

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"));
@@ -137,14 +137,9 @@ class AddToppraTimeParameterization : public planning_interface::PlanningRespons
     auto& plant = dynamic_cast<const MultibodyPlant<double>&>(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;
+    VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities());
+    q << moveit::drake::getJointPositions(current_state, getGroupName(), plant);
+    q << moveit::drake::getJointVelocities(current_state, getGroupName(), plant);
     plant.SetPositionsAndVelocities(&plant_context, q);
 
     // Create drake::trajectories::Trajectory from moveit trajectory

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 <moveit/drake/conversions.hpp>
+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<double>
+getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajectory,
+                       const moveit::core::JointModelGroup* group)
+{
+  std::vector<double> breaks;
+  breaks.reserve(robot_trajectory.getWayPointCount());
+  std::vector<Eigen::MatrixXd> 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<double>::FirstOrderHold(breaks, samples);
+}
+
+void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& 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<size_t>(std::ceil(drake_trajectory.end_time() / delta_t) + 1);
+
+  for (unsigned int i = 0; i < num_pts; ++i)
+  {
+    const auto t_scale = static_cast<double>(i) / static_cast<double>(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::core::RobotState>(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 getJointPositions(const moveit::core::RobotState& moveit_state,
+                                                const std::string& group_name, const MultibodyPlant<double>& 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 getJointVelocities(const moveit::core::RobotState& moveit_state,
+                                                 const std::string& group_name, const MultibodyPlant<double>& 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