meshcat vis (#53)

* Add contrained planning tutorial * Make it work * Update limit compution * Re-enable jerk bounds * Remove unused variables * Updates and small bug fix * Fix issues by making kinematics limits finite and adding padding to constraints * adds a parameter for meshcat visualisation --------- Co-authored-by: Sebastian Jahr <[email protected]> Co-authored-by: Sebastian Castro <[email protected]>
moveit · Oct 13, 2024 · fec4b0a · fec4b0a
1 parent 6851f8a
commit fec4b0a
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 16 deletions.
diff --git a/res/ktopt_moveit_parameters.yaml b/res/ktopt_moveit_parameters.yaml
@@ -108,3 +108,11 @@ ktopt_interface:
       gt_eq<>: [0.0]
     }
   }
+  meshcat_visualise: {
+    type: bool,
+    description: "Whether to visualise the Drake scene grpah trajectory in Meshcat.",
+    default_value: false,
+    validation: {
+      gt_eq<>: [0.0]
+    }
+  }
diff --git a/src/ktopt_planning_context.cpp b/src/ktopt_planning_context.cpp
@@ -159,22 +159,27 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
   moveit::drake::getRobotTrajectory(traj, params_.trajectory_time_step, plant, res.trajectory);
 
   // Visualize the trajectory with Meshcat
-  // visualizer_->StartRecording();
-  // const auto num_pts = static_cast<size_t>(std::ceil(traj.end_time() / params_.trajectory_time_step) + 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) * traj.end_time();
-  //   plant.SetPositions(&plant_context, traj.value(t));
-  //   auto& vis_context = visualizer_->GetMyContextFromRoot(*diagram_context_);
-  //   visualizer_->ForcedPublish(vis_context);
-  //   // Without these sleeps, the visualizer won't give you time to load your
-  //   // browser
-  //   // TODO: This should not hold up planning time
-  //   std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(params_.trajectory_time_step * 10000.0)));
-  // }
-  // visualizer_->StopRecording();
-  // visualizer_->PublishRecording();
+
+  // TODO: add to conversions
+  if (params_.meshcat_visualise)
+  {
+    visualizer_->StartRecording();
+    const auto num_pts = static_cast<size_t>(std::ceil(traj.end_time() / params_.trajectory_time_step) + 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) * traj.end_time();
+      plant.SetPositions(&plant_context, traj.value(t));
+      auto& vis_context = visualizer_->GetMyContextFromRoot(*diagram_context_);
+      visualizer_->ForcedPublish(vis_context);
+      // Without these sleeps, the visualizer won't give you time to load your
+      // browser
+      // TODO: This should not hold up planning time
+      std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(params_.trajectory_time_step * 10000.0)));
+    }
+    visualizer_->StopRecording();
+    visualizer_->PublishRecording();
+  }
   res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
   return;
 }