Working version

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)
@@ -77,4 +70,28 @@ getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajecto
  */
 void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& drake_trajectory, const double delta_t,
                         std::shared_ptr<::robot_trajectory::RobotTrajectory>& moveit_trajectory);
+
+/**
+ * @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);
+
+/**
+ * @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

@@ -33,12 +33,11 @@
  *********************************************************************/
 
 /* Author: Sebastian Jahr
-*/
+ */
 
 #include <moveit/drake/conversions.hpp>
 namespace moveit::drake
 {
-
 using ::drake::geometry::SceneGraph;
 using ::drake::multibody::AddMultibodyPlantSceneGraph;
 using ::drake::multibody::MultibodyPlant;
@@ -71,6 +70,9 @@ getPiecewisePolynomial(const ::robot_trajectory::RobotTrajectory& robot_trajecto
 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);
@@ -82,8 +84,8 @@ void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& drake_t
     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->getRobotModel()->getActiveJointModels();
-    for (size_t joint_index = 0; joint_index < active_joints.size(); ++joint_index)
+    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));
@@ -93,4 +95,35 @@ void getRobotTrajectory(const ::drake::trajectories::Trajectory<double>& drake_t
     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();
+    std::cout << "Joint name: " << joint_name << " Joint index: " << joint_index << std::endl;
+    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

src/ktopt_planning_context.cpp

@@ -1,5 +1,6 @@
 #include <cmath>
 
+#include <moveit/drake/conversions.hpp>
 #include <moveit/robot_state/conversions.h>
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/constraint_samplers/constraint_sampler_manager.h>
@@ -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,29 @@ 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<const MultibodyPlant<double>&>(diagram_->GetSubsystemByName("plant"));
-  const auto& scene_graph = dynamic_cast<const SceneGraph<double>&>(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();
+
+  // Print all joint names
+  // for (const auto& joint : joints)
+  //{
+  //  RCLCPP_INFO_STREAM(getLogger(), "Joint name: " << joint->getName());
+  //}
 
   // 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::getJointPositions(start_state, getGroupName(), plant);
+  q << moveit::drake::getJointVelocities(start_state, getGroupName(), plant);
 
   // drake accepts a VectorX<T>
   auto& plant_context = diagram_->GetMutableSubsystemContext(plant, diagram_context_.get());
@@ -88,11 +91,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::getJointPositions(start_state, getGroupName(), plant),
+                                    moveit::drake::getJointPositions(start_state, getGroupName(), plant), 0.0);
+  trajopt.AddPathVelocityConstraint(moveit::drake::getJointVelocities(start_state, getGroupName(), plant),
+                                    moveit::drake::getJointVelocities(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::getJointPositions(goal_state, getGroupName(), plant),
+                                    moveit::drake::getJointPositions(goal_state, getGroupName(), plant), 1.0);
+  trajopt.AddPathVelocityConstraint(moveit::drake::getJointVelocities(goal_state, getGroupName(), plant),
+                                    moveit::drake::getJointVelocities(goal_state, getGroupName(), plant), 1.0);
 
   // TODO: Add constraints on joint position/velocity/acceleration
   // trajopt.AddPositionBounds(
@@ -132,33 +139,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<robot_trajectory::RobotTrajectory>(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<robot_trajectory::RobotTrajectory>(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<size_t>(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<double>(i) / static_cast<double>(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<moveit::core::RobotState>(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<int>(params_.trajectory_time_step * 10000.0)));
   }
@@ -192,7 +188,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 +233,6 @@ void KTOptPlanningContext::transcribePlanningScene(const planning_scene::Plannin
   {
     RCLCPP_ERROR_STREAM(getLogger(), "caught exception ... " << e.what());
   }
-  auto& plant = dynamic_cast<MultibodyPlant<double>&>(builder->GetMutableSubsystemByName("plant"));
   auto& scene_graph = dynamic_cast<SceneGraph<double>&>(builder->GetMutableSubsystemByName("scene_graph"));
   for (const auto& object : planning_scene.getWorld()->getObjectIds())
   {
@@ -252,12 +247,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 +324,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!");