EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add perceptive interface, fix compile problems

This commit is contained in:
Huang Zhenbiao 2025-03-20 23:30:23 +08:00
parent c262520093
commit 725f9e9e30
27 changed files with 1336 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
README.md
View File

@ -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/)

12
controllers/ocs2_quadruped_controller/CMakeLists.txt
View File

@ -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

6
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/interface/LeggedInterface.h
View File

@ -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,7 +35,7 @@ namespace ocs2::legged_robot
void setupJointNames(const std::vector<std::string>& joint_names,
const std::vector<std::string>& foot_names);
void setupOptimalControlProblem(const std::string& task_file,
virtual void setupOptimalControlProblem(const std::string& task_file,
const std::string& urdf_file,
const std::string& reference_file,
bool verbose);
@ -66,7 +66,7 @@ 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,
virtual void setupReferenceManager(const std::string& taskFile, const std::string& urdfFile,
const std::string& referenceFile,
bool verbose);

71
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h Normal file
View File

@ -0,0 +1,71 @@
//
// Created by qiayuan on 23-1-2.
//
#pragma once
#include <ocs2_core/reference/ModeSchedule.h>
#include <convex_plane_decomposition/PlanarRegion.h>
#include <convex_plane_decomposition/PolygonTypes.h>
#include <convex_plane_decomposition/SegmentedPlaneProjection.h>
#include <ocs2_centroidal_model/CentroidalModelInfo.h>
#include <ocs2_core/reference/TargetTrajectories.h>
#include <ocs2_legged_robot/common/Types.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
namespace ocs2::legged_robot
{
class ConvexRegionSelector
{
public:
ConvexRegionSelector(CentroidalModelInfo info,
std::shared_ptr<convex_plane_decomposition::PlanarTerrain> PlanarTerrainPtr,
const EndEffectorKinematics<scalar_t>& 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<scalar_t> getMiddleTimes(size_t leg) const { return middleTimes_[leg]; }
std::vector<convex_plane_decomposition::PlanarTerrainProjection> getProjections(size_t leg)
{
return feetProjections_[leg];
}
std::shared_ptr<convex_plane_decomposition::PlanarTerrain> getPlanarTerrainPtr() { return planarTerrainPtr_; }
feet_array_t<scalar_t> getInitStandFinalTimes() { return initStandFinalTime_; }
feet_array_t<std::vector<bool>> extractContactFlags(const std::vector<size_t>& phaseIDsStock) const;
private:
static std::pair<int, int> findIndex(size_t index, const std::vector<bool>& contactFlagStock);
vector3_t getNominalFoothold(size_t leg, scalar_t time, const vector_t& initState,
TargetTrajectories& targetTrajectories);
feet_array_t<std::vector<convex_plane_decomposition::PlanarTerrainProjection>> feetProjections_;
feet_array_t<std::vector<convex_plane_decomposition::CgalPolygon2d>> convexPolygons_;
feet_array_t<std::vector<vector3_t>> nominalFootholds_;
feet_array_t<std::vector<scalar_t>> middleTimes_;
feet_array_t<scalar_t> initStandFinalTime_;
feet_array_t<std::vector<scalar_t>> timeEvents_;
const CentroidalModelInfo info_;
size_t numVertices_;
convex_plane_decomposition::PlanarTerrain planarTerrain_;
std::shared_ptr<convex_plane_decomposition::PlanarTerrain> planarTerrainPtr_;
std::unique_ptr<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
};
} // namespace ocs2::legged_robot

56
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedInterface.h Normal file
View File

@ -0,0 +1,56 @@
//
// Created by qiayuan on 22-12-27.
//
#pragma once
#include <convex_plane_decomposition/PlanarRegion.h>
#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
#include <ocs2_sphere_approximation/PinocchioSphereInterface.h>
#include <grid_map_sdf/SignedDistanceField.hpp>
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<grid_map::SignedDistanceField> getSignedDistanceFieldPtr() const
{
return signedDistanceFieldPtr_;
}
std::shared_ptr<convex_plane_decomposition::PlanarTerrain> getPlanarTerrainPtr() const
{
return planarTerrainPtr_;
}
std::shared_ptr<PinocchioSphereInterface> getPinocchioSphereInterfacePtr() const
{
return pinocchioSphereInterfacePtr_;
}
size_t getNumVertices() const { return numVertices_; }
protected:
size_t numVertices_ = 16;
std::shared_ptr<convex_plane_decomposition::PlanarTerrain> planarTerrainPtr_;
std::shared_ptr<grid_map::SignedDistanceField> signedDistanceFieldPtr_;
std::shared_ptr<PinocchioSphereInterface> pinocchioSphereInterfacePtr_;
};
} // namespace ocs2::legged_robot

45
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedPrecomputation.h Normal file
View File

@ -0,0 +1,45 @@
//
// Created by qiayuan on 23-1-1.
//
#pragma once
#include <ocs2_quadruped_controller/interface/LeggedRobotPreComputation.h>
#include <convex_plane_decomposition/PlanarRegion.h>
#include <convex_plane_decomposition/PolygonTypes.h>
#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<FootPlacementConstraint::Parameter>& getFootPlacementConParameters() const
{
return footPlacementConParameters_;
}
PerceptiveLeggedPrecomputation(const PerceptiveLeggedPrecomputation& rhs);
private:
std::pair<matrix_t, vector_t> getPolygonConstraint(
const convex_plane_decomposition::CgalPolygon2d& polygon) const;
const ConvexRegionSelector* convexRegionSelectorPtr_;
std::vector<FootPlacementConstraint::Parameter> footPlacementConParameters_;
};
} // namespace ocs2::legged_robot

52
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h Normal file
View File

@ -0,0 +1,52 @@
//
// Created by qiayuan on 23-1-3.
//
#pragma once
#include <memory>
#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
#include <ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h>
namespace ocs2::legged_robot
{
class PerceptiveLeggedReferenceManager : public SwitchedModelReferenceManager
{
public:
PerceptiveLeggedReferenceManager(CentroidalModelInfo info, std::shared_ptr<GaitSchedule> gaitSchedulePtr,
std::shared_ptr<SwingTrajectoryPlanner> swingTrajectoryPtr,
std::shared_ptr<ConvexRegionSelector> convexRegionSelectorPtr,
const EndEffectorKinematics<scalar_t>& endEffectorKinematics,
scalar_t comHeight);
const std::shared_ptr<ConvexRegionSelector>& 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<bool>& contactFlagStocks,
std::vector<convex_plane_decomposition::PlanarTerrainProjection>& projections);
std::pair<scalar_array_t, scalar_array_t> getHeights(const std::vector<bool>& contactFlagStocks,
const std::vector<
convex_plane_decomposition::PlanarTerrainProjection>&
projections);
const CentroidalModelInfo info_;
feet_array_t<vector3_t> lastLiftoffPos_;
std::shared_ptr<ConvexRegionSelector> convexRegionSelectorPtr_;
std::unique_ptr<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
scalar_t comHeight_;
};
} // namespace legged

41
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootCollisionConstraint.h Normal file
View File

@ -0,0 +1,41 @@
//
// Created by qiayuan on 23-1-1.
//
#pragma once
#include <ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h>
#include <ocs2_core/constraint/StateConstraint.h>
#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
#include <grid_map_sdf/SignedDistanceField.hpp>
namespace ocs2::legged_robot
{
class FootCollisionConstraint final : public StateConstraint
{
public:
FootCollisionConstraint(const SwitchedModelReferenceManager& referenceManager,
const EndEffectorKinematics<scalar_t>& endEffectorKinematics,
std::shared_ptr<grid_map::SignedDistanceField> 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<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
std::shared_ptr<grid_map::SignedDistanceField> sdfPtr_;
const size_t contactPointIndex_;
const scalar_t clearance_;
};
} // namespace legged

44
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/FootPlacementConstraint.h Normal file
View File

@ -0,0 +1,44 @@
//
// Created by qiayuan on 23-1-1.
//
#pragma once
#include <ocs2_quadruped_controller/interface/SwitchedModelReferenceManager.h>
#include <ocs2_core/constraint/StateConstraint.h>
#include <ocs2_robotic_tools/end_effector/EndEffectorKinematics.h>
namespace ocs2::legged_robot
{
class FootPlacementConstraint final : public StateConstraint
{
public:
struct Parameter
{
matrix_t a;
vector_t b;
};
FootPlacementConstraint(const SwitchedModelReferenceManager& referenceManager,
const EndEffectorKinematics<scalar_t>& 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<EndEffectorKinematics<scalar_t>> endEffectorKinematicsPtr_;
const size_t contactPointIndex_;
const size_t numVertices_;
};
} // namespace legged

39
controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/perceptive_interface/constraint/SphereSdfConstraint.h Normal file
View File

@ -0,0 +1,39 @@
//
// Created by qiayuan on 22-12-27.
//
#pragma once
#include <memory>
#include <ocs2_core/constraint/StateConstraint.h>
#include <ocs2_sphere_approximation/PinocchioSphereKinematics.h>
#include <grid_map_sdf/SignedDistanceField.hpp>
namespace ocs2::legged_robot
{
class SphereSdfConstraint final : public StateConstraint
{
public:
SphereSdfConstraint(const PinocchioSphereKinematics& sphereKinematics,
std::shared_ptr<grid_map::SignedDistanceField> 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<PinocchioSphereKinematics> sphereKinematicsPtr_;
std::shared_ptr<grid_map::SignedDistanceField> sdfPtr_;
size_t numConstraints_{};
};
} // namespace legged

6
controllers/ocs2_quadruped_controller/package.xml
View File

@ -19,6 +19,12 @@
<depend>ocs2_legged_robot_ros</depend>
<depend>angles</depend>
<!--/ Dependencies for Perceptive Mode -->
<depend>convex_plane_decomposition_msgs</depend>
<depend>convex_plane_decomposition_ros</depend>
<depend>grid_map_sdf</depend>
<depend>ocs2_sphere_approximation</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

206
controllers/ocs2_quadruped_controller/src/perceptive_interface/ConvexRegionSelector.cpp Normal file
View File

@ -0,0 +1,206 @@
//
// Created by qiayuan on 23-1-2.
//
#include "ocs2_quadruped_controller/perceptive_interface/ConvexRegionSelector.h"
#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include <ocs2_core/misc/Lookup.h>
#include <ocs2_legged_robot/gait/MotionPhaseDefinition.h>
#include <convex_plane_decomposition/ConvexRegionGrowing.h>
namespace ocs2::legged_robot
{
ConvexRegionSelector::ConvexRegionSelector(CentroidalModelInfo info,
std::shared_ptr<convex_plane_decomposition::PlanarTerrain>
planarTerrainPtr,
const EndEffectorKinematics<scalar_t>& 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<std::vector<int>> startIndices;
feet_array_t<std::vector<int>> finalIndices;
for (size_t leg = 0; leg < info_.numThreeDofContacts; leg++)
{
startIndices[leg] = std::vector<int>(numPhases, 0);
finalIndices[leg] = std::vector<int>(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<std::vector<bool>> ConvexRegionSelector::extractContactFlags(
const std::vector<size_t>& phaseIDsStock) const
{
const size_t numPhases = phaseIDsStock.size();
feet_array_t<std::vector<bool>> contactFlagStock;
std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector<bool>(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<int, int> ConvexRegionSelector::findIndex(size_t index, const std::vector<bool>& 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

136
controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedInterface.cpp Normal file
View File

@ -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 <ocs2_core/misc/LoadData.h>
#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 <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
#include <ocs2_core/soft_constraint/StateSoftConstraint.h>
#include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematicsCppAd.h>
#include <memory>
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<convex_plane_decomposition::PlanarTerrain>();
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<grid_map::SignedDistanceField>();
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<EndEffectorKinematics<scalar_t>> eeKinematicsPtr = getEeKinematicsPtr({footName}, footName);
std::unique_ptr<PenaltyBase> placementPenalty(
new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-2, 1e-4)));
std::unique_ptr<PenaltyBase> collisionPenalty(
new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-2, 1e-3)));
// For foot placement
std::unique_ptr<FootPlacementConstraint> footPlacementConstraint(
new FootPlacementConstraint(*reference_manager_ptr_, *eeKinematicsPtr, i, numVertices_));
problem_ptr_->stateSoftConstraintPtr->add(
footName + "_footPlacement",
std::make_unique<StateSoftConstraint>(std::move(footPlacementConstraint), std::move(placementPenalty)));
// For foot Collision
std::unique_ptr<FootCollisionConstraint> footCollisionConstraint(
new FootCollisionConstraint(*reference_manager_ptr_, *eeKinematicsPtr, signedDistanceFieldPtr_, i,
0.03));
problem_ptr_->stateSoftConstraintPtr->add(
footName + "_footCollision",
std::make_unique<StateSoftConstraint>(std::move(footCollisionConstraint), std::move(collisionPenalty)));
}
// For collision avoidance
scalar_t thighExcess = 0.025;
scalar_t calfExcess = 0.02;
std::vector<std::string> collisionLinks = {"LF_calf", "RF_calf", "LH_calf", "RH_calf"};
const std::vector<scalar_t>& maxExcesses = {calfExcess, calfExcess, calfExcess, calfExcess};
pinocchioSphereInterfacePtr_ = std::make_shared<PinocchioSphereInterface>(
*pinocchio_interface_ptr_, collisionLinks, maxExcesses, 0.6);
CentroidalModelPinocchioMapping pinocchioMapping(centroidal_model_info_);
auto sphereKinematicsPtr = std::make_unique<PinocchioSphereKinematics>(
*pinocchioSphereInterfacePtr_, pinocchioMapping);
std::unique_ptr<SphereSdfConstraint> sphereSdfConstraint(
new SphereSdfConstraint(*sphereKinematicsPtr, signedDistanceFieldPtr_));
// std::unique_ptr<PenaltyBase> penalty(new RelaxedBarrierPenalty(RelaxedBarrierPenalty::Config(1e-3, 1e-3)));
// problem_ptr_->stateSoftConstraintPtr->add(
// "sdfConstraint", std::unique_ptr<StateCost>(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<SwingTrajectoryPlanner>(
loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4);
std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr = getEeKinematicsPtr(
{model_settings_.contactNames3DoF}, "ALL_FOOT");
auto convexRegionSelector =
std::make_unique<ConvexRegionSelector>(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<PerceptiveLeggedPrecomputation>(
*pinocchio_interface_ptr_, centroidal_model_info_, *reference_manager_ptr_->getSwingTrajectoryPlanner(),
model_settings_,
*dynamic_cast<PerceptiveLeggedReferenceManager&>(*reference_manager_ptr_).getConvexRegionSelectorPtr());
}
} // namespace legged

96
controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedPrecomputation.cpp Normal file
View File

@ -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<matrix_t, vector_t> 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

206
controllers/ocs2_quadruped_controller/src/perceptive_interface/PerceptiveLeggedReferenceManager.cpp Normal file
View File

@ -0,0 +1,206 @@
//
// Created by qiayuan on 23-1-3.
//
#include <utility>
#include <ocs2_core/misc/Lookup.h>
#include <ocs2_centroidal_model/AccessHelperFunctions.h>
#include "ocs2_quadruped_controller/perceptive_interface/PerceptiveLeggedReferenceManager.h"
namespace ocs2::legged_robot
{
PerceptiveLeggedReferenceManager::PerceptiveLeggedReferenceManager(CentroidalModelInfo info,
std::shared_ptr<GaitSchedule> gaitSchedulePtr,
std::shared_ptr<SwingTrajectoryPlanner>
swingTrajectoryPtr,
std::shared_ptr<ConvexRegionSelector>
convexRegionSelectorPtr,
const EndEffectorKinematics<scalar_t>&
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<double>(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<scalar_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<bool>& 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<scalar_array_t, scalar_array_t> PerceptiveLeggedReferenceManager::getHeights(
const std::vector<bool>& contactFlagStocks,
const std::vector<convex_plane_decomposition::PlanarTerrainProjection>& 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

60
controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootCollisionConstraint.cpp Normal file
View File

@ -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<scalar_t>& endEffectorKinematics,
std::shared_ptr<grid_map::SignedDistanceField> 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

60
controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/FootPlacementConstraint.cpp Normal file
View File

@ -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<scalar_t>& 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<const PerceptiveLeggedReferenceManager&>(*referenceManagerPtr_).getFootPlacementFlags(time)[
contactPointIndex_];
}
vector_t FootPlacementConstraint::getValue(scalar_t /*time*/, const vector_t& state,
const PreComputation& preComp) const
{
const auto param = cast<PerceptiveLeggedPrecomputation>(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<PerceptiveLeggedPrecomputation>(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

60
controllers/ocs2_quadruped_controller/src/perceptive_interface/constraint/SphereSdfConstraint.cpp Normal file
View File

@ -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<grid_map::SignedDistanceField> 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<PerceptiveLeggedPrecomputation>(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

28
libraries/controller_common/include/controller_common/CtrlInterfaces.h
View File

@ -10,33 +10,34 @@
#include <hardware_interface/loaned_state_interface.hpp>
#include <control_input_msgs/msg/inputs.hpp>
struct CtrlInterfaces {
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
struct CtrlInterfaces
{
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_torque_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_position_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_velocity_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_kp_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
joint_kd_command_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_effort_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_position_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
joint_velocity_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
imu_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
foot_force_state_interface_;
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
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();

18
libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h
View File

@ -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<double> &target_pos,
BaseFixedStand(CtrlInterfaces& ctrl_interfaces,
const std::vector<double>& 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

14
libraries/controller_common/include/controller_common/FSM/FSMState.h
View File

@ -11,20 +11,22 @@
#include <controller_common/CtrlInterfaces.h>
#include <rclcpp/time.hpp>
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

11
libraries/controller_common/include/controller_common/FSM/StateFixedDown.h
View File

@ -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<double> &target_pos,
explicit StateFixedDown(CtrlInterfaces& ctrl_interfaces,
const std::vector<double>& 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;

9
libraries/controller_common/include/controller_common/FSM/StatePassive.h
View File

@ -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;

12
libraries/controller_common/include/controller_common/common/enumClass.h
View File

@ -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

37
libraries/controller_common/src/FSM/BaseFixedStand.cpp
View File

@ -6,22 +6,27 @@
#include <cmath>
BaseFixedStand::BaseFixedStand(CtrlInterfaces &ctrl_interfaces, const std::vector<double> &target_pos,
BaseFixedStand::BaseFixedStand(CtrlInterfaces& ctrl_interfaces, const std::vector<double>& 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,24 +36,30 @@ 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) {
switch (ctrl_interfaces_.control_inputs_.command)
{
case 1:
return FSMStateName::PASSIVE;
case 2:

39
libraries/controller_common/src/FSM/StateFixedDown.cpp
View File

@ -6,24 +6,29 @@
#include <cmath>
StateFixedDown::StateFixedDown(CtrlInterfaces &ctrl_interfaces,
const std::vector<double> &target_pos,
StateFixedDown::StateFixedDown(CtrlInterfaces& ctrl_interfaces,
const std::vector<double>& 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,24 +37,30 @@ 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) {
switch (ctrl_interfaces_.control_inputs_.command)
{
case 1:
return FSMStateName::PASSIVE;
case 2:

35
libraries/controller_common/src/FSM/StatePassive.cpp
View File

@ -6,37 +6,48 @@
#include <iostream>
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;