Types.h&Types.cpp

Intro: 定义了ocs2的基本数据类型
  • size_array_t / size_array2_t 重命名size_t的一维/二维数组
  • scalar_t / scalar_array_t / scalar_array2_t / scalar_array3_t 重命名double的类型、一维、二维、三维数组
  • vector_t / vector_array_t / vector_array2_t / vector_array3_t 重命名Eigen矩阵类型动态大小列向量、一维、二维、三维数组
  • row_vector_t 重命名Eigen矩阵类型动态大小行向量
  • matrix_t / matrix_array_t / matrix_array2_t / matrix_array3_t 重命名Eigen矩阵类型动态大小矩阵、一维矩阵数组、二维矩阵数组、三维矩阵数组
  • ScalarFunctionLinearApproximation 定义标量函数状态-输入函数的一阶微分近似结构体。
    • 公式：
    • 定义了重载运算符+和*用于结构体运算
    • checkSize()函数：检测该结构体维度正确性
  • ScalarFunctionQuadraticApproximation定义标量函数状态-输入函数的二阶微分近似结构体。
    • 公式：
    • 定义了重载运算符+和*用于结构体运算
    • 定义了一些设置函数
    • checkBeingPSD()函数：检测矩阵是否是有效、自伴随(self-adjoint),和半正定positive semi-definite (PSD))。（参数可以为矩阵、ScalarFunctionQuadraticApproximation）
    • checkSize()函数检测该结构体维度正确性。
  • VectorFunctionLinearApproximation定义向量函数状态-输入函数的一阶微分近似结构体，用于储存约束条件等的一阶微分的参数
    • 公式：
    • 定义了重载运算符+和*用于结构体运算
    • checkSize()函数：检测该结构体维度正确性
  • VectorFunctionQuadraticApproximation定义向量函数状态-输入函数的二阶微分近似结构体，用于储存约束条件等的二阶微分的参数
    • 公式：
    • 定义了重载运算符+和*用于结构体运算

NumericTraits.h

  • 定义了两个模板函数 limitEpsilon() 和 weakEpsilon()，它们分别返回 0.000001 (1e-6)和 0.000000001(1e-9)。
  • limitEpsilon 用于确定浮点数 v 是否在 w 的限定epsilon范围内,如果是,则认为 v 接近于 w。limitEpsilon 的值应该大于 weakEpsilon。（判断两个浮点数是否相等的最大允许差异。）
  • weakEpsilon 定义了在比较时的精度。

ComputationRequest.h

  • Request可以同时请求计算的枚举类
  • RequestSet响应请求集合类，定义了重载运算符+，定义了一些集合操作。
  • 定义了重载运算符&和|，返回均为Request枚举类
  • 定义了重载运算符+，返回均为RequestSet枚举类
  • isUnionEmpty()函数：检测两个Request枚举类交集是否为空操作。

PreComputation.h

PreComputation：预计算的基类（接口，需要继承重构）。
这是一个可选模块，用于在系统动力学、成本和约束项之间共享计算。
在获取值或近似值之前调用虚拟请求回调。回调采用一组请求的计算项，这些计算项必须进行计算并存储在PreComputation对象中。将相同的预计算传递给动力学、成本和约束项的getter方法，这些方法可以利用共享的预计算。
如果不使用预计算，则可以将默认构造的PreComputation（）传递给getter。
  • request：请求；
  • requestPreJump：切换时间请求；
  • requestFinal：终点请求；
  • cast：及其派生类引用显示转换。

misc文件夹

该文件夹主要定义了一些OCS2需要的基本运算。

Benchmark.h

  • 定义了可以重复启动和停止的计时器类。收集所有测量间隔的统计数据（采用std::chrono库）
  • reset()： 重置计时器的统计信息
  • startTimer()：开始计时一个时间间隔，获取当前系统时间作为开始时间
  • endTimer()：结束计时一个时间间隔
  • getNumTimedIntervals()：返回计时的时间间隔数量
  • getTotalInMilliseconds()：返回总时间间隔的累计时间，以毫秒为单位
  • getMaxIntervalInMilliseconds()：返回单个时间间隔的最大时长，以毫秒为单位
  • getLastIntervalInMilliseconds()： 返回上一个计时的时间间隔的时长，以毫秒为单位
  • getAverageInMilliseconds()：返回所有计时时间间隔的平均时长，以毫秒为单位

Collection.h

  • 实现了一个通用的添加和获取接口，用于管理代价函数和约束项的集合
  • empty()：检查集合是否没有元素
  • clear()：从Collection中清除所有元素
  • add()：向集合添加一项
  • erase()：从集合中擦除一项
  • extract()：从Collection中移除一个项，并返回他的unique_ptr

CommandLine.h

实现一个命令行界面,允许用户输入命令并对其进行处理。它使用了多线程和原子操作来确保线程安全,并提供了一种优雅的方式来终止命令行读取线程。
  • getCommandLineString 函数用于从命令行读取一行字符串。它创建一个新线程来读取标准输入流,并使用一个原子布尔变量 lineRead 来表示是否已经读取到输入。函数会一直等待直到读取到输入或者 shouldTerminate 函数返回 true(表示需要终止)。如果 shouldTerminate 返回 true,则会终止程序。最后,函数返回读取到的字符串。
  • stringToWords 函数用于将一个字符串分割成单词,并将单词存储在一个向量中。它利用 istringstream 和 istream_iterator 来完成这个操作。函数返回一个包含所有单词的向量。
测试程序
在terminal 中运行可执行文件，输入hello word，回车，会返回hello \n word

Display.h

定义了一个名为 toDelimitedString 的模板函数,用于将容器中的元素连接成一个用分隔符分隔的字符串。例如,对于一个 std::vector<int> 容器 {1, 2, 3, 4, 5} , toDelimitedString(container) 将返回字符串 "1, 2, 3, 4, 5" 。
测试程序

LinearAlgebra.h

  • setTriangularMinimumEigenvalues()：将三角矩阵的特征值设置为最小幅值(保持符号不变)
  • makePsdEigenvalue()：使用特征值分解使输入矩阵变为正半定矩阵
  • makePsdGershgorin()：于 Gershgorin 圆定理使输入矩阵变为正半定矩阵
  • makePsdCholesky()：基于修改 Cholesky 分解使输入矩阵变为正半定矩阵
  • computeInverseMatrixUUT()：计算输入矩阵的逆的 分解,其中 U 是上三角矩阵
  • computeConstraintProjection()：计算线性约束的约束投影,带有权重 inv(Rm)
  • qrConstraintProjection()：基于QR分解返回线性约束投影,包括投影项和的左伪逆
  • luConstraintProjection()：基于LU分解返回线性约束投影,包括投影项和D^T的左伪逆(如果需要)
  • rank()：计算矩阵的秩
  • eigenvalues()：计算矩阵的复数特征值
  • symmetricEigenvalues()：计算对称矩阵的实数特征值
验证代码

LinearFunction.h

可能用于存储和操作仿射函数,可能在控制或优化领域有应用。
仿射函数(affine function)是指形如 f(x) = Ax + b 的函数,其中 A 是一个矩阵, b 是一个向量,x 是自变量向量。
  • swap函数,交换内部变量。
  • setZero函数,将所有容器设置为0。
  • clear函数,清空所有容器。
  • empty函数,检查time_容器是否为空。
  • size函数,返回控制器的大小(即time_容器的大小)。

LinearInterpolation.h

提供了一套通用的线性插值工具,可以插值标量数据或者结构体数据的子字段,支持不同的插值方式(使用给定索引和alpha或从时间数组推导)。
  • timeSegment()函数用于获取给定查询时间在时间数组中所处的区间索引，并计算插值系数alpha。插值系数 alpha 取值在 [0, 1] 范围内,1表示在区间开始处,0表示在区间结束处。
  • interpolate()函数有几个重载版本:
    • 第一个版本使用提供的索引和alpha对数据数组进行插值。如果数据数组中的大小不相等，则插值将捕捉到最接近查询时间的数据点。
    • 第二个版本在给定查询时间和时间数组、数据数组时,进行线性插值。当存在重复值时，选择较低的范围，即“( ” 。示例：t = [0.0, 1.0, 1.0, 2.0]，当查询tk = 1.0时，选择的范围是 (0.0, 1.0]
    • 第三个版本类似于第一个版本,但允许使用accessFun访问Data类型的子字段进行插值。
    • 第四个版本类似于第二个版本,但也允许使用accessFun访问Data类型的子字段进行插值。

LoadData.h

定义了一些用于加载数据的函数，在加载配置文件等数据时很有用，可以根据需要选择加载不同的数据类型。
  • printValue：用于将一个特定类型的值打印到输出流中，可以选择是否打印字段名称和是否使用默认值。
  • loadPtreeValue：从属性树中加载特定类型的值，并可以选择是否打印提取的值或错误信息。
  • loadCppDataType：从使用 INFO 格式的属性树文件加载 C++ 数据类型的值。
  • loadEigenMatrix：从使用 INFO 格式的属性树文件加载 Eigen 矩阵。
  • loadStdVector：从使用 INFO 格式的属性树文件加载标准向量。
测试程序

LoadStdVectorOfPair.h

定义了一些用于加载数据的辅助函数，特别是用于加载包含多对值的向量的函数。
  • fromString 函数模板，用于将字符串转换为不同类型的值。根据模板特化，这个函数适用于 std::string、std::size_t、int、double 和 float 类型。
  •  ExtendedPair 结构体模板扩展了 std::pair 结构，添加了从字符串读取和转换为字符串的方法。它还定义了一些辅助函数，例如转换操作符、流输出操作符以及 toString 方法。
  • 在命名空间 boost::property_tree 中，通过对 ExtendedPair 结构体进行特化，定义了一个类型转换器，用于在属性树 (property_tree) 库的上下文中自动地将字符串和 ExtendedPair 类型转换为彼此。
  • 在命名空间 ocs2::loadData 中，定义了一个名为 loadStdVectorOfPair 的函数模板。该函数用于从文件中加载一对值的向量，其中每个值对由 T1 和 T2 类型的元素组成。加载的数据的格式是一对值的字符串，可以通过 loadStdVector 函数先加载为 ExtendedPair 的向量，然后通过转换函数将其转换为 std::pair 的向量。

LoadStdVectorOfPair.h

定义了一些用于加载数据的辅助函数，特别是用于加载包含多对值的向量的函数。
  • fromString 函数模板，用于将字符串转换为不同类型的值。根据模板特化，这个函数适用于 std::string、std::size_t、int、double 和 float 类型。
  •  ExtendedPair 结构体模板扩展了 std::pair 结构，添加了从字符串读取和转换为字符串的方法。它还定义了一些辅助函数，例如转换操作符、流输出操作符以及 toString 方法。
  • 在命名空间 boost::property_tree 中，通过对 ExtendedPair 结构体进行特化，定义了一个类型转换器，用于在属性树 (property_tree) 库的上下文中自动地将字符串和 ExtendedPair 类型转换为彼此。
  • 在命名空间 ocs2::loadData 中，定义了一个名为 loadStdVectorOfPair 的函数模板。该函数用于从文件中加载一对值的向量，其中每个值对由 T1 和 T2 类型的元素组成。加载的数据的格式是一对值的字符串，可以通过 loadStdVector 函数先加载为 ExtendedPair 的向量，然后通过转换函数将其转换为 std::pair 的向量。

Numerics.h

定义了一些用于比较浮点数是否接近的函数。
  • almost_eq模板函数它有三个模板参数T1, T2, T3，分别表示x的数据类型、y的数据类型和精度prec的数据类型。用机器精度来比较两个浮点数是否相等。
  • almost_le和almost_ge，它们与almost_eq的实现类似，只是在判断条件中加了一个x < y或x > y的比较，分别用来比较两个浮点数是否小于等于或大于等于另一个浮点数。

Lookup.h

用于查找和索引的辅助函数的实现
  • findFirstIndexWithinTol函数通过线性遍历一个已排序的数据数组，找出与给定值在给定的误差范围内最接近的元素的索引。如果找不到匹配的元素，则抛出一个std::runtime_error异常。
  • findIndexInTimeArray函数用于在已排序的时间数组中查找给定时间的索引。使用std::lower_bound函数实现，返回的索引值是第一个大于等于给定时间的元素的索引。
  • findIntervalInTimeArray函数与findIndexInTimeArray函数类似，不同之处在于返回的是时间间隔的索引，而不是时间点的索引。时间间隔的索引是指给定时间所在的两个时间点之间的间隔索引。如果时间数组为空，则返回0。
  • findActiveIntervalInTimeArray函数与findIntervalInTimeArray函数相似，只是当给定时间等于时间数组的第一个元素时，返回0而不是-1。
  • findBoundedActiveIntervalInTimeArray函数是对findActiveIntervalInTimeArray函数的封装，添加了边界检查。如果时间数组为空或只有一个元素，会抛出std::runtime_error异常。如果给定时间小于起始时间或大于结束时间，也会抛出异常。否则，返回时间间隔的索引。

randomMatrices.h

  • generateSPDmatrix()：生成一个随机的、对称正定且对角线主导的矩阵,不需要指定维数。
  • generateSPDmatrix(int size)：生成一个随机的、对称正定且对角线主导的 size * size 矩阵。
  • generateFullRowRankmatrix(size_t m, size_t n)：生成一个 m * n 的行满秩矩阵,要求 m <= n。
测试程序

reference 参考轨迹 文件夹

ModeSchedule.h & ModeSchedule.cpp

定义了一个名为ModeSchedule的结构体和一些与它相关的函数。ModeSchedule结构体表示了一系列N个模式，每个模式之间由N-1个事件时间分隔。结构体包含以下成员：
  • eventTimes：一个大小为N-1的scalar_t类型的向量，表示N-1个事件的时间。
  • modeSequence：一个大小为N的size_t类型的向量，表示N个模式的序列。
ModeSchedule结构体还包含了一些成员函数和全局函数，如：
  • ModeSchedule()：默认构造函数，创建一个空的ModeSchedule对象。
  • ModeSchedule(std::vector<scalar_t> eventTimesInput, std::vector<size_t> modeSequenceInput)：构造函数，接受一个事件时间向量和一个模式序列向量，并将其赋值给相应的成员变量。
  • size_t modeAtTime(scalar_t time) const：根据给定的查询时间，返回相应的模式。事件按照事件时间划分，如果查询时间与某一个切换时间相等，则返回较低的计数的模式。
  • void clear()：清空ModeSchedule对象，将事件时间和模式序列清空。
  • void swap(ModeSchedule& lh, ModeSchedule& rh)：交换两个ModeSchedule对象的值。
  • std::ostream& operator<<(std::ostream& stream, const ModeSchedule& modeSchedule)：将ModeSchedule对象写入输出流。
  • size_t getNumberOfPrecedingEvents(const scalar_array_t& timeTrajectory, const size_array_t& postEventIndices, scalar_t eventTime)：获取给定事件时间之前已发生的事件数量。
  • std::pair<scalar_t, scalar_t> findIntersectionToExtendableInterval(const scalar_array_t& timeTrajectory, const scalar_array_t& eventTimes, const std::pair<scalar_t, scalar_t>& timePeriod)：找到时间周期与模式涵盖的时间区间（可扩展区间）的交集。

TargetTrajectories.h

定义了一个名为TargetTrajectories的结构体，它是一个接口类，用于用户定义的目标轨迹。
  • explicit TargetTrajectories(size_t size = 0)：构造函数，可以传入一个size参数来指定轨迹的大小，默认为0。
  • TargetTrajectories(scalar_array_t desiredTimeTrajectory, vector_array_t desiredStateTrajectory, vector_array_t desiredInputTrajectory = vector_array_t())：构造函数，接受三个参数，分别是时间轨迹、状态轨迹和输入轨迹。
  • void clear()：清空轨迹，即将时间轨迹、状态轨迹和输入轨迹清空。
  • bool empty() const：判断轨迹是否为空，如果时间轨迹或状态轨迹为空，则返回true，否则返回false。
  • size_t size() const：返回轨迹的大小，即时间轨迹的长度。
  • bool operator==(const TargetTrajectories& other)：判断两个目标轨迹是否相等。
  • bool operator!=(const TargetTrajectories& other)：判断两个目标轨迹是否不相等。
  • vector_t getDesiredState(scalar_t time) const：根据给定的时间值，返回目标状态。
  • vector_t getDesiredInput(scalar_t time) const：根据给定的时间值，返回目标输入。
  • swap函数用于交换两个TargetTrajectories对象，
  • 重载的<<运算符，用于将TargetTrajectories对象输出到流中。
  • 三个公共成员变量：
    •  scalar_array_t timeTrajectory：时间轨迹数组。
    •  vector_array_t stateTrajectory：状态轨迹数组。
    •  vector_array_t inputTrajectory：输入轨迹数组。
      •  

initialization文件夹

该文件夹主要定义了初始化的结构体

Initializer.h

定义了一个名为Initializer的接口类，它是求解器用来初始化在没有控制器可用的时间步骤中的状态和输入的接口类。
  • Initializer() = default;：默认构造函数。
  • virtual ~Initializer() = default;：虚析构函数，用于释放资源。
  • virtual Initializer* clone() const = 0;：纯虚函数，用于在派生类中实现，克隆一个Initializer对象，返回其指针。
  • virtual void compute(scalar_t time, const vector_t& state, scalar_t nextTime, vector_t& input, vector_t& nextState) = 0;：纯虚函数，用于在派生类中实现，根据当前时间、当前状态和下一个时间步骤计算当前输入和下一个状态。

DefaultInitializer.h

定义了一个名为DefaultInitializer的类，它是Initializer接口类的默认实现。该类用于在没有可用控制器的时间步骤中将输入设置为零，将下一个状态设置为当前状态。DefaultInitializer继承自Initializer，并实现了父类中的纯虚函数。包含以下成员函数和成员变量：
  • explicit DefaultInitializer(size_t inputDim)：构造函数，传入输入空间的维度。
  • ~DefaultInitializer() override = default;：析构函数，使用default指定默认实现。
  • DefaultInitializer* clone() const override { return new DefaultInitializer(*this); }：通过复制构造函数来克隆一个DefaultInitializer对象，返回其指针。
  • void compute(scalar_t time, const vector_t& state, scalar_t nextTime, vector_t& input, vector_t& nextState) override：重写了基类的compute函数，根据当前时间、当前状态和下一个时间步骤，将输入置零，并将下一个状态设置为当前状态。
  • protected:：保护部分，包含一个复制构造函数DefaultInitializer(const DefaultInitializer& rhs) = default;，它具有默认的操作。
  • size_t inputDim_;：输入空间的维度。
 

OperatingPoints.h

使用操作轨迹或单个点来进行状态和输入的初始化。
  • OperatingPoints(const vector_t& stateOperatingPoint, const vector_t& inputOperatingPoint)：构造函数，接受一个状态操作点和一个输入操作点来初始化对象。在构造函数中，它将操作点添加到轨迹中。
  • OperatingPoints(scalar_array_t timeTrajectory, vector_array_t stateTrajectory, vector_array_t inputTrajectory)：构造函数，接受时间戳数组、状态轨迹数组和输入轨迹数组来初始化对象。
  • ~OperatingPoints() override = default;：析构函数，使用default指定默认实现。
  • OperatingPoints* clone() const override { return new OperatingPoints(*this); }：通过复制构造函数来克隆一个OperatingPoints对象，返回其指针。
  • void compute(scalar_t time, const vector_t& state, scalar_t nextTime, vector_t& input, vector_t& nextState) override：重写了基类的compute函数。根据当前时间、当前状态和下一个时间步长，使用线性插值在操作轨迹中进行查找，并将相应的输入和下一个状态返回给调用者。
  • const scalar_array_t timeTrajectory_;//时间轨迹 const vector_array_t stateTrajectory_;//状态轨迹 const vector_array_t inputTrajectory_;//输入轨迹
 

constraint 约束条件 文件夹

该文件夹主要定义了约束条件

ConstraintOrder.h

定义了一个名为ConstraintOrder的枚举类，首先是线性约束，然后是二次约束

StateConstraint.h

定义了一个名为StateConstraint的类，它是用于表示仅与状态相关的约束函数的基类。
  • getOrder()：返回约束的阶数（线性或二次）。
  • isActive(scalar_t time)：判断约束在给定时间是否处于活跃状态。默认实现始终返回true。
  • getNumConstraints(scalar_t time)：纯虚函数，用于在派生类中实现，返回给定时间约束向量的大小。
  • getValue()：计算给定时间、给定状态和预计算信息下的约束向量值。
  • getLinearApproximation()：返回给定时间、给定状态和预计算信息下的约束的线性近似。默认实现抛出运行时错误，要求派生类提供具体的实现。
  • getQuadraticApproximation()：返回给定时间、给定状态和预计算信息下的约束的二次近似。默认实现抛出运行时错误，要求派生类提供具体的实现。

StateConstraintCollection.h

定义了一个名为StateConstraint的类，它是用于表示仅与状态相关的约束函数的基类，是一个约束函数的集合类。StateConstraintCollection类继承自Collection<StateConstraint>，并提供了一些方法来获取连接的约束向量和近似。每个约束都可以通过其字符串名称进行访问，并且可以被激活或取消激活。
  • getNumConstraints(scalar_t time)：返回给定时间活动约束向量的大小。
  • getTermsSize()：返回给定时间每一项活动约束数量，如果该项未被激活，则返回0。
  • getValue()：计算给定时间、给定状态和预计算信息下的所有约束数组，如果该项不激活，则相应元素是大小为0的向量。
  • getLinearApproximation()：返回给定时间、给定状态和预计算信息下的约束的线性近似。
  • getQuadraticApproximation()：返回给定时间、给定状态和预计算信息下的约束的二次近似。

LinearStateConstraint.h & LinearStateConstraint.cpp

定义了一个名为LinearStateConstraint的类，表示线性的仅与状态相关的约束。
  • LinearStateConstraint(vector_t h, matrix_t F)：构造函数，接受常量h和状态系数矩阵F作为参数，用于构建线性约束方程。
  • clone()：函数创建并返回一个当前对象的副本。
  • size_t getNumConstraints(scalar_t time) const final;：重写基类的getNumConstraints函数，返回约束数量，为h向量的行数。该函数在此类中被final修饰，表示不可被子类重写。
  • vector_t getValue(scalar_t t, const vector_t& x, const PreComputation& /* preComputation */) const final;：重写基类的getValue函数，计算约束向量的值，将常量项h_加上状态系数矩阵F_与状态向量x的乘积。。该函数在此类中被final修饰，表示不可被子类重写。
  • VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const PreComputation& /* preComputation */) const final;：重写基类的getLinearApproximation函数，返回约束的线性近似，其中f表示约束向量的值，dfdx表示状态约束对状态的导数矩阵。该函数在此类中被final修饰，表示不可被子类重写。

StateInputConstraint.h

表示状态和输入相关的约束函数的基类。
  • explicit StateInputConstraint(ConstraintOrder order)：构造函数，接受一个ConstraintOrder参数，用于指定约束的阶数（线性或二次）。
  • virtual StateInputConstraint* clone() const = 0;：纯虚函数，用于在派生类中实现，克隆一个StateInputConstraint对象，并返回其指针。
  • constexpr ConstraintOrder getOrder() const { return order_; };：成员函数，返回约束的阶数（线性或二次）。
  • virtual bool isActive(scalar_t time) const { return true; }：虚函数，用于判断约束在给定时间是否处于活跃状态。默认实现始终返回true。
  • virtual size_t getNumConstraints(scalar_t time) const = 0;：纯虚函数，用于在派生类中实现，返回给定时间的约束向量的大小。
  • virtual vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const = 0;：纯虚函数，用于在派生类中实现，计算给定时间、给定状态和给定输入的约束向量的值。
  • virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const：虚函数，用于在派生类中实现，返回给定时间、给定状态和给定输入的约束的线性近似。默认实现抛出运行时错误，要求派生类提供具体的实现。
  • virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const：虚函数，用于在派生类中实现，返回给定时间、给定状态和给定输入的约束的二次近似。默认实现抛出运行时错误，要求派生类提供具体的实现。
 

LinearStateInputConstraint.h & LinearStateInputConstraint.cpp

表示状态和输入相关的约束函数的基类。
  • explicit StateInputConstraint(ConstraintOrder order)：构造函数，接受一个ConstraintOrder参数，用于指定约束的阶数（线性或二次）。
  • virtual StateInputConstraint* clone() const = 0;：纯虚函数，用于在派生类中实现，克隆一个StateInputConstraint对象，并返回其指针。
  • constexpr ConstraintOrder getOrder() const { return order_; };：成员函数，返回约束的阶数（线性或二次）。
  • virtual bool isActive(scalar_t time) const { return true; }：虚函数，用于判断约束在给定时间是否处于活跃状态。默认实现始终返回true。
  • virtual size_t getNumConstraints(scalar_t time) const = 0;：纯虚函数，用于在派生类中实现，返回给定时间的约束向量的大小。
  • virtual vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const = 0;：纯虚函数，用于在派生类中实现，计算给定时间、给定状态和给定输入的约束向量的值。
  • virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const：虚函数，用于在派生类中实现，返回给定时间、给定状态和给定输入的约束的线性近似。默认实现抛出运行时错误，要求派生类提供具体的实现。
  • virtual VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const：虚函数，用于在派生类中实现，返回给定时间、给定状态和给定输入的约束的二次近似。默认实现抛出运行时错误，要求派生类提供具体的实现。
 

cost 代价函数 文件夹

该文件夹主要定义了代价函数，包括状态代价函数和状态-输入代价函数。建立了状态代价的基类，实例化了二次状态代价类，实现了状态代价的集合。建立了状态-输入代价的基类，实例化了二次状态-输入代价类，实现了状态-输入代价集合。

StateCost.h

定义了一个名为StateCost的基类，首先是线性约束，然后是二次约束
  • virtual StateCost* clone() const = 0;：定义了一个纯虚函数clone，它需要被任何继承StateCost的子类重写，以创建该成本项的一个副本。
  • virtual bool isActive(scalar_t time) const { return true; }：定义了一个虚函数isActive，用于检查代价项在给定时间是否有效。默认实现总是返回true。
  • virtual scalar_t getValue(...) const = 0;：定义了一个纯虚函数getValue，它需要被继承类重写以计算代价项的值。
  • virtual ScalarFunctionQuadraticApproximation getQuadraticApproximation(...) const = 0;：定义了一个纯虚函数getQuadraticApproximation，它需要被继承类重写以获取代价项的二次近似。
  • StateCost(const StateCost& rhs) = default;：定义了一个默认的拷贝构造函数。

QuadraticStateCost.h & QuadraticStateCost.cpp

定义了一个名为QuadraticStateCost类，继承自StateCost，二次状态代价函数的表达式一般为
  • explicit QuadraticStateCost(matrix_t Q);：定义了一个构造函数，它接受一个matrix_t类型的参数Q，这个参数用于定义成本函数中的二次型项。
  • ~QuadraticStateCost() override = default;：定义了一个默认的虚析构函数。
  • QuadraticStateCost* clone() const override;：重写了StateCost类中的纯虚函数clone，用于创建QuadraticStateCost对象的副本。
  • getValue(...) const final;和getQuadraticApproximation(...) const final;：重写了StateCost类中的纯虚函数，用于计算成本函数的值和二次近似。final关键字表示这些函数在派生类中不能被进一步重写。

StateCostCollection.h & StateCostCollection.cpp

定义了一个名为StateCostCollection的类，继承自Collection模板类，能够将代价函数集合起来，提供了获取代价值总和的方法和二次估计，每一项代价可以通过添加时的名称获取，也可以决定每一项代价是否要被激活。
  • virtual StateCostCollection* clone() const;：声明了一个虚函数clone，它将返回StateCostCollection对象的一个副本。
  • virtual scalar_t getValue(...) const;：声明了一个虚函数getValue，用于计算当前时间、状态和预处理信息下的成本值。通过遍历所有的成本项，并检查它们是否在当前时间激活，来累加计算总成本。
  • virtual ScalarFunctionQuadraticApproximation getQuadraticApproximation(...) const;：声明了一个虚函数getQuadraticApproximation，用于获取成本函数的二次近似。首先找到第一个激活的成本项，然后使用这个成本项的二次近似作为初始值，接着遍历其他激活的成本项，并将它们的二次近似累加到初始值上。

StateInputCost.h

定义了一个名为StateInputCost的抽象类，表示状态输入代价的基类。
  • virtual StateInputCost* clone() const = 0;：声明了一个纯虚函数 clone，它需要在派生类中被重写以返回 StateInputCost 对象的副本。由于此函数是纯虚函数，任何派生类都必须提供其实现，并且 StateInputCost 类因此成为了一个抽象类。
  • virtual bool isActive(scalar_t time) const { return true; }：提供了一个虚函数 isActive，用于检查成本项在给定时间是否有效。默认实现总是返回 true，派生类可以重写此函数以改变行为。
  • virtual scalar_t getValue(...) const = 0;：声明了一个纯虚函数 getValue，它需要在派生类中被重写以计算给定时间、状态、输入、目标轨迹和预处理信息下的成本项的值。
  • virtual ScalarFunctionQuadraticApproximation getQuadraticApproximation(...) const = 0;：声明了一个纯虚函数 getQuadraticApproximation，它需要在派生类中被重写以获取给定时间、状态、输入、目标轨迹和预处理信息下成本项的二次近似。这个二次近似通常用于控制算法中的优化过程。

QuadraticStateInputCost.h & QuadraticStateInputCost.cpp

定义了一个名为QuadraticStateInputCost的类，继承自StateInputCost类，表示二次状态-输入代价。表达式为
  • QuadraticStateInputCost(matrix_t Q, matrix_t R, matrix_t P = matrix_t())：定义了一个构造函数，它接受三个矩阵 Q、R 和 P 作为参数，分别代表状态权重、输入权重和状态-输入交叉项权重。P 参数有默认值 matrix_t()，表示如果没有提供交叉项权重，则默认为一个空矩阵。
  • ~QuadraticStateInputCost() override = default：定义了一个默认的虚析构函数。
  • QuadraticStateInputCost* clone() const override：重写了 StateInputCost 类中的 clone 函数，用于创建 QuadraticStateInputCost 对象的副本。
  • scalar_t getValue(...) const final：重写了 StateInputCost 类中的 getValue 函数，用于计算成本项的值。计算并返回状态偏差和输入偏差（实际值-期望值）的加权和以及它们之间的交叉项的总和。
  • ScalarFunctionQuadraticApproximation getQuadraticApproximation(...) const final：重写了 StateInputCost 类中的 getQuadraticApproximation 函数，用于获取成本项的二次近似。

StateInputCostCollection.h & StateInputCostCollection.cpp

定义了一个 StateInputCostCollection 的类，它继承自 Collection<StateInputCost>，用于组合多个状态-输入代价项，并提供方法来获取总成本值和二次近似。
  • StateInputCostCollection* clone() const override;：重写了 Collection 类中的 clone 函数，用于创建 StateInputCostCollection 对象的副本。
  • virtual scalar_t getValue(...) const;：声明了一个虚函数 getValue，用于计算当前时间、状态、输入、目标轨迹和预处理信息下的总成本值。通过遍历所有的成本项，并检查它们是否在当前时间激活，来累加计算总成本。
  • virtual ScalarFunctionQuadraticApproximation getQuadraticApproximation(...) const;：声明了一个虚函数 getQuadraticApproximation，用于获取总代价的二次近似。首先找到第一个激活的成本项，然后使用这个成本项的二次近似作为初始值，接着遍历其他激活的成本项，并将它们的二次近似累加到初始值上。

control 控制器 文件夹

该文件夹主要定义了前馈、线性控制器

ControllerType.h

定义了一个名为 ControllerType 的枚举类，它用于表示控制器的不同类型。
  • UNKNOWN: 未知类型。这个选项通常用于指示某个对象的控制器类型未知或未定义。
  • FEEDFORWARD: 前馈控制器。前馈控制器通常基于系统的模型来预测未来状态并产生控制输出，而不考虑反馈信息。
  • LINEAR: 线性控制器。线性控制器基于系统的线性模型来生成控制输出，通常通过状态空间方程或线性化技术来设计。
  • ONNX: ONNX控制器。ONNX（Open Neural Network Exchange）是一种用于表示深度学习模型的开放式格式。这个选项可能表示控制器是使用深度学习模型来实现的。
  • BEHAVIORAL: 行为控制器。行为控制器可能是基于规则、状态机、行为树等方式来实现的，通常用于模拟人类或动物的行为。

ControllerBase.h

定义了一个ControllerBase控制器基类
  1. virtual vector_t computeInput(scalar_t t, const vector_t &x) = 0;：这个函数用于计算给定时间t和状态x下的控制输入。纯虚函数。
  1. virtual void concatenate(const ControllerBase *otherController, int index, int length) = 0;：这个函数用于将当前控制器与另一个控制器合并。虚函数，需要派生类实现。
  1. void concatenate(const ControllerBase *otherController)：这个函数是concatenate的重载版本，提供了一个简单的合并接口。
  1. virtual int size() const = 0;：返回控制器的大小。
  1. virtual ControllerType getType() const = 0;：返回控制器的类型。
  1. virtual void clear() = 0;：清除控制器，使其回到空状态。
  1. virtual bool empty() const = 0;：检查控制器是否为空。
  1. virtual ControllerBase *clone() const = 0;：创建控制器的深拷贝。
  1. virtual void display() const {}：用于显示控制器的数据，这里没有实现。
  1. virtual scalar_array_t controllerEventTimes() const { return {}; }：返回控制器设计的事件时间。
  1. virtual void flatten(const scalar_array_t &timeArray, const std::vector<std::vector<float> *> &flatArray2) const：将控制器在给定时间的数据展平为数组结构，用于ROS（机器人操作系统）传输。

FeedforwardController.h & FeedforwardController.cpp

定义了一个名为 FeedforwardController 的类，派生自 ControllerBase 类，用于实现前馈控制逻辑，不能被再次继承。前馈控制是一种不依赖于系统状态反馈的控制策略，通常用于系统建模不准确或存在延迟时。
  1. 类成员变量：
      • timeStamp_：存储控制器时间戳的数组。
      • uffArray_：存储前馈控制输入的数组。
  1. 构造函数：
      • FeedforwardController()默认构造函数：不初始化对象。
      • FeedforwardController(scalar_array_t controllerTime, vector_array_t controllerFeedforward)带参数的构造函数：初始化控制器所需的成员变量，接受时间戳数组和前馈控制输入数组。
      • FeedforwardController(const scalar_array_t &controllerTime, const vector_array_t &stateTrajectory, ControllerBase *controller)从控制器和期望状态轨迹中初始化的构造函数：通过给定的时间戳、状态轨迹和基类控制器，计算并存储前馈控制输入。
      • FeedforwardController(const FeedforwardController &other);拷贝构造函数：复制其他 FeedforwardController 对象的数据。
      • FeedforwardController(FeedforwardController &&other);移动构造函数：通过交换数据来初始化。
  1. 赋值运算符 operator=：使用拷贝和交换（copy-and-swap）技术实现赋值。创建一个临时对象 rhs，然后交换 rhs 和当前对象的内容，最后返回当前对象。
  1. setController 方法：设置控制器的时间戳和前馈控制输入。将输入的时间戳数组和前馈控制输入数组分别赋值给 timeStamp_ 和 uffArray_。
  1. computeInput 方法：重写基类的纯虚函数，使用线性插值计算当前时间 t 下的控制输入。使用线性插值工具 LinearInterpolation::interpolate 计算给定时间 t 下的控制输入。
  1. concatenate 方法：重写基类的纯虚函数，将当前控制器与另一个前馈控制器合并。检查 nextController 是否为 FeedforwardController 类型，且 timeStamp_ 中的最后一个时间戳小于 nextController 的第一个时间戳，插入 nextController 的时间戳和前馈控制输入到当前对象中。
  1. size、getType、clear、empty、clone、display 方法：重写基类的纯虚函数，实现返回 timeStamp_ 的大小、返回控制器类型 FEEDFORWARD、清除控制器的数据、清除控制器的数据、克隆当前控制器、显示控制器内容。
  1. flatten 和 flattenSingle 方法：将控制器数据展平为数组结构，以便于传输或存储。
  1. unFlatten 方法：将展平的数据恢复为 FeedforwardController 对象。
  1. swap 方法：交换两个 FeedforwardController 对象的数据。
  1. 重载了 << 运算符，以便于输出 FeedforwardController 对象的信息。

LinearController.h & LinearController.cpp

定义了一个名为LinearController的类，继承自ControllerBase基类。实现了线性控制器，它是一个时间和状态依赖的控制器，形式为 ，其中是时间相关的反馈增益矩阵，是时间相关的前馈控制输入。
  • 构造函数
    • LinearController() ：默认构造函数，不做任何初始化工作，使用编译器生成的默认实现。
    • LinearController(scalar_array_t controllerTime, vector_array_t controllerBias, matrix_array_t controllerGain)：初始化构造函数，使用成员初始化列表将传入的参数移动到类成员变量中，以避免不必要的拷贝操作。
    • LinearController(const LinearController &other); ：拷贝构造函数，创建 other 对象的副本。调用另一个构造函数，用 other 的时间戳、偏置数组和增益数组初始化当前对象，同时拷贝 other 的 deltaBiasArray_。
    • LinearController(LinearController &&other)：移动构造函数，从 other 对象中移动数据。
  • LinearController &operator=(LinearController rhs)：拷贝赋值运算符，使用拷贝并交换习惯用法。创建一个临时对象 rhs，然后交换 rhs 和当前对象的内容，最后返回当前对象。
  • vector_t computeInput(scalar_t t, const vector_t &x)：计算给定时间和状态下的控制输入。使用线性插值工具计算给定时间 t 下的偏置 uff 和反馈增益 k，然后计算控制输入 uff + k * x。
  • void concatenate(const ControllerBase *nextController, int index, int length)：将另一个控制器的控制输入连接到当前控制器。检查 nextController 是否为 LinearController 类型，且 timeStamp_ 中的最后一个时间戳小于 nextController 的第一个时间戳，插入 nextController 的时间戳、偏置数组和增益数组到当前对象中。
  • size、getType、clear、empty、clone、display 方法：重写基类的纯虚函数，实现返回 timeStamp_ 的大小、返回控制器类型 FEEDFORWARD、清除控制器的数据、清除控制器的数据、克隆当前控制器、显示控制器内容。
  • void getFeedbackGain(scalar_t time, matrix_t &gain)获取给定时间点的反馈增益。使用线性插值工具计算给定时间 time 下的反馈增益 gain。
  • void getBias(scalar_t time, vector_t &bias)：获取给定时间点的偏置。使用线性插值工具计算给定时间 time 下的偏置 bias。
  • scalar_array_t controllerEventTimes() const：返回控制器的事件时间数组。
      1. 检查 timeStamp_ 的大小，如果少于两个时间戳，返回一个空的 scalar_array_t。
      1. 初始化 eventTimes 数组和 lastevent 变量。
      1. 遍历 timeStamp_ 数组，检测事件：
          • 如果两个连续的时间戳差小于某个阈值，表示检测到事件。
          • 如果自上次事件以来的时间足够长，则记录新事件时间。
          • 否则，更新上次事件时间。
  • void flatten(const scalar_array_t &timeArray, const std::vector<std::vector<float> *> &flatArray2) ：扁平化控制器数据。检查 timeArray 和 flatArray2 的大小是否一致；遍历 timeArray，调用 flattenSingle 扁平化单个时间点的控制数据。
  • void flattenSingle(scalar_t time, std::vector<float>& flatArray)：扁平化单个时间点的控制数据。计算给定时间 time 下的偏置 uff 和反馈增益 k ；初始化并清空 flatArray；将 uff 和 k 的值按指定格式存入 flatArray。

StateBasedLinearController.h & StateBasedLinearController.cpp

定义了一个名为 StateBasedLinearController 的类，继承自 ControllerBase 类，声明为 final，意味着不能再被其他类继承。

构造函数和析构函数

  1. 声明程序
    1. 实现程序
      1. 作用
          • 构造函数：初始化 StateBasedLinearController 类的对象。
          • 析构函数：销毁 StateBasedLinearController 类的对象。
      1. 实现方式
          • 使用默认的构造函数和析构函数，由编译器自动生成默认的行为。

      方法：setController

      1. 声明程序
        1. 实现程序
          1. 作用
              • 设置控制器指针并计算该控制器的事件时间。
          1. 实现方式
              • 检查传入的 ctrlPtr 是否为 nullptr，如果是，抛出异常。
              • 将 ctrlPtr_ 设置为传入的控制器指针。
              • 调用 ctrlPtr 的 controllerEventTimes 方法获取事件时间，并将其赋值给 ctrlEventTimes_。

          方法：computeTrajectorySpreadingInput

          1. 声明程序
            1. 实现程序
              1. 作用
                  • 基于轨迹扩展方案计算控制输入。
              1. 实现方式
                  • 获取当前模式并检查事件数量。
                  • 根据事件时间和当前模式计算 tauMinus、tau 和 tauPlus。
                  • 根据 t 和事件时间决定计算控制输入的方式，并返回相应的输入。

              方法：computeInput

              1. 声明程序
                1. 实现程序
                  1. 作用
                      • 计算在给定时间 t 和状态 x 下的控制输入。
                  1. 实现方式
                      • 调用 computeTrajectorySpreadingInput 方法并传入必要参数。

                  方法：concatenate

                  1. 声明程序
                    1. 实现程序
                      1. 作用
                          • 将当前控制器与下一个控制器连接起来。
                      1. 实现方式
                          • 调用 ctrlPtr_ 的 concatenate 方法并传入相应参数。

                      方法：size

                      1. 声明程序
                        1. 实现程序
                          1. 作用
                              • 返回控制器的大小。
                          1. 实现方式
                              • 调用 ctrlPtr_ 的 size 方法并返回结果。

                          方法：getType

                          1. 声明程序
                            1. 实现程序
                              1. 作用
                                  • 返回控制器的类型。
                              1. 实现方式
                                  • 调用 ctrlPtr_ 的 getType 方法并返回结果。

                              方法：clear

                              1. 声明程序
                                1. 实现程序
                                  1. 作用
                                      • 清除控制器中的数据。
                                  1. 实现方式
                                      • 调用 ctrlPtr_ 的 clear 方法。

                                  方法：empty

                                  1. 声明程序
                                    1. 实现程序
                                      1. 作用
                                          • 检查控制器是否为空。
                                      1. 实现方式
                                          • 调用 ctrlPtr_ 的 empty 方法并返回结果。

                                      方法：display

                                      1. 声明程序
                                        1. 实现程序
                                          1. 作用
                                              • 显示控制器的信息。
                                          1. 实现方式
                                              • 调用 ctrlPtr_ 的 display 方法。

                                          方法：clone

                                          1. 声明程序
                                            1. 实现程序
                                              1. 作用
                                                  • 克隆一个新的 StateBasedLinearController 对象。
                                              1. 实现方式
                                                  • 使用拷贝构造函数创建一个新的 StateBasedLinearController 对象并返回。