Class ConstraintKinematicsSolver

Class ConstraintKinematicsSolver#

Inheritance Relationships#

Base Type#

Class Documentation#

class ConstraintKinematicsSolver : public Karana::Core::LockingBase#

Class for solving for coordinates that satisfy closure constraints.

This class is the base class for bodies from which physical and compound body classes are derived. See Constraint kinematics section for more information.

Public Types

enum class CKFrozenCoordType#

Enum with values to classify the type of coordinate to freeze.

Values:

enumerator Q#

only the Q configuration coordinate

enumerator U#

only the U velocity coordinate

enumerator Q_U#

both the Q configuration and U velocity coordinates

using CoordIndices = std::vector<unsigned int>#

alias for list of coordinate indices

Public Functions

ConstraintKinematicsSolver(std::string_view name, kc::ks_ptr<SubGraph> sg)#

Constructor.

Parameters:

  • name – instance name

  • sg – the SubGraph to use for coordinates and constraints

~ConstraintKinematicsSolver()#

Destructor.

double solveQ()#

Solve for the Q generalized coordinates to satisfy constraints.

See Generalized Q, U etc coordinates section for discussion on Q, U etc coordinates.

Returns:

The final error.

double solveU()#

Solve for the U velocity coordinates to satisfy velocity level constraints.

See Generalized Q, U etc coordinates section for discussion on Q, U etc coordinates.

Returns:

The final error.

double solveUdot()#

Solve for the Udot accel coordinates to satisfy accel level constraints.

Solve for the Udot gen accel coordinates to zero out constraints acceleration error. Unlike, solveQ() and solveU(), this method does change the frozen gen accel values.

See Generalized Q, U etc coordinates section for discussion on Q, U etc coordinates.

Returns:

The final error.

void freezeCoord(kc::ks_ptr<CoordBase> coord, size_t index, CKFrozenCoordType type = CKFrozenCoordType::Q_U)#

Method to freeze a coordinate component.

The frozen coordinate element values are not changed by the solveQ(), solveU() and solveUdot() methods used for solving the constraint kinematics.

Parameters:

  • coord – The coordinate whose element is being frozen

  • index – The index for the coordinate element being frozen

  • type – The coordinate type to freeze

void unfreezeCoord(kc::ks_ptr<CoordBase> coord, size_t index, CKFrozenCoordType type = CKFrozenCoordType::Q_U)#

Method to unfreeze a coordinate component.

Parameters:

  • coord – The coordinate whose element is being unfrozen

  • index – The index for the coordinate element being unfrozen

  • type – The coordinate type to unfreeze

void clearFrozenCoords()#

Method to empty out list of Q and U frozen coordinates.

CoordIndices frozenCoords(bool Q_type) const#

Return the list of frozen coordinate indices.

Parameters:

Q_type – If true, the Q frozen coord indices are returned, else the U ones

Returns:

the list of frozen coordinate indices.

km::Vec getNonfrozenQ() const#

Return the non-frozen Q coordinates as an array.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

Array of values

void setNonfrozenQ(const Eigen::Ref<const km::Vec> &Q)#

Set the non-frozen Q coordinates.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

Q – Array of values.

km::Vec getNonfrozenU() const#

Return the non-frozen U velocity coordinates as an array.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

Array of values

void setNonfrozenU(const Eigen::Ref<const km::Vec> &U)#

Set the non-frozen U velocity coordinates.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

U – Array of values.

km::Vec getNonfrozenUdot() const#

Return the non-frozen Udot acceleration coordinates as an array.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Returns:

Array of values

void setNonfrozenUdot(const Eigen::Ref<const km::Vec> &u_dot)#

Set the non-frozen Udot acceleration coordinates.

See Generalized Q, U etc coordinates section for more on Q, U etc generalized coordinates.

Parameters:

u_dot – Array of values.

void dump(std::string_view prefix = "") const#

Print out information about the CK solver.

Parameters:

prefix – the prefix to use for each line of output

bool checkVelJacobian(double tolerance = 1e-6) const#

Verify that the velocity Jacobian values from the analytical and numerical differencing approaches agree.

Parameters:

tolerance – the comparison tolerance to use

Returns:

return true if the Jacobians agree

Public Static Functions

static kc::ks_ptr<ConstraintKinematicsSolver> create(std::string_view name, kc::ks_ptr<SubGraph> sg)#

Factory method to create a ConstraintKinematicsSolver instance.

Creates constraint kinematics solver for the specified SubGraph’s coordinates and constraints. Frozen_coordinates can be set in the SubGraph’s CoordData to specify ones that should be frozen and not changed by the kinematics solver.

Parameters:

  • name – instance name

  • sg – the SubGraph to use for coordinates and constraints

Returns:

new ConstraintKinematicsSolver instance.

Protected Functions

CoordIndices _updateFrozenCoords(CoordIndices &frozen_coords, size_t new_frozen, size_t total_n)#

Helper method to handle internal frozen coords data for new frozen coordinate.

Parameters:

  • frozen_coords – list of indices of frozen coordinates

  • new_frozen – index of newly frozen coordinate

  • total_n – total_n the overall number of coordinate indices

Returns:

the list of nonfrozen indices

CoordIndices _updateNonFrozenCoords(CoordIndices &frozen_coords, size_t new_unfrozen, size_t total_n)#

Helper method to handle internal frozen coords data for new unfrozen coordinate.

Parameters:

  • frozen_coords – list of indices of frozen coordinates

  • new_unfrozen – index of newly unfrozen coordinate

  • total_n – total_n the overall number of coordinate indices

Returns:

the list of nonfrozen indices

inline kc::ks_ptr<km::NonlinearSolver> _nlSolver()#

Return the nonlinear solver instance configured to solver the constraint kinematics problem.

Returns:

the nonlinear solver instance

void _discard(kc::ks_ptr<Base> &base) override#

Discard the provided ConstraintKinematicsSolver.

Parameters:

base – - Base pointer to the ConstraintKinematicsSolver to discard.

virtual void _makeHealthy() override#

Helper method to create the nonlinear solver instance.

void _setup()#

Method to tear down and recreate cached data including the Jacobian generator and the nonlinear solver.

This method is called by _makeHealthy() when the constraints or coord base elements change.

void _makeNlSolver()#

Helper method to create the nonlinear solver member.

void _updateDependentCoordsQ()#

Update the list of dependent Q coordinates to use by the non-linear solver.

Update the list of indices of Q coords that should be used by the non-linear solver to satisfy the coord constraints. This method picks the complement of the best independent Q coord indices. It also skips the frozen coords since they are not supposed to change. This method can be called to update this list of dependent coordinates since the choice is configuration state dependent.

km::Mat _velJacobianNumDiff() const#

Assemble the overall velocity Jacobian matrix for all the constraints using numerical approach.

All subhinge columns are included.

Returns:

the Jacobian matrix

CoordIndices _recomputeNonFrozenCoord(const CoordIndices &frozen_coords, size_t n) const#

Recompute _nonfrozen_indices from _frozen_indices.

Parameters:

  • frozen_coords – list of frozen coordinate indices

  • n – the overall number for coordinate indices

Returns:

the list of non-frozen coordinate indices

Protected Attributes

kc::ks_ptr<SubGraph> _sg#

the SubGraph for this CK solver

kc::ks_ptr<MultiJacobianGenerator> _cutjoint_loops_multi_jacgen = nullptr#

MultiJacobianGenerator for hinge-based loop constraints

kc::ks_ptr<MultiJacobianGenerator> _non_cutjoint_loops_multi_jacgen = nullptr#

MultiJacobianGenerator for non-hinge-based loop constraints

kc::ks_ptr<km::NonlinearSolver> _nl_solver = nullptr#

the nonlinear solver used for inverse kinematics

CoordIndices _dependent_Q_coord_indices#

list of indices of dependent coordinate that are allowed to change when solving for Q values when enforcing the configuration level constraints.

CoordIndices _frozen_indices_Q#

list of indices of Q coordinates that are frozen by the application by calls to freezeCoords(). These are used to compute _nonfrozen_indices that used by solveQ().

CoordIndices _frozen_indices_U#

list of indices of U coordinates that are frozen by the application by calls to freezeCoords(). These are used to compute _nonfrozen_indices that used by solveU() and solveUdot() methods.

CoordIndices _nonfrozen_indices_Q#

list of indices of Q coordinates that are not frozen by the application. These are used by the solveQ() method.

CoordIndices _nonfrozen_indices_U#

list of indices of U coordinates that are not frozen by the application. These are used by the solveU() and solveUdot() methods.

Contents