diff --git a/README.md b/README.md index 684ce63..8b6190e 100644 --- a/README.md +++ b/README.md @@ -10,8 +10,9 @@ This repository contains the ros2-control based controllers for the quadruped ro Todo List: - [x] **[2025-02-23]** Add Gazebo Playground -- [ ] OCS2 controller for Gazebo Simulation -- [ ] Refactor FSM and Unitree Guide Controller + - [x] OCS2 controller for Gazebo Simulation + - [x] Refactor FSM and Unitree Guide Controller +- [ ] OCS2 Perceptive locomotion demo Video for Unitree Guide Controller: [![](http://i1.hdslb.com/bfs/archive/310e6208920985ac43015b2da31c01ec15e2c5f9.jpg)](https://www.bilibili.com/video/BV1aJbAeZEuo/) diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index 43768d1..c22562c 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -21,6 +21,11 @@ set(CONTROLLER_INCLUDE_DEPENDS nav_msgs qpoases_colcon ament_index_cpp + + convex_plane_decomposition_msgs + convex_plane_decomposition_ros + grid_map_sdf + ocs2_sphere_approximation ) # find dependencies @@ -58,6 +63,13 @@ add_library(${PROJECT_NAME} SHARED src/control/TargetManager.cpp src/control/CtrlComponent.cpp + src/perceptive_interface/constraint/FootCollisionConstraint.cpp + src/perceptive_interface/constraint/FootPlacementConstraint.cpp + src/perceptive_interface/constraint/SphereSdfConstraint.cpp + src/perceptive_interface/ConvexRegionSelector.cpp + src/perceptive_interface/PerceptiveLeggedInterface.cpp + src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp + src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h index f970b34..e89443b 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h @@ -22,7 +22,7 @@ namespace ocs2::legged_robot { - class LeggedInterface final : public RobotInterface + class LeggedInterface : public RobotInterface { public: LeggedInterface(const std::string& task_file, @@ -35,10 +35,10 @@ namespace ocs2::legged_robot void setupJointNames(const std::vector& joint_names, const std::vector& foot_names); - void setupOptimalControlProblem(const std::string& task_file, - const std::string& urdf_file, - const std::string& reference_file, - bool verbose); + virtual void setupOptimalControlProblem(const std::string& task_file, + const std::string& urdf_file, + const std::string& reference_file, + bool verbose); const OptimalControlProblem& getOptimalControlProblem() const override { return *problem_ptr_; } @@ -66,9 +66,9 @@ namespace ocs2::legged_robot void setupModel(const std::string& task_file, const std::string& urdf_file, const std::string& reference_file); - void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile, - const std::string& referenceFile, - bool verbose); + virtual void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile, + const std::string& referenceFile, + bool verbose); std::shared_ptr loadGaitSchedule(const std::string& file, bool verbose) const; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h new file mode 100644 index 0000000..933b131 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h @@ -0,0 +1,71 @@ +// +// Created by qiayuan on 23-1-2. +// + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace ocs2::legged_robot +{ + class ConvexRegionSelector + { + public: + ConvexRegionSelector(CentroidalModelInfo info, + std::shared_ptr PlanarTerrainPtr, + const EndEffectorKinematics& endEffectorKinematics, size_t numVertices); + + void update(const ModeSchedule& modeSchedule, scalar_t initTime, const vector_t& initState, + TargetTrajectories& targetTrajectories); + + convex_plane_decomposition::PlanarTerrainProjection getProjection(size_t leg, scalar_t time) const; + + convex_plane_decomposition::CgalPolygon2d getConvexPolygon(size_t leg, scalar_t time) const; + + vector3_t getNominalFootholds(size_t leg, scalar_t time) const; + + std::vector getMiddleTimes(size_t leg) const { return middleTimes_[leg]; } + + std::vector getProjections(size_t leg) + { + return feetProjections_[leg]; + } + + std::shared_ptr getPlanarTerrainPtr() { return planarTerrainPtr_; } + + feet_array_t getInitStandFinalTimes() { return initStandFinalTime_; } + + feet_array_t> extractContactFlags(const std::vector& phaseIDsStock) const; + + private: + static std::pair findIndex(size_t index, const std::vector& contactFlagStock); + + vector3_t getNominalFoothold(size_t leg, scalar_t time, const vector_t& initState, + TargetTrajectories& targetTrajectories); + + feet_array_t> feetProjections_; + feet_array_t> convexPolygons_; + + feet_array_t> nominalFootholds_; + feet_array_t> middleTimes_; + + feet_array_t initStandFinalTime_; + + feet_array_t> timeEvents_; + + const CentroidalModelInfo info_; + size_t numVertices_; + + convex_plane_decomposition::PlanarTerrain planarTerrain_; + std::shared_ptr planarTerrainPtr_; + std::unique_ptr> endEffectorKinematicsPtr_; + }; +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h new file mode 100644 index 0000000..d6ca037 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h @@ -0,0 +1,56 @@ +// +// Created by qiayuan on 22-12-27. +// + +#pragma once + +#include +#include +#include +#include + +namespace ocs2::legged_robot +{ + class PerceptiveLeggedInterface final : public LeggedInterface + { + public: + using LeggedInterface::LeggedInterface; + + void setupOptimalControlProblem(const std::string& taskFile, + const std::string& urdfFile, + const std::string& referenceFile, + bool verbose) override; + + void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile, + const std::string& referenceFile, + bool verbose) override; + + void setupPreComputation(const std::string& taskFile, const std::string& urdfFile, + const std::string& referenceFile, + bool verbose); + + std::shared_ptr getSignedDistanceFieldPtr() const + { + return signedDistanceFieldPtr_; + } + + std::shared_ptr getPlanarTerrainPtr() const + { + return planarTerrainPtr_; + } + + std::shared_ptr getPinocchioSphereInterfacePtr() const + { + return pinocchioSphereInterfacePtr_; + } + + size_t getNumVertices() const { return numVertices_; } + + protected: + size_t numVertices_ = 16; + + std::shared_ptr planarTerrainPtr_; + std::shared_ptr signedDistanceFieldPtr_; + std::shared_ptr pinocchioSphereInterfacePtr_; + }; +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h new file mode 100644 index 0000000..97cbe49 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h @@ -0,0 +1,45 @@ +// +// Created by qiayuan on 23-1-1. +// + +#pragma once + +#include + +#include +#include + +#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" +#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h" + +namespace ocs2::legged_robot +{ + /** Callback for caching and reference update */ + class PerceptiveLeggedPrecomputation : public LeggedRobotPreComputation + { + public: + PerceptiveLeggedPrecomputation(PinocchioInterface pinocchioInterface, const CentroidalModelInfo& info, + const SwingTrajectoryPlanner& swingTrajectoryPlanner, ModelSettings settings, + const ConvexRegionSelector& convexRegionSelector); + ~PerceptiveLeggedPrecomputation() override = default; + + PerceptiveLeggedPrecomputation* clone() const override { return new PerceptiveLeggedPrecomputation(*this); } + + void request(RequestSet request, scalar_t t, const vector_t& x, const vector_t& u) override; + + const std::vector& getFootPlacementConParameters() const + { + return footPlacementConParameters_; + } + + PerceptiveLeggedPrecomputation(const PerceptiveLeggedPrecomputation& rhs); + + private: + std::pair getPolygonConstraint( + const convex_plane_decomposition::CgalPolygon2d& polygon) const; + + const ConvexRegionSelector* convexRegionSelectorPtr_; + + std::vector footPlacementConParameters_; + }; +} // namespace ocs2::legged_robot diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h new file mode 100644 index 0000000..41f6e95 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h @@ -0,0 +1,52 @@ +// +// Created by qiayuan on 23-1-3. +// +#pragma once + +#include + +#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" + +#include + +namespace ocs2::legged_robot +{ + class PerceptiveLeggedReferenceManager : public SwitchedModelReferenceManager + { + public: + PerceptiveLeggedReferenceManager(CentroidalModelInfo info, std::shared_ptr gaitSchedulePtr, + std::shared_ptr swingTrajectoryPtr, + std::shared_ptr convexRegionSelectorPtr, + const EndEffectorKinematics& endEffectorKinematics, + scalar_t comHeight); + + const std::shared_ptr& getConvexRegionSelectorPtr() { return convexRegionSelectorPtr_; } + + contact_flag_t getFootPlacementFlags(scalar_t time) const; + + protected: + void modifyReferences(scalar_t initTime, scalar_t finalTime, const vector_t& initState, + TargetTrajectories& targetTrajectories, + ModeSchedule& modeSchedule) override; + + virtual void updateSwingTrajectoryPlanner(scalar_t initTime, const vector_t& initState, + ModeSchedule& modeSchedule); + + void modifyProjections(scalar_t initTime, const vector_t& initState, size_t leg, size_t initIndex, + const std::vector& contactFlagStocks, + std::vector& projections); + + std::pair getHeights(const std::vector& contactFlagStocks, + const std::vector< + convex_plane_decomposition::PlanarTerrainProjection>& + projections); + + const CentroidalModelInfo info_; + feet_array_t lastLiftoffPos_; + + std::shared_ptr convexRegionSelectorPtr_; + std::unique_ptr> endEffectorKinematicsPtr_; + + scalar_t comHeight_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h new file mode 100644 index 0000000..520e67a --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h @@ -0,0 +1,41 @@ +// +// Created by qiayuan on 23-1-1. +// + +#pragma once + +#include +#include +#include +#include + +namespace ocs2::legged_robot +{ + class FootCollisionConstraint final : public StateConstraint + { + public: + FootCollisionConstraint(const SwitchedModelReferenceManager& referenceManager, + const EndEffectorKinematics& endEffectorKinematics, + std::shared_ptr sdfPtr, size_t contactPointIndex, + scalar_t clearance); + + ~FootCollisionConstraint() override = default; + FootCollisionConstraint* clone() const override { return new FootCollisionConstraint(*this); } + + bool isActive(scalar_t time) const override; + size_t getNumConstraints(scalar_t /*time*/) const override { return 1; } + vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const override; + + private: + FootCollisionConstraint(const FootCollisionConstraint& rhs); + + const SwitchedModelReferenceManager* referenceManagerPtr_; + std::unique_ptr> endEffectorKinematicsPtr_; + std::shared_ptr sdfPtr_; + + const size_t contactPointIndex_; + const scalar_t clearance_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h new file mode 100644 index 0000000..dc213dc --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h @@ -0,0 +1,44 @@ +// +// Created by qiayuan on 23-1-1. +// + +#pragma once + +#include +#include +#include + +namespace ocs2::legged_robot +{ + class FootPlacementConstraint final : public StateConstraint + { + public: + struct Parameter + { + matrix_t a; + vector_t b; + }; + + FootPlacementConstraint(const SwitchedModelReferenceManager& referenceManager, + const EndEffectorKinematics& endEffectorKinematics, size_t contactPointIndex, + size_t numVertices); + + ~FootPlacementConstraint() override = default; + FootPlacementConstraint* clone() const override { return new FootPlacementConstraint(*this); } + + bool isActive(scalar_t time) const override; + size_t getNumConstraints(scalar_t /*time*/) const override { return numVertices_; } + vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const override; + + private: + FootPlacementConstraint(const FootPlacementConstraint& rhs); + + const SwitchedModelReferenceManager* referenceManagerPtr_; + std::unique_ptr> endEffectorKinematicsPtr_; + + const size_t contactPointIndex_; + const size_t numVertices_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h new file mode 100644 index 0000000..e5448ae --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h @@ -0,0 +1,39 @@ +// +// Created by qiayuan on 22-12-27. +// + +#pragma once + +#include + +#include +#include +#include + +namespace ocs2::legged_robot +{ + class SphereSdfConstraint final : public StateConstraint + { + public: + SphereSdfConstraint(const PinocchioSphereKinematics& sphereKinematics, + std::shared_ptr sdfPtr); + + /** Default destructor */ + ~SphereSdfConstraint() override = default; + + SphereSdfConstraint* clone() const override { return new SphereSdfConstraint(*this); } + + size_t getNumConstraints(scalar_t /*time*/) const override { return numConstraints_; } + + vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const override; + + private: + SphereSdfConstraint(const SphereSdfConstraint& rhs); + + std::unique_ptr sphereKinematicsPtr_; + std::shared_ptr sdfPtr_; + size_t numConstraints_{}; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/package.xml b/controllers/ocs2_quadruped_controller/package.xml index f506b6f..bce1218 100644 --- a/controllers/ocs2_quadruped_controller/package.xml +++ b/controllers/ocs2_quadruped_controller/package.xml @@ -19,6 +19,12 @@ ocs2_legged_robot_ros angles + + convex_plane_decomposition_msgs + convex_plane_decomposition_ros + grid_map_sdf + ocs2_sphere_approximation + ament_lint_auto ament_lint_common diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp b/controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp new file mode 100644 index 0000000..90bd0b2 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp @@ -0,0 +1,206 @@ +// +// Created by qiayuan on 23-1-2. +// +#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" + +#include +#include +#include + +#include + +namespace ocs2::legged_robot +{ + ConvexRegionSelector::ConvexRegionSelector(CentroidalModelInfo info, + std::shared_ptr + planarTerrainPtr, + const EndEffectorKinematics& endEffectorKinematics, + size_t numVertices) + : info_(std::move(info)), + numVertices_(numVertices), + planarTerrainPtr_(std::move(planarTerrainPtr)), + endEffectorKinematicsPtr_(endEffectorKinematics.clone()) + { + } + + convex_plane_decomposition::PlanarTerrainProjection ConvexRegionSelector::getProjection( + size_t leg, scalar_t time) const + { + const auto index = lookup::findIndexInTimeArray(timeEvents_[leg], time); + return feetProjections_[leg][index]; + } + + convex_plane_decomposition::CgalPolygon2d ConvexRegionSelector::getConvexPolygon(size_t leg, scalar_t time) const + { + const auto index = lookup::findIndexInTimeArray(timeEvents_[leg], time); + return convexPolygons_[leg][index]; + } + + vector3_t ConvexRegionSelector::getNominalFootholds(size_t leg, scalar_t time) const + { + const auto index = lookup::findIndexInTimeArray(timeEvents_[leg], time); + return nominalFootholds_[leg][index]; + } + + void ConvexRegionSelector::update(const ModeSchedule& modeSchedule, scalar_t initTime, const vector_t& initState, + TargetTrajectories& targetTrajectories) + { + planarTerrain_ = *planarTerrainPtr_; + // Need copy storage it since PlanarTerrainProjection.regionPtr is a pointer + const auto& modeSequence = modeSchedule.modeSequence; + const auto& eventTimes = modeSchedule.eventTimes; + const auto contactFlagStocks = extractContactFlags(modeSequence); + const size_t numPhases = modeSequence.size(); + + // Find start and final index of time for legs + feet_array_t> startIndices; + feet_array_t> finalIndices; + for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++) + { + startIndices[leg] = std::vector(numPhases, 0); + finalIndices[leg] = std::vector(numPhases, 0); + // find the startTime and finalTime indices for swing feet + for (size_t i = 0; i < numPhases; i++) + { + // skip if it is a stance leg + if (contactFlagStocks[leg][i]) + { + std::tie(startIndices[leg][i], finalIndices[leg][i]) = findIndex(i, contactFlagStocks[leg]); + } + } + } + + for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++) + { + feetProjections_[leg].clear(); + convexPolygons_[leg].clear(); + nominalFootholds_[leg].clear(); + feetProjections_[leg].resize(numPhases); + convexPolygons_[leg].resize(numPhases); + nominalFootholds_[leg].resize(numPhases); + middleTimes_[leg].clear(); + initStandFinalTime_[leg] = 0; + + scalar_t lastStandMiddleTime = NAN; + // Stand leg foot + for (size_t i = 0; i < numPhases; ++i) + { + if (contactFlagStocks[leg][i]) + { + const int standStartIndex = startIndices[leg][i]; + const int standFinalIndex = finalIndices[leg][i]; + const scalar_t standStartTime = eventTimes[standStartIndex]; + const scalar_t standFinalTime = eventTimes[standFinalIndex]; + const scalar_t standMiddleTime = standStartTime + (standFinalTime - standStartTime) / 2; + + if (!numerics::almost_eq(standMiddleTime, lastStandMiddleTime)) + { + vector3_t footPos = getNominalFoothold(leg, standMiddleTime, initState, targetTrajectories); + auto penaltyFunction = [](const vector3_t& /*projectedPoint*/) { return 0.0; }; + const auto projection = getBestPlanarRegionAtPositionInWorld( + footPos, planarTerrain_.planarRegions, penaltyFunction); + scalar_t growthFactor = 1.05; + const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape( + projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame, + numVertices_, growthFactor); + + feetProjections_[leg][i] = projection; + convexPolygons_[leg][i] = convexRegion; + nominalFootholds_[leg][i] = footPos; + middleTimes_[leg].push_back(standMiddleTime); + } + else + { + feetProjections_[leg][i] = feetProjections_[leg][i - 1]; + convexPolygons_[leg][i] = convexPolygons_[leg][i - 1]; + nominalFootholds_[leg][i] = nominalFootholds_[leg][i - 1]; + } + + if (standStartTime < initTime && initTime < standFinalTime) + { + initStandFinalTime_[leg] = standFinalTime; + } + } + } + } + + for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++) + { + timeEvents_[leg] = eventTimes; + } + } + + feet_array_t> ConvexRegionSelector::extractContactFlags( + const std::vector& phaseIDsStock) const + { + const size_t numPhases = phaseIDsStock.size(); + + feet_array_t> contactFlagStock; + std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector(numPhases)); + + for (size_t i = 0; i < numPhases; i++) + { + const auto contactFlag = modeNumber2StanceLeg(phaseIDsStock[i]); + for (size_t j = 0; j < info_.numThreeDofContacts; j++) + { + contactFlagStock[j][i] = contactFlag[j]; + } + } + return contactFlagStock; + } + + std::pair ConvexRegionSelector::findIndex(size_t index, const std::vector& contactFlagStock) + { + const size_t numPhases = contactFlagStock.size(); + + if (!contactFlagStock[index]) + { + return {0, 0}; + } + + // find the starting time + int startTimesIndex = 0; + for (int ip = index - 1; ip >= 0; ip--) + { + if (!contactFlagStock[ip]) + { + startTimesIndex = ip; + break; + } + } + // find the final time + int finalTimesIndex = numPhases - 2; + for (size_t ip = index + 1; ip < numPhases; ip++) + { + if (!contactFlagStock[ip]) + { + finalTimesIndex = ip - 1; + break; + } + } + return {startTimesIndex, finalTimesIndex}; + } + + vector3_t ConvexRegionSelector::getNominalFoothold(size_t leg, scalar_t time, const vector_t& initState, + TargetTrajectories& targetTrajectories) + { + scalar_t height = 0.4; + + vector_t desiredState = targetTrajectories.getDesiredState(time); + vector3_t desiredVel = centroidal_model::getNormalizedMomentum(desiredState, info_).head(3); + vector3_t measuredVel = centroidal_model::getNormalizedMomentum(initState, info_).head(3); + + auto feedback = (vector3_t() << (std::sqrt(height / 9.81) * (measuredVel - desiredVel)).head(2), 0).finished(); + vector_t zyx = centroidal_model::getBasePose(desiredState, info_).tail(3); + scalar_t offset = tan(-zyx(1)) * height; + vector3_t offsetVector(offset * cos(-zyx(1)), 0, offset * sin(-zyx(1))); + matrix3_t R; + scalar_t z = zyx(0); + R << cos(z), -sin(z), 0, // clang-format off + sin(z), cos(z), 0, + 0, 0, 1; // clang-format on + // return endEffectorKinematicsPtr_->getPosition(targetTrajectories.getDesiredState(time))[leg] + feedback; + return endEffectorKinematicsPtr_->getPosition(targetTrajectories.getDesiredState(time))[leg] - R.transpose() * + offsetVector; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp b/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp new file mode 100644 index 0000000..224d8ef --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp @@ -0,0 +1,136 @@ +// +// Created by qiayuan on 22-12-27. +// + +#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h" +#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h" +#include + +#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h" +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h" +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h" + +#include +#include +#include + +#include + +namespace ocs2::legged_robot +{ + void PerceptiveLeggedInterface::setupOptimalControlProblem(const std::string& taskFile, const std::string& urdfFile, + const std::string& referenceFile, bool verbose) + { + planarTerrainPtr_ = std::make_shared(); + + double width{5.0}, height{5.0}; + convex_plane_decomposition::PlanarRegion plannerRegion; + plannerRegion.transformPlaneToWorld.setIdentity(); + plannerRegion.bbox2d = convex_plane_decomposition::CgalBbox2d(-height / 2, -width / 2, +height / 2, width / 2); + convex_plane_decomposition::CgalPolygonWithHoles2d boundary; + boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(+height / 2, +width / 2)); + boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(-height / 2, +width / 2)); + boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(-height / 2, -width / 2)); + boundary.outer_boundary().push_back(convex_plane_decomposition::CgalPoint2d(+height / 2, -width / 2)); + plannerRegion.boundaryWithInset.boundary = boundary; + convex_plane_decomposition::CgalPolygonWithHoles2d insets; + insets.outer_boundary().push_back( + convex_plane_decomposition::CgalPoint2d(+height / 2 - 0.01, +width / 2 - 0.01)); + insets.outer_boundary().push_back( + convex_plane_decomposition::CgalPoint2d(-height / 2 + 0.01, +width / 2 - 0.01)); + insets.outer_boundary().push_back( + convex_plane_decomposition::CgalPoint2d(-height / 2 + 0.01, -width / 2 + 0.01)); + insets.outer_boundary().push_back( + convex_plane_decomposition::CgalPoint2d(+height / 2 - 0.01, -width / 2 + 0.01)); + plannerRegion.boundaryWithInset.insets.push_back(insets); + planarTerrainPtr_->planarRegions.push_back(plannerRegion); + + std::string layer = "elevation_before_postprocess"; + planarTerrainPtr_->gridMap.setGeometry(grid_map::Length(5.0, 5.0), 0.03); + planarTerrainPtr_->gridMap.add(layer, 0); + planarTerrainPtr_->gridMap.add("smooth_planar", 0); + signedDistanceFieldPtr_ = std::make_shared(); + signedDistanceFieldPtr_->calculateSignedDistanceField(planarTerrainPtr_->gridMap, layer, 0.1); + + LeggedInterface::setupOptimalControlProblem(taskFile, urdfFile, referenceFile, verbose); + + for (size_t i = 0; i < centroidal_model_info_.numThreeDofContacts; i++) + { + const std::string& footName = modelSettings().contactNames3DoF[i]; + std::unique_ptr> eeKinematicsPtr = getEeKinematicsPtr({footName}, footName); + + std::unique_ptr placementPenalty( + new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-2, 1e-4))); + std::unique_ptr collisionPenalty( + new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-2, 1e-3))); + + // For foot placement + std::unique_ptr footPlacementConstraint( + new FootPlacementConstraint(*reference_manager_ptr_, *eeKinematicsPtr, i, numVertices_)); + problem_ptr_->stateSoftConstraintPtr->add( + footName + "_footPlacement", + std::make_unique(std::move(footPlacementConstraint), std::move(placementPenalty))); + + // For foot Collision + std::unique_ptr footCollisionConstraint( + new FootCollisionConstraint(*reference_manager_ptr_, *eeKinematicsPtr, signedDistanceFieldPtr_, i, + 0.03)); + problem_ptr_->stateSoftConstraintPtr->add( + footName + "_footCollision", + std::make_unique(std::move(footCollisionConstraint), std::move(collisionPenalty))); + } + + // For collision avoidance + scalar_t thighExcess = 0.025; + scalar_t calfExcess = 0.02; + + std::vector collisionLinks = {"LF_calf", "RF_calf", "LH_calf", "RH_calf"}; + const std::vector& maxExcesses = {calfExcess, calfExcess, calfExcess, calfExcess}; + + pinocchioSphereInterfacePtr_ = std::make_shared( + *pinocchio_interface_ptr_, collisionLinks, maxExcesses, 0.6); + + CentroidalModelPinocchioMapping pinocchioMapping(centroidal_model_info_); + auto sphereKinematicsPtr = std::make_unique( + *pinocchioSphereInterfacePtr_, pinocchioMapping); + + std::unique_ptr sphereSdfConstraint( + new SphereSdfConstraint(*sphereKinematicsPtr, signedDistanceFieldPtr_)); + + // std::unique_ptr penalty(new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-3, 1e-3))); + // problem_ptr_->stateSoftConstraintPtr->add( + // "sdfConstraint", std::unique_ptr(new StateSoftConstraint(std::move(sphereSdfConstraint), std::move(penalty)))); + } + + void PerceptiveLeggedInterface::setupReferenceManager(const std::string& taskFile, const std::string& /*urdfFile*/, + const std::string& referenceFile, bool verbose) + { + auto swingTrajectoryPlanner = + std::make_unique( + loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4); + + std::unique_ptr> eeKinematicsPtr = getEeKinematicsPtr( + {model_settings_.contactNames3DoF}, "ALL_FOOT"); + auto convexRegionSelector = + std::make_unique(centroidal_model_info_, planarTerrainPtr_, *eeKinematicsPtr, + numVertices_); + + scalar_t comHeight = 0; + loadData::loadCppDataType(referenceFile, "comHeight", comHeight); + reference_manager_ptr_.reset(new PerceptiveLeggedReferenceManager( + centroidal_model_info_, loadGaitSchedule(referenceFile, verbose), + std::move(swingTrajectoryPlanner), std::move(convexRegionSelector), + *eeKinematicsPtr, comHeight)); + } + + void PerceptiveLeggedInterface::setupPreComputation(const std::string& /*taskFile*/, + const std::string& /*urdfFile*/, + const std::string& /*referenceFile*/, bool /*verbose*/) + { + problem_ptr_->preComputationPtr = std::make_unique( + *pinocchio_interface_ptr_, centroidal_model_info_, *reference_manager_ptr_->getSwingTrajectoryPlanner(), + model_settings_, + *dynamic_cast(*reference_manager_ptr_).getConvexRegionSelectorPtr()); + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp b/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp new file mode 100644 index 0000000..ad05720 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp @@ -0,0 +1,96 @@ +// +// Created by qiayuan on 23-1-1. +// + +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" + +namespace ocs2::legged_robot +{ + PerceptiveLeggedPrecomputation::PerceptiveLeggedPrecomputation(PinocchioInterface pinocchioInterface, + const CentroidalModelInfo& info, + const SwingTrajectoryPlanner& swingTrajectoryPlanner, + ModelSettings settings, + const ConvexRegionSelector& convexRegionSelector) + : LeggedRobotPreComputation(std::move(pinocchioInterface), info, swingTrajectoryPlanner, std::move(settings)), + convexRegionSelectorPtr_(&convexRegionSelector) + { + footPlacementConParameters_.resize(info.numThreeDofContacts); + } + + PerceptiveLeggedPrecomputation::PerceptiveLeggedPrecomputation(const PerceptiveLeggedPrecomputation& rhs) + : LeggedRobotPreComputation(rhs), convexRegionSelectorPtr_(rhs.convexRegionSelectorPtr_) + { + footPlacementConParameters_.resize(rhs.footPlacementConParameters_.size()); + } + + void PerceptiveLeggedPrecomputation::request(RequestSet request, scalar_t t, const vector_t& x, const vector_t& u) + { + if (!request.containsAny(Request::Cost + Request::Constraint + Request::SoftConstraint)) + { + return; + } + LeggedRobotPreComputation::request(request, t, x, u); + + if (request.contains(Request::Constraint)) + { + for (size_t i = 0; i < footPlacementConParameters_.size(); i++) + { + FootPlacementConstraint::Parameter params; + + auto projection = convexRegionSelectorPtr_->getProjection(i, t); + if (projection.regionPtr == nullptr) + { + // Swing leg + continue; + } + + matrix_t polytopeA; + vector_t polytopeB; + std::tie(polytopeA, polytopeB) = getPolygonConstraint(convexRegionSelectorPtr_->getConvexPolygon(i, t)); + matrix_t p = (matrix_t(2, 3) << // clang-format off + 1, 0, 0, + 0, 1, 0).finished(); // clang-format on + params.a = polytopeA * p * projection.regionPtr->transformPlaneToWorld.inverse().linear(); + params.b = polytopeB + polytopeA * projection.regionPtr->transformPlaneToWorld.inverse().translation(). + head(2); + + footPlacementConParameters_[i] = params; + } + } + } + + std::pair PerceptiveLeggedPrecomputation::getPolygonConstraint( + const convex_plane_decomposition::CgalPolygon2d& polygon) const + { + size_t numVertices = polygon.size(); + matrix_t polytopeA = matrix_t::Zero(numVertices, 2); + vector_t polytopeB = vector_t::Zero(numVertices); + + for (size_t i = 0; i < numVertices; i++) + { + size_t j = i + 1; + if (j == numVertices) + { + j = 0; + } + size_t k = j + 1; + if (k == numVertices) + { + k = 0; + } + const auto point_a = polygon.vertex(i); + const auto point_b = polygon.vertex(j); + const auto point_c = polygon.vertex(k); + + polytopeA.row(i) << point_b.y() - point_a.y(), point_a.x() - point_b.x(); + polytopeB(i) = point_a.y() * point_b.x() - point_a.x() * point_b.y(); + if (polytopeA.row(i) * (vector_t(2) << point_c.x(), point_c.y()).finished() + polytopeB(i) < 0) + { + polytopeA.row(i) *= -1; + polytopeB(i) *= -1; + } + } + + return {polytopeA, polytopeB}; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp b/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp new file mode 100644 index 0000000..b7cd72c --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp @@ -0,0 +1,206 @@ +// +// Created by qiayuan on 23-1-3. +// + +#include +#include +#include +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h" + +namespace ocs2::legged_robot +{ + PerceptiveLeggedReferenceManager::PerceptiveLeggedReferenceManager(CentroidalModelInfo info, + std::shared_ptr gaitSchedulePtr, + std::shared_ptr + swingTrajectoryPtr, + std::shared_ptr + convexRegionSelectorPtr, + const EndEffectorKinematics& + endEffectorKinematics, + scalar_t comHeight) + : info_(std::move(info)), + SwitchedModelReferenceManager(std::move(gaitSchedulePtr), std::move(swingTrajectoryPtr)), + convexRegionSelectorPtr_(std::move(convexRegionSelectorPtr)), + endEffectorKinematicsPtr_(endEffectorKinematics.clone()), + comHeight_(comHeight) + { + } + + void PerceptiveLeggedReferenceManager::modifyReferences(scalar_t initTime, scalar_t finalTime, + const vector_t& initState, + TargetTrajectories& targetTrajectories, + ModeSchedule& modeSchedule) + { + const auto timeHorizon = finalTime - initTime; + modeSchedule = getGaitSchedule()->getModeSchedule(initTime - timeHorizon, finalTime + timeHorizon); + + TargetTrajectories newTargetTrajectories; + int nodeNum = 11; + for (size_t i = 0; i < nodeNum; ++i) + { + scalar_t time = initTime + static_cast(i) * timeHorizon / (nodeNum - 1); + vector_t state = targetTrajectories.getDesiredState(time); + vector_t input = targetTrajectories.getDesiredState(time); + + const auto& map = convexRegionSelectorPtr_->getPlanarTerrainPtr()->gridMap; + vector_t pos = centroidal_model::getBasePose(state, info_).head(3); + + // Base Orientation + scalar_t step = 0.3; + grid_map::Vector3 normalVector; + normalVector(0) = (map.atPosition("smooth_planar", pos + grid_map::Position(-step, 0)) - + map.atPosition("smooth_planar", pos + grid_map::Position(step, 0))) / + (2 * step); + normalVector(1) = (map.atPosition("smooth_planar", pos + grid_map::Position(0, -step)) - + map.atPosition("smooth_planar", pos + grid_map::Position(0, step))) / + (2 * step); + normalVector(2) = 1; + normalVector.normalize(); + matrix3_t R; + scalar_t z = centroidal_model::getBasePose(state, info_)(3); + R << cos(z), -sin(z), 0, // clang-format off + sin(z), cos(z), 0, + 0, 0, 1; // clang-format on + vector_t v = R.transpose() * normalVector; + centroidal_model::getBasePose(state, info_)(4) = atan(v.x() / v.z()); + + // Base Z Position + centroidal_model::getBasePose(state, info_)(2) = + map.atPosition("smooth_planar", pos) + comHeight_ / cos(centroidal_model::getBasePose(state, info_)(4)); + + newTargetTrajectories.timeTrajectory.push_back(time); + newTargetTrajectories.stateTrajectory.push_back(state); + newTargetTrajectories.inputTrajectory.push_back(input); + } + targetTrajectories = newTargetTrajectories; + + // Footstep + convexRegionSelectorPtr_->update(modeSchedule, initTime, initState, targetTrajectories); + + // Swing trajectory + updateSwingTrajectoryPlanner(initTime, initState, modeSchedule); + } + + void PerceptiveLeggedReferenceManager::updateSwingTrajectoryPlanner(scalar_t initTime, const vector_t& initState, + ModeSchedule& modeSchedule) + { + const auto contactFlagStocks = convexRegionSelectorPtr_->extractContactFlags(modeSchedule.modeSequence); + feet_array_t liftOffHeightSequence, touchDownHeightSequence; + + for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++) + { + size_t initIndex = lookup::findIndexInTimeArray(modeSchedule.eventTimes, initTime); + + auto projections = convexRegionSelectorPtr_->getProjections(leg); + modifyProjections(initTime, initState, leg, initIndex, contactFlagStocks[leg], projections); + + scalar_array_t liftOffHeights, touchDownHeights; + std::tie(liftOffHeights, touchDownHeights) = getHeights(contactFlagStocks[leg], projections); + liftOffHeightSequence[leg] = liftOffHeights; + touchDownHeightSequence[leg] = touchDownHeights; + } + swingTrajectoryPtr_->update(modeSchedule, liftOffHeightSequence, touchDownHeightSequence); + } + + void PerceptiveLeggedReferenceManager::modifyProjections(scalar_t initTime, const vector_t& initState, size_t leg, + size_t initIndex, + const std::vector& contactFlagStocks, + std::vector< + convex_plane_decomposition::PlanarTerrainProjection>& + projections) + { + if (contactFlagStocks[initIndex]) + { + lastLiftoffPos_[leg] = endEffectorKinematicsPtr_->getPosition(initState)[leg]; + lastLiftoffPos_[leg].z() -= 0.02; + for (int i = initIndex; i < projections.size(); ++i) + { + if (!contactFlagStocks[i]) + { + break; + } + projections[i].positionInWorld = lastLiftoffPos_[leg]; + } + for (int i = initIndex; i >= 0; --i) + { + if (!contactFlagStocks[i]) + { + break; + } + projections[i].positionInWorld = lastLiftoffPos_[leg]; + } + } + if (initTime > convexRegionSelectorPtr_->getInitStandFinalTimes()[leg]) + { + for (int i = initIndex; i >= 0; --i) + { + if (contactFlagStocks[i]) + { + projections[i].positionInWorld = lastLiftoffPos_[leg]; + } + if (!contactFlagStocks[i] && !contactFlagStocks[i + 1]) + { + break; + } + } + } + // for (int i = 0; i < numPhases; ++i) { + // if (leg == 1) std::cerr << std::setprecision(3) << projections[i].positionInWorld.z() << "\t"; + // } + // std::cerr << std::endl; + } + + std::pair PerceptiveLeggedReferenceManager::getHeights( + const std::vector& contactFlagStocks, + const std::vector& projections) + { + scalar_array_t liftOffHeights, touchDownHeights; + const size_t numPhases = projections.size(); + + liftOffHeights.clear(); + liftOffHeights.resize(numPhases); + touchDownHeights.clear(); + touchDownHeights.resize(numPhases); + + for (size_t i = 1; i < numPhases; ++i) + { + if (!contactFlagStocks[i]) + { + liftOffHeights[i] = contactFlagStocks[i - 1] + ? projections[i - 1].positionInWorld.z() + : liftOffHeights[i - 1]; + } + } + for (int i = numPhases - 2; i >= 0; --i) + { + if (!contactFlagStocks[i]) + { + touchDownHeights[i] = contactFlagStocks[i + 1] + ? projections[i + 1].positionInWorld.z() + : touchDownHeights[i + 1]; + } + } + + // for (int i = 0; i < numPhases; ++i) { + // std::cerr << std::setprecision(3) << liftOffHeights[i] << "\t"; + // } + // std::cerr << std::endl; + // for (int i = 0; i < numPhases; ++i) { + // std::cerr << std::setprecision(3) << contactFlagStocks[i] << "\t"; + // } + // std::cerr << std::endl; + + return {liftOffHeights, touchDownHeights}; + } + + contact_flag_t PerceptiveLeggedReferenceManager::getFootPlacementFlags(scalar_t time) const + { + contact_flag_t flag; + const auto finalTime = convexRegionSelectorPtr_->getInitStandFinalTimes(); + for (int i = 0; i < flag.size(); ++i) + { + flag[i] = getContactFlags(time)[i] && time >= finalTime[i]; + } + return flag; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp b/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp new file mode 100644 index 0000000..846a2a7 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp @@ -0,0 +1,60 @@ +// +// Created by qiayuan on 23-1-26. +// +#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h" + +namespace ocs2::legged_robot +{ + FootCollisionConstraint::FootCollisionConstraint(const SwitchedModelReferenceManager& referenceManager, + const EndEffectorKinematics& endEffectorKinematics, + std::shared_ptr sdfPtr, + size_t contactPointIndex, + scalar_t clearance) + : StateConstraint(ConstraintOrder::Linear), + referenceManagerPtr_(&referenceManager), + endEffectorKinematicsPtr_(endEffectorKinematics.clone()), + sdfPtr_(std::move(sdfPtr)), + contactPointIndex_(contactPointIndex), + clearance_(clearance) + { + } + + FootCollisionConstraint::FootCollisionConstraint(const FootCollisionConstraint& rhs) + : StateConstraint(ConstraintOrder::Linear), + referenceManagerPtr_(rhs.referenceManagerPtr_), + endEffectorKinematicsPtr_(rhs.endEffectorKinematicsPtr_->clone()), + sdfPtr_(rhs.sdfPtr_), + contactPointIndex_(rhs.contactPointIndex_), + clearance_(rhs.clearance_) + { + } + + bool FootCollisionConstraint::isActive(scalar_t time) const + { + scalar_t offset = 0.05; + return !referenceManagerPtr_->getContactFlags(time)[contactPointIndex_] && + !referenceManagerPtr_->getContactFlags(time + 0.5 * offset)[contactPointIndex_] && + !referenceManagerPtr_->getContactFlags(time - offset)[contactPointIndex_]; + } + + vector_t FootCollisionConstraint::getValue(scalar_t /*time*/, const vector_t& state, + const PreComputation& /*preComp*/) const + { + vector_t value(1); + value(0) = sdfPtr_->getDistanceAt(grid_map::Position3(endEffectorKinematicsPtr_->getPosition(state).front())) - + clearance_; + return value; + } + + VectorFunctionLinearApproximation FootCollisionConstraint::getLinearApproximation( + scalar_t time, const vector_t& state, + const PreComputation& preComp) const + { + VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(1, state.size(), 0); + approx.f = getValue(time, state, preComp); + approx.dfdx = sdfPtr_->getDistanceGradientAt(grid_map::Position3(endEffectorKinematicsPtr_->getPosition(state).front())). + transpose() * + endEffectorKinematicsPtr_->getPositionLinearApproximation(state).front().dfdx; + return approx; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp b/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp new file mode 100644 index 0000000..583986f --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp @@ -0,0 +1,60 @@ +// +// Created by qiayuan on 23-1-1. +// + +#include "ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h" +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h" + +namespace ocs2::legged_robot +{ + FootPlacementConstraint::FootPlacementConstraint(const SwitchedModelReferenceManager& referenceManager, + const EndEffectorKinematics& endEffectorKinematics, + size_t contactPointIndex, + size_t numVertices) + : StateConstraint(ConstraintOrder::Linear), + referenceManagerPtr_(&referenceManager), + endEffectorKinematicsPtr_(endEffectorKinematics.clone()), + contactPointIndex_(contactPointIndex), + numVertices_(numVertices) + { + } + + FootPlacementConstraint::FootPlacementConstraint(const FootPlacementConstraint& rhs) + : StateConstraint(ConstraintOrder::Linear), + referenceManagerPtr_(rhs.referenceManagerPtr_), + endEffectorKinematicsPtr_(rhs.endEffectorKinematicsPtr_->clone()), + contactPointIndex_(rhs.contactPointIndex_), + numVertices_(rhs.numVertices_) + { + } + + bool FootPlacementConstraint::isActive(scalar_t time) const + { + return dynamic_cast(*referenceManagerPtr_).getFootPlacementFlags(time)[ + contactPointIndex_]; + } + + vector_t FootPlacementConstraint::getValue(scalar_t /*time*/, const vector_t& state, + const PreComputation& preComp) const + { + const auto param = cast(preComp).getFootPlacementConParameters()[ + contactPointIndex_]; + return param.a * endEffectorKinematicsPtr_->getPosition(state).front() + param.b; + } + + VectorFunctionLinearApproximation FootPlacementConstraint::getLinearApproximation( + scalar_t /*time*/, const vector_t& state, + const PreComputation& preComp) const + { + VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero( + numVertices_, state.size(), 0); + const auto param = cast(preComp).getFootPlacementConParameters()[ + contactPointIndex_]; + + const auto positionApprox = endEffectorKinematicsPtr_->getPositionLinearApproximation(state).front(); + approx.f = param.a * positionApprox.f + param.b; + approx.dfdx = param.a * positionApprox.dfdx; + return approx; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp b/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp new file mode 100644 index 0000000..1a109d3 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp @@ -0,0 +1,60 @@ +// +// Created by qiayuan on 22-12-27. +// + +#include "ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h" +#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h" + +namespace ocs2::legged_robot +{ + SphereSdfConstraint::SphereSdfConstraint(const PinocchioSphereKinematics& sphereKinematics, + std::shared_ptr sdfPtr) + : StateConstraint(ConstraintOrder::Linear), + sphereKinematicsPtr_(sphereKinematics.clone()), + sdfPtr_(std::move(sdfPtr)), + numConstraints_(sphereKinematicsPtr_->getPinocchioSphereInterface().getNumSpheresInTotal()) + { + } + + vector_t SphereSdfConstraint::getValue(scalar_t /*time*/, const vector_t& state, + const PreComputation& preComp) const + { + vector_t value(numConstraints_); + + sphereKinematicsPtr_->setPinocchioInterface( + cast(preComp).getPinocchioInterface()); + auto position = sphereKinematicsPtr_->getPosition(state); + auto radius = sphereKinematicsPtr_->getPinocchioSphereInterface().getSphereRadii(); + for (int i = 0; i < numConstraints_; ++i) + { + value(i) = sdfPtr_->getDistanceAt(grid_map::Position3(position[i])) - radius[i]; + } + return value; + } + + VectorFunctionLinearApproximation SphereSdfConstraint::getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const + { + VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero( + numConstraints_, state.size(), 0); + approx.f = getValue(time, state, preComp); + + auto position = sphereKinematicsPtr_->getPosition(state); + auto sphereApprox = sphereKinematicsPtr_->getPositionLinearApproximation(state); + + for (int i = 0; i < numConstraints_; ++i) + { + vector_t sdfGradient = sdfPtr_->getDistanceGradientAt(grid_map::Position3(position[i])); + approx.dfdx.row(i) = sdfGradient.transpose() * sphereApprox[i].dfdx; + } + return approx; + } + + SphereSdfConstraint::SphereSdfConstraint(const SphereSdfConstraint& rhs) + : StateConstraint(rhs), + sphereKinematicsPtr_(rhs.sphereKinematicsPtr_->clone()), + sdfPtr_(rhs.sdfPtr_), + numConstraints_(rhs.numConstraints_) + { + } +} // namespace legged diff --git a/libraries/controller_common/include/controller_common/CtrlInterfaces.h b/libraries/controller_common/include/controller_common/CtrlInterfaces.h index 9969aae..ec27b43 100644 --- a/libraries/controller_common/include/controller_common/CtrlInterfaces.h +++ b/libraries/controller_common/include/controller_common/CtrlInterfaces.h @@ -10,33 +10,34 @@ #include #include -struct CtrlInterfaces { - std::vector > +struct CtrlInterfaces +{ + std::vector> joint_torque_command_interface_; - std::vector > + std::vector> joint_position_command_interface_; - std::vector > + std::vector> joint_velocity_command_interface_; - std::vector > + std::vector> joint_kp_command_interface_; - std::vector > + std::vector> joint_kd_command_interface_; - std::vector > + std::vector> joint_effort_state_interface_; - std::vector > + std::vector> joint_position_state_interface_; - std::vector > + std::vector> joint_velocity_state_interface_; - std::vector > + std::vector> imu_state_interface_; - std::vector > + std::vector> foot_force_state_interface_; - std::vector > + std::vector> odom_state_interface_; @@ -45,7 +46,8 @@ struct CtrlInterfaces { CtrlInterfaces() = default; - void clear() { + void clear() + { joint_torque_command_interface_.clear(); joint_position_command_interface_.clear(); joint_velocity_command_interface_.clear(); diff --git a/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h b/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h index 1c462c7..a7fbe10 100644 --- a/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h +++ b/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h @@ -1,23 +1,22 @@ // // Created by biao on 24-9-10. // - -#ifndef BASEFIXEDSTAND_H -#define BASEFIXEDSTAND_H +#pragma once #include "FSMState.h" -class BaseFixedStand : public FSMState { +class BaseFixedStand : public FSMState +{ public: - BaseFixedStand(CtrlInterfaces &ctrl_interfaces, - const std::vector &target_pos, + BaseFixedStand(CtrlInterfaces& ctrl_interfaces, + const std::vector& target_pos, double kp, double kd); void enter() override; - void run(const rclcpp::Time &time, - const rclcpp::Duration &period) override; + void run(const rclcpp::Time& time, + const rclcpp::Duration& period) override; void exit() override; @@ -34,6 +33,3 @@ protected: double percent_ = 0; //% double phase = 0.0; }; - - -#endif //BASEFIXEDSTAND_H diff --git a/libraries/controller_common/include/controller_common/FSM/FSMState.h b/libraries/controller_common/include/controller_common/FSM/FSMState.h index d195fb1..86e73a9 100644 --- a/libraries/controller_common/include/controller_common/FSM/FSMState.h +++ b/libraries/controller_common/include/controller_common/FSM/FSMState.h @@ -11,20 +11,22 @@ #include #include -class FSMState { +class FSMState +{ public: virtual ~FSMState() = default; - FSMState(const FSMStateName &state_name, std::string state_name_string, CtrlInterfaces &ctrl_interfaces) + FSMState(const FSMStateName& state_name, std::string state_name_string, CtrlInterfaces& ctrl_interfaces) : state_name(state_name), state_name_string(std::move(state_name_string)), - ctrl_interfaces_(ctrl_interfaces) { + ctrl_interfaces_(ctrl_interfaces) + { } virtual void enter() = 0; - virtual void run(const rclcpp::Time &time, - const rclcpp::Duration &period) = 0; + virtual void run(const rclcpp::Time& time, + const rclcpp::Duration& period) = 0; virtual void exit() = 0; @@ -34,7 +36,7 @@ public: std::string state_name_string; protected: - CtrlInterfaces &ctrl_interfaces_; + CtrlInterfaces& ctrl_interfaces_; }; #endif //FSMSTATE_H diff --git a/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h b/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h index a867851..bd9812c 100644 --- a/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h +++ b/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h @@ -7,18 +7,19 @@ #include "FSMState.h" -class StateFixedDown final : public FSMState { +class StateFixedDown final : public FSMState +{ public: - explicit StateFixedDown(CtrlInterfaces &ctrl_interfaces, - const std::vector &target_pos, + explicit StateFixedDown(CtrlInterfaces& ctrl_interfaces, + const std::vector& target_pos, double kp, double kd ); void enter() override; - void run(const rclcpp::Time &time, - const rclcpp::Duration &period) override; + void run(const rclcpp::Time& time, + const rclcpp::Duration& period) override; void exit() override; diff --git a/libraries/controller_common/include/controller_common/FSM/StatePassive.h b/libraries/controller_common/include/controller_common/FSM/StatePassive.h index 2477075..3262451 100644 --- a/libraries/controller_common/include/controller_common/FSM/StatePassive.h +++ b/libraries/controller_common/include/controller_common/FSM/StatePassive.h @@ -6,14 +6,15 @@ #define STATEPASSIVE_H #include "FSMState.h" -class StatePassive final : public FSMState { +class StatePassive final : public FSMState +{ public: - explicit StatePassive(CtrlInterfaces &ctrl_interfaces); + explicit StatePassive(CtrlInterfaces& ctrl_interfaces); void enter() override; - void run(const rclcpp::Time &time, - const rclcpp::Duration &period) override; + void run(const rclcpp::Time& time, + const rclcpp::Duration& period) override; void exit() override; diff --git a/libraries/controller_common/include/controller_common/common/enumClass.h b/libraries/controller_common/include/controller_common/common/enumClass.h index d45c604..a0bec2d 100644 --- a/libraries/controller_common/include/controller_common/common/enumClass.h +++ b/libraries/controller_common/include/controller_common/common/enumClass.h @@ -5,7 +5,8 @@ #ifndef ENUMCLASS_H #define ENUMCLASS_H -enum class FSMStateName { +enum class FSMStateName +{ // EXIT, INVALID, PASSIVE, @@ -21,18 +22,21 @@ enum class FSMStateName { RL }; -enum class FSMMode { +enum class FSMMode +{ NORMAL, CHANGE }; -enum class FrameType { +enum class FrameType +{ BODY, HIP, GLOBAL }; -enum class WaveStatus { +enum class WaveStatus +{ STANCE_ALL, SWING_ALL, WAVE_ALL diff --git a/libraries/controller_common/src/FSM/BaseFixedStand.cpp b/libraries/controller_common/src/FSM/BaseFixedStand.cpp index 5f77d46..c5c653e 100644 --- a/libraries/controller_common/src/FSM/BaseFixedStand.cpp +++ b/libraries/controller_common/src/FSM/BaseFixedStand.cpp @@ -6,22 +6,27 @@ #include -BaseFixedStand::BaseFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector &target_pos, - const double kp, - const double kd) +BaseFixedStand::BaseFixedStand(CtrlInterfaces& ctrl_interfaces, const std::vector& target_pos, + const double kp, + const double kd) : FSMState(FSMStateName::FIXEDSTAND, "fixed stand", ctrl_interfaces), - kp_(kp), kd_(kd) { + kp_(kp), kd_(kd) +{ duration_ = ctrl_interfaces_.frequency_ * 1.2; - for (int i = 0; i < 12; i++) { + for (int i = 0; i < 12; i++) + { target_pos_[i] = target_pos[i]; } } -void BaseFixedStand::enter() { - for (int i = 0; i < 12; i++) { - start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value(); +void BaseFixedStand::enter() +{ + for (int i = 0; i < 12; i++) + { + start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional().value(); } - for (int i = 0; i < 12; i++) { + for (int i = 0; i < 12; i++) + { std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0); std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); @@ -31,29 +36,35 @@ void BaseFixedStand::enter() { ctrl_interfaces_.control_inputs_.command = 0; } -void BaseFixedStand::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) { +void BaseFixedStand::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/) +{ percent_ += 1 / duration_; phase = std::tanh(percent_); - for (int i = 0; i < 12; i++) { + for (int i = 0; i < 12; i++) + { std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value( phase * target_pos_[i] + (1 - phase) * start_pos_[i]); } } -void BaseFixedStand::exit() { +void BaseFixedStand::exit() +{ percent_ = 0; } -FSMStateName BaseFixedStand::checkChange() { - if (percent_ < 1.5) { +FSMStateName BaseFixedStand::checkChange() +{ + if (percent_ < 1.5) + { return FSMStateName::FIXEDSTAND; } - switch (ctrl_interfaces_.control_inputs_.command) { - case 1: - return FSMStateName::PASSIVE; - case 2: - return FSMStateName::FIXEDDOWN; - default: - return FSMStateName::FIXEDSTAND; + switch (ctrl_interfaces_.control_inputs_.command) + { + case 1: + return FSMStateName::PASSIVE; + case 2: + return FSMStateName::FIXEDDOWN; + default: + return FSMStateName::FIXEDSTAND; } } diff --git a/libraries/controller_common/src/FSM/StateFixedDown.cpp b/libraries/controller_common/src/FSM/StateFixedDown.cpp index abdfde6..7faa476 100644 --- a/libraries/controller_common/src/FSM/StateFixedDown.cpp +++ b/libraries/controller_common/src/FSM/StateFixedDown.cpp @@ -6,24 +6,29 @@ #include -StateFixedDown::StateFixedDown(CtrlInterfaces &ctrl_interfaces, - const std::vector &target_pos, +StateFixedDown::StateFixedDown(CtrlInterfaces& ctrl_interfaces, + const std::vector& target_pos, const double kp, const double kd) : FSMState(FSMStateName::FIXEDDOWN, "fixed down", ctrl_interfaces), - kp_(kp), kd_(kd) { + kp_(kp), kd_(kd) +{ duration_ = ctrl_interfaces_.frequency_ * 1.2; - for (int i = 0; i < 12; i++) { + for (int i = 0; i < 12; i++) + { target_pos_[i] = target_pos[i]; } } -void StateFixedDown::enter() { - for (int i = 0; i < 12; i++) { - start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_value(); +void StateFixedDown::enter() +{ + for (int i = 0; i < 12; i++) + { + start_pos_[i] = ctrl_interfaces_.joint_position_state_interface_[i].get().get_optional().value(); } ctrl_interfaces_.control_inputs_.command = 0; - for (int i = 0; i < 12; i++) { + for (int i = 0; i < 12; i++) + { std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(start_pos_[i]); std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0); std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0); @@ -32,29 +37,35 @@ void StateFixedDown::enter() { } } -void StateFixedDown::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) { +void StateFixedDown::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/) +{ percent_ += 1 / duration_; phase = std::tanh(percent_); - for (int i = 0; i < 12; i++) { + for (int i = 0; i < 12; i++) + { std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value( phase * target_pos_[i] + (1 - phase) * start_pos_[i]); } } -void StateFixedDown::exit() { +void StateFixedDown::exit() +{ percent_ = 0; } -FSMStateName StateFixedDown::checkChange() { - if (percent_ < 1.5) { +FSMStateName StateFixedDown::checkChange() +{ + if (percent_ < 1.5) + { return FSMStateName::FIXEDDOWN; } - switch (ctrl_interfaces_.control_inputs_.command) { - case 1: - return FSMStateName::PASSIVE; - case 2: - return FSMStateName::FIXEDSTAND; - default: - return FSMStateName::FIXEDDOWN; + switch (ctrl_interfaces_.control_inputs_.command) + { + case 1: + return FSMStateName::PASSIVE; + case 2: + return FSMStateName::FIXEDSTAND; + default: + return FSMStateName::FIXEDDOWN; } } diff --git a/libraries/controller_common/src/FSM/StatePassive.cpp b/libraries/controller_common/src/FSM/StatePassive.cpp index a1be9e6..ede8c5f 100644 --- a/libraries/controller_common/src/FSM/StatePassive.cpp +++ b/libraries/controller_common/src/FSM/StatePassive.cpp @@ -6,37 +6,48 @@ #include -StatePassive::StatePassive(CtrlInterfaces &ctrl_interfaces) : FSMState( - FSMStateName::PASSIVE, "passive", ctrl_interfaces) { +StatePassive::StatePassive(CtrlInterfaces& ctrl_interfaces) : FSMState( + FSMStateName::PASSIVE, "passive", ctrl_interfaces) +{ } -void StatePassive::enter() { - for (auto i: ctrl_interfaces_.joint_torque_command_interface_) { +void StatePassive::enter() +{ + for (auto i : ctrl_interfaces_.joint_torque_command_interface_) + { std::ignore = i.get().set_value(0); } - for (auto i: ctrl_interfaces_.joint_position_command_interface_) { + for (auto i : ctrl_interfaces_.joint_position_command_interface_) + { std::ignore = i.get().set_value(0); } - for (auto i: ctrl_interfaces_.joint_velocity_command_interface_) { + for (auto i : ctrl_interfaces_.joint_velocity_command_interface_) + { std::ignore = i.get().set_value(0); } - for (auto i: ctrl_interfaces_.joint_kp_command_interface_) { + for (auto i : ctrl_interfaces_.joint_kp_command_interface_) + { std::ignore = i.get().set_value(0); } - for (auto i: ctrl_interfaces_.joint_kd_command_interface_) { + for (auto i : ctrl_interfaces_.joint_kd_command_interface_) + { std::ignore = i.get().set_value(1); } ctrl_interfaces_.control_inputs_.command = 0; } -void StatePassive::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) { +void StatePassive::run(const rclcpp::Time&/*time*/, const rclcpp::Duration&/*period*/) +{ } -void StatePassive::exit() { +void StatePassive::exit() +{ } -FSMStateName StatePassive::checkChange() { - if (ctrl_interfaces_.control_inputs_.command == 2) { +FSMStateName StatePassive::checkChange() +{ + if (ctrl_interfaces_.control_inputs_.command == 2) + { return FSMStateName::FIXEDDOWN; } return FSMStateName::PASSIVE;