#include <collocation_edges.h>
Public Types | |
using | Ptr = std::shared_ptr< UncompressedCollocationEdge > |
using | UPtr = std::unique_ptr< UncompressedCollocationEdge > |
![]() | |
using | ConstPtr = std::shared_ptr< const Edge > |
using | Ptr = std::shared_ptr< Edge > |
using | UPtr = std::unique_ptr< Edge > |
using | VertexContainer = std::vector< VertexInterface * > |
Typedef to represent the vertex container. More... | |
![]() | |
using | Ptr = std::shared_ptr< BaseEdge > |
using | UPtr = std::unique_ptr< BaseEdge > |
![]() | |
using | Ptr = std::shared_ptr< EdgeInterface > |
using | UPtr = std::unique_ptr< EdgeInterface > |
Public Member Functions | |
void | computeValues (Eigen::Ref< Eigen::VectorXd > values) override |
Compute function values. More... | |
void | finalize () |
int | getDimension () const override |
Get dimension of the edge (dimension of the cost-function/constraint value vector) More... | |
bool | isLeastSquaresForm () const override |
Defines if the edge is formulated as Least-Squares form. More... | |
bool | isLinear () const override |
Return true if the edge is linear (and hence its Hessian is always zero) More... | |
UncompressedCollocationEdge (SystemDynamicsInterface::Ptr dynamics, QuadratureCollocationInterface::Ptr quadrature) | |
![]() | |
Edge ()=delete | |
Edge (int num_vertices) | |
int | getNumVertices () const override |
Return number of attached vertices. More... | |
const VertexInterface * | getVertex (int idx) const override |
VertexInterface * | getVertexRaw (int idx) override |
Get access to vertex with index idx (0 <= idx < numVertices) More... | |
bool | providesJacobian () const override |
Return true if a custom Jacobian is provided (e.g. computeJacobian() is overwritten) More... | |
void | resizeVertexContainer (int n) |
Set number n of vertices attached to this edge. More... | |
void | setVertex (int idx, VertexInterface &vertex) |
int | verticesDimension () const override |
Return the combined dimension of all attached vertices (excluding fixed vertex components) More... | |
![]() | |
virtual void | computeHessian (int vtx_idx_i, int vtx_idx_j, const Eigen::Ref< const Eigen::MatrixXd > &block_jacobian_i, Eigen::Ref< Eigen::MatrixXd > block_hessian_ij, const double *multipliers=nullptr, double weight=1.0) |
virtual void | computeHessianInc (int vtx_idx_i, int vtx_idx_j, const Eigen::Ref< const Eigen::MatrixXd > &block_jacobian_i, Eigen::Ref< Eigen::MatrixXd > block_hessian_ij, const double *multipliers=nullptr, double weight=1.0) |
Compute edge block Hessian for a given vertex pair. More... | |
virtual void | computeHessianInc (int vtx_idx_i, int vtx_idx_j, Eigen::Ref< Eigen::MatrixXd > block_hessian_ij, const double *multipliers=nullptr, double weight=1.0) |
virtual void | computeJacobian (int vtx_idx, Eigen::Ref< Eigen::MatrixXd > block_jacobian, const double *multipliers=nullptr) |
Compute edge block jacobian for a given vertex. More... | |
int | getEdgeIdx () const |
Retrieve current edge index (warning, this value is determined within the related HyperGraph) More... | |
virtual bool | providesHessian () const |
Return true if a custom Hessian is provided (e.g. computeHessian() is overwritten) More... | |
void | reserveCacheMemory (int num_value_vectors, int num_jacobians) |
void | reserveValuesCacheMemory (int num_value_vectors) |
void | reserveJacobiansCacheMemory (int num_jacobians) |
void | computeValuesCached () |
Call computeValues() and store result to previously allocated internal cache (call allocateInteralValuesCache() first once) More... | |
void | computeSquaredNormOfValuesCached () |
compute the specialied squared-norm method for computing the values (note only the first element in the values cache is used) More... | |
EdgeCache & | getCache () |
Retreive values computed previously via computeValuesCached() More... | |
const EdgeCache & | getCache () const |
![]() | |
virtual double | computeSquaredNormOfValues () |
virtual double | computeSumOfValues () |
int | getNumFiniteVerticesLowerBounds () const |
int | getNumFiniteVerticesUpperBounds () const |
virtual | ~EdgeInterface () |
Virtual destructor. More... | |
Protected Member Functions | |
void | clearInternalBuffer () |
Private Attributes | |
QuadratureCollocationInterface::Ptr | _collocation |
int | _dim_x = 0 |
int | _dimension = 0 |
const ScalarVertex * | _dt = nullptr |
SystemDynamicsInterface::Ptr | _dynamics |
Eigen::VectorXd | _dynamics_quadrature |
std::vector< Eigen::VectorXd * > | _intermediate_u |
bool | _intermediate_u_internal_buf = false |
std::vector< Eigen::VectorXd * > | _intermediate_x |
Eigen::VectorXd | _midpoint_error |
int | _num_intermediate_points = 0 |
const VectorVertex * | _u1 = nullptr |
const VectorVertex * | _u2 = nullptr |
const VectorVertex * | _x1 = nullptr |
const VectorVertex * | _x2 = nullptr |
Additional Inherited Members | |
![]() | |
VertexContainer | _vertices |
Vertex container. More... | |
![]() | |
EdgeCache | _cache |
int | _edge_idx = 0 |
Definition at line 1260 of file collocation_edges.h.
using corbo::UncompressedCollocationEdge::Ptr = std::shared_ptr<UncompressedCollocationEdge> |
Definition at line 1263 of file collocation_edges.h.
using corbo::UncompressedCollocationEdge::UPtr = std::unique_ptr<UncompressedCollocationEdge> |
Definition at line 1264 of file collocation_edges.h.
|
inlineexplicit |
Definition at line 1266 of file collocation_edges.h.
|
inlineprotected |
Definition at line 1348 of file collocation_edges.h.
|
inlineoverridevirtual |
Compute function values.
Here, the actual cost/constraint function values are computed:
[in] | values | values should be stored here according to getDimension(). |
Implements corbo::Edge<>.
Definition at line 1326 of file collocation_edges.h.
|
inline |
Definition at line 1274 of file collocation_edges.h.
|
inlineoverridevirtual |
Get dimension of the edge (dimension of the cost-function/constraint value vector)
Implements corbo::Edge<>.
Definition at line 1318 of file collocation_edges.h.
|
inlineoverridevirtual |
Defines if the edge is formulated as Least-Squares form.
Least-squares cost terms are defined as and the function values and Jacobian are computed for
rather than for
. Specialiezed least-squares solvers require the optimization problem to be defined in this particular form. Other solvers can automatically compute the square of least-squares edges if required. However, the other way round is more restrictive: general solvers might not cope with non-least-squares forms.
Note, in the LS-form case computeValues() computes e(x) and otherwise f(x).
Reimplemented from corbo::BaseEdge.
Definition at line 1324 of file collocation_edges.h.
|
inlineoverridevirtual |
Return true if the edge is linear (and hence its Hessian is always zero)
Implements corbo::Edge<>.
Definition at line 1320 of file collocation_edges.h.
|
private |
Definition at line 1359 of file collocation_edges.h.
|
private |
Definition at line 1364 of file collocation_edges.h.
|
private |
Definition at line 1365 of file collocation_edges.h.
|
private |
Definition at line 1376 of file collocation_edges.h.
|
private |
Definition at line 1358 of file collocation_edges.h.
|
private |
Definition at line 1361 of file collocation_edges.h.
|
private |
Definition at line 1369 of file collocation_edges.h.
|
private |
Definition at line 1370 of file collocation_edges.h.
|
private |
Definition at line 1368 of file collocation_edges.h.
|
private |
Definition at line 1362 of file collocation_edges.h.
|
private |
Definition at line 1367 of file collocation_edges.h.
|
private |
Definition at line 1373 of file collocation_edges.h.
|
private |
Definition at line 1375 of file collocation_edges.h.
|
private |
Definition at line 1372 of file collocation_edges.h.
|
private |
Definition at line 1374 of file collocation_edges.h.