diff --git a/legged_controllers/CMakeLists.txt b/legged_controllers/CMakeLists.txt index dd1faa62..e664f240 100644 --- a/legged_controllers/CMakeLists.txt +++ b/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 ) diff --git a/legged_controllers/include/legged_controllers/LeggedController.h b/legged_controllers/include/legged_controllers/LeggedController.h index 7fea6456..c9b1a147 100644 --- a/legged_controllers/include/legged_controllers/LeggedController.h +++ b/legged_controllers/include/legged_controllers/LeggedController.h @@ -18,6 +18,7 @@ #include #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 rbdConversions_; std::shared_ptr stateEstimate_; - std::shared_ptr visualizer_; + std::shared_ptr robotVisualizer_; + std::shared_ptr selfCollisionVisualization_; ros::Publisher observationPublisher_; SystemObservation currentObservation_; std::vector 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 { diff --git a/legged_controllers/include/legged_controllers/visualization/LeggedSelfCollisionVisualization.h b/legged_controllers/include/legged_controllers/visualization/LeggedSelfCollisionVisualization.h new file mode 100644 index 00000000..51f7f9dc --- /dev/null +++ b/legged_controllers/include/legged_controllers/visualization/LeggedSelfCollisionVisualization.h @@ -0,0 +1,39 @@ +// +// Created by qiayuan on 23-1-30. +// + +#pragma once +#include + +#include + +#include + +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::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 mappingPtr_; + + scalar_t lastTime_; + scalar_t minPublishTimeDifference_; +}; + +} // namespace legged diff --git a/legged_controllers/package.xml b/legged_controllers/package.xml index 4b8936d3..9c10e70c 100644 --- a/legged_controllers/package.xml +++ b/legged_controllers/package.xml @@ -18,6 +18,8 @@ controller_interface ocs2_legged_robot_ros + ocs2_self_collision_visualization + angles controller_manager diff --git a/legged_controllers/src/LeggedController.cpp b/legged_controllers/src/LeggedController.cpp index 2f1ef53a..78e8814a 100644 --- a/legged_controllers/src/LeggedController.cpp +++ b/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(leggedInterface_->getPinocchioInterface(), pinocchioMapping, leggedInterface_->modelSettings().contactNames3DoF); - visualizer_ = std::make_shared(leggedInterface_->getPinocchioInterface(), - leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh); + robotVisualizer_ = std::make_shared(leggedInterface_->getPinocchioInterface(), + leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh); + selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(), + leggedInterface_->getGeometryInterface(), pinocchioMapping, nh)); // Hardware interface auto* hybridJointInterface = robot_hw->get(); @@ -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_)); diff --git a/legged_interface/include/legged_interface/LeggedInterface.h b/legged_interface/include/legged_interface/LeggedInterface.h index a0012e74..751c1b64 100644 --- a/legged_interface/include/legged_interface/LeggedInterface.h +++ b/legged_interface/include/legged_interface/LeggedInterface.h @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -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 getSwitchedModelReferenceManagerPtr() const { return referenceManagerPtr_; } const Initializer& getInitializer() const override { return *initializerPtr_; } @@ -86,6 +88,7 @@ class LeggedInterface : public RobotInterface { std::unique_ptr pinocchioInterfacePtr_; CentroidalModelInfo centroidalModelInfo_; + std::unique_ptr geometryInterfacePtr_; std::unique_ptr problemPtr_; std::shared_ptr referenceManagerPtr_; diff --git a/legged_interface/include/legged_interface/constraint/LeggedSelfCollisionConstraint.h b/legged_interface/include/legged_interface/constraint/LeggedSelfCollisionConstraint.h index 57eb948e..46e13aa1 100644 --- a/legged_interface/include/legged_interface/constraint/LeggedSelfCollisionConstraint.h +++ b/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& mapping, PinocchioGeometryInterface pinocchioGeometryInterface, + LeggedSelfCollisionConstraint(const CentroidalModelPinocchioMapping& mapping, PinocchioGeometryInterface pinocchioGeometryInterface, scalar_t minimumDistance) : SelfCollisionConstraint(mapping, std::move(pinocchioGeometryInterface), minimumDistance) {} ~LeggedSelfCollisionConstraint() override = default; diff --git a/legged_interface/src/LeggedInterface.cpp b/legged_interface/src/LeggedInterface.cpp index cc3339c7..67ab95a0 100644 --- a/legged_interface/src/LeggedInterface.cpp +++ b/legged_interface/src/LeggedInterface.cpp @@ -356,15 +356,15 @@ std::unique_ptr 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(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 constraint = std::make_unique( - CentroidalModelPinocchioMapping(centroidalModelInfo_), std::move(geometryInterface), minimumDistance); + CentroidalModelPinocchioMapping(centroidalModelInfo_), *geometryInterfacePtr_, minimumDistance); auto penalty = std::make_unique(RelaxedBarrierPenalty::Config{mu, delta});