Add LeggedSelfCollisionVisualization

legged_controllers/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED
         legged_estimation
         controller_interface
         ocs2_legged_robot_ros
+        ocs2_self_collision_visualization
         angles
         )
 
@@ -32,6 +33,7 @@ catkin_package(
         legged_estimation
         controller_interface
         ocs2_legged_robot_ros
+        ocs2_self_collision_visualization
         angles
 )
 

legged_controllers/include/legged_controllers/LeggedController.h

@@ -18,6 +18,7 @@
 #include <legged_wbc/WbcBase.h>
 
 #include "legged_controllers/SafetyChecker.h"
+#include "legged_controllers/visualization/LeggedSelfCollisionVisualization.h"
 
 namespace legged {
 using namespace ocs2;
@@ -52,18 +53,18 @@ class LeggedController : public controller_interface::MultiInterfaceController<H
   std::shared_ptr<CentroidalModelRbdConversions> rbdConversions_;
   std::shared_ptr<StateEstimateBase> stateEstimate_;
 
-  std::shared_ptr<LeggedRobotVisualizer> visualizer_;
+  std::shared_ptr<LeggedRobotVisualizer> robotVisualizer_;
+  std::shared_ptr<LeggedSelfCollisionVisualization> selfCollisionVisualization_;
   ros::Publisher observationPublisher_;
 
   SystemObservation currentObservation_;
   std::vector<HybridJointHandle> hybridJointHandles_;
 
-  benchmark::RepeatedTimer mpcTimer_;
-  benchmark::RepeatedTimer wbcTimer_;
-
  private:
   std::thread mpcThread_;
   std::atomic_bool controllerRunning_{}, mpcRunning_{};
+  benchmark::RepeatedTimer mpcTimer_;
+  benchmark::RepeatedTimer wbcTimer_;
 };
 
 class LeggedCheaterController : public LeggedController {

legged_controllers/include/legged_controllers/visualization/LeggedSelfCollisionVisualization.h

@@ -0,0 +1,39 @@
+//
+// Created by qiayuan on 23-1-30.
+//
+
+#pragma once
+#include <ros/ros.h>
+
+#include <ocs2_self_collision_visualization/GeometryInterfaceVisualization.h>
+
+#include <utility>
+
+namespace legged {
+
+using namespace ocs2;
+
+class LeggedSelfCollisionVisualization : public GeometryInterfaceVisualization {
+ public:
+  LeggedSelfCollisionVisualization(PinocchioInterface pinocchioInterface, PinocchioGeometryInterface geometryInterface,
+                                   const CentroidalModelPinocchioMapping& mapping, ros::NodeHandle& nh, scalar_t maxUpdateFrequency = 50.0)
+      : mappingPtr_(mapping.clone()),
+        GeometryInterfaceVisualization(std::move(pinocchioInterface), std::move(geometryInterface), nh, "odom"),
+        lastTime_(std::numeric_limits<scalar_t>::lowest()),
+        minPublishTimeDifference_(1.0 / maxUpdateFrequency) {}
+  void update(const SystemObservation& observation) {
+    if (observation.time - lastTime_ > minPublishTimeDifference_) {
+      lastTime_ = observation.time;
+
+      publishDistances(mappingPtr_->getPinocchioJointPosition(observation.state));
+    }
+  }
+
+ private:
+  std::unique_ptr<CentroidalModelPinocchioMapping> mappingPtr_;
+
+  scalar_t lastTime_;
+  scalar_t minPublishTimeDifference_;
+};
+
+}  // namespace legged

legged_controllers/package.xml

@@ -18,6 +18,8 @@
 
     <depend>controller_interface</depend>
     <depend>ocs2_legged_robot_ros</depend>
+    <depend>ocs2_self_collision_visualization</depend>
+
     <depend>angles</depend>
 
     <exec_depend>controller_manager</exec_depend>

legged_controllers/src/LeggedController.cpp

@@ -39,14 +39,15 @@ bool LeggedController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHand
   setupLeggedInterface(taskFile, urdfFile, referenceFile, verbose);
   setupMpc();
   setupMrt();
-
   // Visualization
   ros::NodeHandle nh;
   CentroidalModelPinocchioMapping pinocchioMapping(leggedInterface_->getCentroidalModelInfo());
   eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(leggedInterface_->getPinocchioInterface(), pinocchioMapping,
                                                                       leggedInterface_->modelSettings().contactNames3DoF);
-  visualizer_ = std::make_shared<LeggedRobotVisualizer>(leggedInterface_->getPinocchioInterface(),
-                                                        leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh);
+  robotVisualizer_ = std::make_shared<LeggedRobotVisualizer>(leggedInterface_->getPinocchioInterface(),
+                                                             leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh);
+  selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(),
+                                                                         leggedInterface_->getGeometryInterface(), pinocchioMapping, nh));
 
   // Hardware interface
   auto* hybridJointInterface = robot_hw->get<HybridJointInterface>();
@@ -143,7 +144,8 @@ void LeggedController::update(const ros::Time& time, const ros::Duration& period
   }
 
   // Visualization
-  visualizer_->update(currentObservation_, mpcMrtInterface_->getPolicy(), mpcMrtInterface_->getCommand());
+  robotVisualizer_->update(currentObservation_, mpcMrtInterface_->getPolicy(), mpcMrtInterface_->getCommand());
+  selfCollisionVisualization_->update(currentObservation_);
 
   // Publish the observation. Only needed for the command interface
   observationPublisher_.publish(ros_msg_conversions::createObservationMsg(currentObservation_));

legged_interface/include/legged_interface/LeggedInterface.h

@@ -17,6 +17,7 @@
 #include <ocs2_pinocchio_interface/PinocchioInterface.h>
 #include <ocs2_robotic_tools/common/RobotInterface.h>
 #include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
+#include <ocs2_self_collision/PinocchioGeometryInterface.h>
 #include <ocs2_sqp/SqpSettings.h>
 
 #include <ocs2_legged_robot/common/ModelSettings.h>
@@ -49,6 +50,7 @@ class LeggedInterface : public RobotInterface {
   const RolloutBase& getRollout() const { return *rolloutPtr_; }
   PinocchioInterface& getPinocchioInterface() { return *pinocchioInterfacePtr_; }
   const CentroidalModelInfo& getCentroidalModelInfo() const { return centroidalModelInfo_; }
+  PinocchioGeometryInterface& getGeometryInterface() { return *geometryInterfacePtr_; }
   std::shared_ptr<SwitchedModelReferenceManager> getSwitchedModelReferenceManagerPtr() const { return referenceManagerPtr_; }
 
   const Initializer& getInitializer() const override { return *initializerPtr_; }
@@ -86,6 +88,7 @@ class LeggedInterface : public RobotInterface {
 
   std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
   CentroidalModelInfo centroidalModelInfo_;
+  std::unique_ptr<PinocchioGeometryInterface> geometryInterfacePtr_;
 
   std::unique_ptr<OptimalControlProblem> problemPtr_;
   std::shared_ptr<SwitchedModelReferenceManager> referenceManagerPtr_;

legged_interface/include/legged_interface/constraint/LeggedSelfCollisionConstraint.h

@@ -15,7 +15,7 @@ using namespace legged_robot;
 
 class LeggedSelfCollisionConstraint final : public SelfCollisionConstraint {
  public:
-  LeggedSelfCollisionConstraint(const PinocchioStateInputMapping<scalar_t>& mapping, PinocchioGeometryInterface pinocchioGeometryInterface,
+  LeggedSelfCollisionConstraint(const CentroidalModelPinocchioMapping& mapping, PinocchioGeometryInterface pinocchioGeometryInterface,
                                 scalar_t minimumDistance)
       : SelfCollisionConstraint(mapping, std::move(pinocchioGeometryInterface), minimumDistance) {}
   ~LeggedSelfCollisionConstraint() override = default;

legged_interface/src/LeggedInterface.cpp

@@ -356,15 +356,15 @@ std::unique_ptr<StateCost> LeggedInterface::getSelfCollisionConstraint(const Pin
   loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionObjectPairs", collisionObjectPairs, verbose);
   loadData::loadStdVectorOfPair(taskFile, prefix + ".collisionLinkPairs", collisionLinkPairs, verbose);
 
-  PinocchioGeometryInterface geometryInterface(pinocchioInterface, collisionLinkPairs, collisionObjectPairs);
+  geometryInterfacePtr_ = std::make_unique<PinocchioGeometryInterface>(pinocchioInterface, collisionLinkPairs, collisionObjectPairs);
   if (verbose) {
     std::cerr << " #### =============================================================================\n";
-    const size_t numCollisionPairs = geometryInterface.getNumCollisionPairs();
+    const size_t numCollisionPairs = geometryInterfacePtr_->getNumCollisionPairs();
     std::cerr << "SelfCollision: Testing for " << numCollisionPairs << " collision pairs\n";
   }
 
   std::unique_ptr<StateConstraint> constraint = std::make_unique<LeggedSelfCollisionConstraint>(
-      CentroidalModelPinocchioMapping(centroidalModelInfo_), std::move(geometryInterface), minimumDistance);
+      CentroidalModelPinocchioMapping(centroidalModelInfo_), *geometryInterfacePtr_, minimumDistance);
 
   auto penalty = std::make_unique<RelaxedBarrierPenalty>(RelaxedBarrierPenalty::Config{mu, delta});