Class SubGraph

• Defined in File SubGraph.h

Inheritance Relationships

Base Type

• public Karana::Dynamics::SubTree (Class SubTree)

Derived Type

• public Karana::Dynamics::Multibody (Class Multibody)

Class Documentation

class SubGraph : public Karana::Dynamics::SubTree

Represents a subtree of bodies with motion constraints.

This class is a container for a sub-tree of bodies with possible motion constraints on them.

See the Physical bodies and hinges , Creating a tree multibody system , and

Sub-Graphs with Constraints sections for more discussion related to the SubGraph class.

Subclassed by Karana::Dynamics::Multibody

Public Functions

SubGraph(const std::string &name, kc::ks_ptr<BodyBase> root, kc::ks_ptr<SubGraph> parent_subgraph = nullptr)

Constructs a SubGraph.

inline virtual ~SubGraph()

SubGraph destructor.

inline virtual const std::string &typeString() const noexcept override

Returns the type string of LockingBase.

Returns:

The type string.

inline virtual bool isSubGraph() const override

Method to check wether this is a subtree or a subgraph.

Returns:

False if this is a subtree and not a subgraph

const kc::ks_ptr<CoordData> &constraintCoordData() const

Return the CoordData for the loop constraint subhinges See.

The CoordData container section for more on the CoordData class.

Returns:

the constraints CoordData instance

kc::ks_ptr<SubGraph> aggregationSubGraph(const std::string &name, std::vector<kc::ks_ptr<BilateralConstraintBase>> constraints)

Create and return an aggegration subgraph for the list of constraints.

For a set of bilateral constraints, the aggregation sub-graph is the smallest sub-graph that contains their constrained bodies, and is “path-induced” for the underlying sub-graph. A path-induced sub-graph includes all the bodies from the parent sub-graph that are connected to each other via hinges and bilateral constraints (and form a sub-tree if the constraints are ignored). Aggregation sub-graphs are used for multibody system constraint-embedding.

Parameters:

• name – The name for the new subgraph

• constraints – The input list of constraints

Returns:

the aggregation subgraph

kc::ks_ptr<BilateralConstraintBase> getEnabledConstraint(const std::string &name) const

Look up an enabled constraint by name. See.

Bilateral closure constraints section for more info on loop constraints.

Parameters:

name – the constraint’s name

Returns:

the constraint instance, or null if there is no enabled constraint with the specified name

std::vector<kc::ks_ptr<CoordinateConstraint>> getBodyCoordinateConstraints(const PhysicalBody &body) const

Return coordinate constraints connected to a body. See.

Bilateral closure constraints section for more info on constraints.

Parameters:

body – the body instannce

Returns:

the list of coordinate constraints involving the body

std::vector<kc::ks_ptr<LoopConstraintBase>> getBodyLoopConstraints(const PhysicalBody &body) const

Return loop constraints connected to a body. See.

Bilateral closure constraints section for more info on constraints.

Parameters:

body – the body instannce

Returns:

the list of loop constraints involving the body

std::vector<kc::ks_ptr<PhysicalBody>> getLoopConstraintBodies(const LoopConstraintBase &c) const

Return PhysicalBody instances involved in a loop for a loop constraint.

The Karana::Frame::Frame pair associated with a loop constraint define a apir of bodies whose relative motion is constrained. All PhysicalBody instances belonging to the minimal spanning tree containing this pair of bodies is effected by the loop constraint, and ara included in the list of bodies returned by this method. See the

Bilateral closure constraints section for more info on loop constraints.

Parameters:

c – the constraint instannce

Returns:

the list of bodies in the loop

void inheritConstraints()

Add all loop and coordinate constraints from the parent subgraph that are legal for this subgraph.

This method is a helper method to populate a new subgraph with all the constraints from the parent subgraph.

virtual void displayModel(const std::string &prefix = "") const override

Display information about the subtree and its bodies.

Parameters:

prefix – the prefix for each line in the output

virtual std::string dumpString(const std::string &prefix, const Karana::Core::Base::dumpOptions *options) const override

Return a formatted string containing information about this object.

Parameters:

• prefix – String prefix to use for formatting.

• options – Dump options (if null, defaults will be used).

Returns:

A string representation of the object.

virtual std::vector<kc::ks_ptr<CoordData>> coordDataList() const override

Return the list of CoordData for this sub-tree.

The CoordData list contains one for the body subhinges in the the subtree, and another for the body deformation coordinates. See The CoordData container for more on the CoordData class.

Returns:

the list of CoordData

std::vector<kc::ks_ptr<BilateralConstraintBase>> enabledConstraints() const

Return the list of enabled constraints for the subgraph.

This returns the list of constraints that are currently “active” for this supgraph. See Bilateral closure constraints section for more info on enabling/disabling constraints.

Returns:

the list of enabled constraints

void enableConstraint(const kc::ks_ptr<BilateralConstraintBase> constraint)

Enable a constrant for the subgraph.

If not already, add this loop constraint to the enabled list for use in constraint kinematics, and TA dynamics etc algorithms. See Bilateral closure constraints section for more info on enabling/disabling loop constraints.

Parameters:

constraint – the new constraint

void disableConstraint(kc::ks_ptr<BilateralConstraintBase> constraint)

Disable a constrant for the subgraph.

Remove this constraint from the enabled constraints list for this subgraph. See Bilateral closure constraints section for more info on enabling/disabling constraints.

Parameters:

constraint – the constraint

const kc::ks_ptr<ConstraintKinematicsSolver> &cks()

Return the constraint kinematics solver for the registered constraints.

See the Constraint kinematics section for more on the ConstraintKinematicsSolver class.

Returns:

the ConstraintKinematicsSolver instance

virtual const std::pair<kc::ks_ptr<CoordBase>, size_t> coordAt(size_t U_offset) const override

Returns coord obj and its coord offset corresponding to overall U offset.

A SubGraph has multiple CoordData has get/set methods to pack and unpack the coordinate values for its set of CoordBase instances. This method returns the CoordBase and its local coordinate offset for the specified overall U offset value. This method returns the CoorBase and its local offset for the specified overall column offset value in the matrix returned by the Algorithms::evalVelocityConstraintMatrix() method, or the vector returned by the SubGraph::getU() method.

Parameters:

U_offset – the input overall U offset value

Returns:

The CoordBase instance and its local U coord offset

const std::pair<kc::ks_ptr<kc::LockingBase>, size_t> constraintErrorAt(size_t row_offset) const

Return the constraint obj and its local offset in the overall constraint error vector.

The constraint errors are organized as 6 elements for each hinge-loop constraint, followed by scalar entries for the non-hinge loop constraints, and then scalars for each of the coordinate constraints. Given an index value, this method returns the corresponding constraint and its local offset in this vector.

Not that this is also the organization of the rows in the matrices returned by the Algorithms::evalPoseGradientMatrix() and Algorithms::evalVelocityConstraintMatrix(True) methods and the vector returned by the SubGraph::velError() method.

The related constraintResidualAt() method returns the constraint based on the packing order for constraint errors.

Parameters:

row_offset – the overall row offset value

Returns:

The

Returns:

corresponding constraint instance and its local offset

const std::pair<kc::ks_ptr<kc::LockingBase>, size_t> constraintResidualAt(size_t row_offset) const

Return the constraint obj and its local offset in the overall constraint residuals vector.

The constraint residuals are organized as the residual elements for each hinge-loop constraint (6-constraint.hinge().nU() size), followed by scalar entries for the non-hinge loop constraints, and then scalars for each of the coordinate constraints. Given an index value, this method returns the corresponding constraint and its local offset in this vector.

This method returns the constraint and its local offset for the specified overall row offset value in the matrix returned by the Algorithms::evalVelocityConstraintMatrix(False) method.

The related constraintErrorAt() method returns the constraint based on the packing order for constraint errors.

Parameters:

row_offset – the input overall row offset value

Returns:

corresponding constraint instance and its local offset

km::Mat constraintsGc()

Compute the Gc overall constraint matrix for the loop constraints (WIP)

The Gc constraint matrix defines the overall velocity level constraints for the generalized velocity coordinates based on the loop closoure constraints. Thus Gc*U=0 when the velocity constraints are satisfied. Gc itself is Q * Jacobian.

The column space of Gc^* defines the squeeze forces, i.e (Gc^* x Y) generalized forces cause no motion for arbitrary Y.

Returns:

The velocity constraint Gc matrix

km::Mat Gamma()

Compute the OSCM related Gamma matrix for the constraint nodes.

The Gamma matrix is used by the TA constraint dynamics algorithm to compute the Lagrange multipliers for the TA forward dynamics correction step.

Returns:

The Gamma matrix

const kc::ks_ptr<CoordData> graphCoordData() const

Return the subhinge+body+consttraints CoordData for the subgraph.

This CoordData is a combination of the subhinge, body and constraint CoordDatas

See The CoordData container for more on the Coorddata class.

Returns:

the CoordData for the subgraph

void articulateSubhinge(kc::ks_ptr<SubhingeBase> shg, size_t subhinge_index, bool disable_ik = false, double rangeQ = .5, double dQ = .01, double pause = .01)

Sequenatially articulate a regular or loop constraint subhinge for a physical body.

With graphics on, this step can be used to examine and debug issues with the model. If there are enabled loop constraints, this method will invoke the constraint kinematics solver each step to enforce the loop constraints.

Parameters:

• shg – subhinge to articulate

• subhinge_index – the index of the coordinate element to articulate

• disable_ik – if true, constraints solver is not invoked during articulation

• rangeQ – is the excursion angle range (in radians)

• dQ – is the angle step size (in radians)

• pause – time in seconds to sleep between articulation steps

void resetCks()

virtual void dumpTree(const std::string &prefix = "", const dumpTreeOptions options = dumpTreeOptions(false, false, false, false, false)) const override

Display the body tree. The contents of the options struct can be used to control the content and verbosity of the displayed output.

Parameters:

options – Options to tailor the output of dumpTree

km::Vec poseError() const

Return the overall constraints pose error vector for the enabled constraints.

Returns:

the pose error vector

km::Vec velError() const

Return the overall constraint spatial velocity error for the enabled constraints.

Returns:

the spatial velocity error

km::Vec accelError() const

Return the overall constraint spatial acceleration error for the enabled loop constraints.

Returns:

the spatial acceleration error

km::Mat constraintsErrorPoseGradient() const

Return the pose error gradient matrix for all the constraints.

Assemble the overall pose gradient matrix for all the constraint errors wrt to all subgraph coords. The gradient matrix is thus (6*n_hlc+n_nhlc+n_cc, nU) in size, where n_hlc denotes the number of hinge loop constraints, n_nhlc the number of non-hinge loop constraints, and n_cc the number of coordinate constraints. This is used by the solveQ() method of the CK solver.

Returns:

the constraint pose error gradient matrix

km::Mat constraintsErrorVelJacobian() const

Return the velocity error Jacobian matrix for all the constraints.

Assemble the overall velocity Jacobian matrix for all the constraints wrt to all subgraph coords. The Jacobian matrix is thus (6*n_hlc+n_nhlc+n_cc, nU) in size, where n_hlc denotes the number of hinge loop constraints, n_nhlc the number of non-hinge loop constraints, and n_cc the number of coordinate constraints. This is used by the solveU() and solveUdot() methods of the CK solver.

Returns:

the constraint velocity error Jacobian matrix

const std::vector<kc::ks_ptr<LoopConstraintHinge>> &enabledHingeLoopConstraints() const

Return the list of enabled hinge-loop constraints for the subgraph.

This returns the list of hinge-loop constraints that are currently “active” for this supgraph. See Bilateral closure constraints section for more info on enabling/disabling constraints.

Returns:

the list of enabled hinge loop-constraints

const std::vector<kc::ks_ptr<LoopConstraintConVel>> &enabledConVelLoopConstraints() const

Return the list of enabled non-hinge-loop (convel) constraints for the subgraph.

This returns the list of non-hinge-loop constraints that are currently “active” for this supgraph. See Bilateral closure constraints section for more info on enabling/disabling constraints.

Returns:

the list of enabled non-hinge loop-constraints

const std::vector<kc::ks_ptr<CoordinateConstraint>> &enabledCoordinateConstraints() const

Return the list of enabled coordinate constraints for the subgraph.

This returns the list of coordinate constraints that are currently “active” for this supgraph. See Coordinate constraints section for more info on enabling/disabling coordinate constraints.

Returns:

the list of enabled coordinate constraints

size_t nLoopConstraintResiduals() const

Return the size of the residuals vector for loop constraints.

This returns the combined length of the residuals for the hinge and non-hinge loop constraints. See

Coordinate constraints section for more info on enabling/disabling coordinate constraints.

Returns:

the size of the residuals vector

Public Static Functions

static kc::ks_ptr<SubGraph> create(kc::ks_ptr<SubGraph> parent_sg, const std::string &name, kc::ks_ptr<BodyBase> new_root, const std::vector<kc::ks_ptr<BilateralConstraintBase>> &constraints = {}, const std::vector<kc::ks_ptr<BodyBase>> &use_branches = {}, const std::vector<kc::ks_ptr<BodyBase>> &stop_at = {})

Factory method to create a new SubGraph instance. See the SubTree documentation for more information on the bodies that are included in the sub-graph.

The constraints specified in the constraint list arguments are automatically added and enabled in the new sub-graph. There is also the option to instead call the inhertitConstraints() later to add all relevant constraints from the parent subgraph to this subgraph in one swoop.

Parameters:

• parent_sg – The parent SubGraph for the new SubGraph

• name – The name for the new subgraph

• new_root – The virtual root body for the new subgraph

• constraints – The list of bilateral constraints for the subgraph

• use_branches – The list of bodies that define branches whose bodies to include

• stop_at – The list of bodies whose descendants should not be included

Returns:

A new SubGraph instance

static std::vector<unsigned int> dependentIndices(size_t nU, const std::vector<unsigned int> &indep_indices)

Helper method to return the complimentary list of indices in a specified range for the specified list of indices. This method is used to get the list of dependent indices for a list of independent indices.

Protected Functions

kc::ks_ptr<MultiJacobianGenerator> _hingeLoopConstraintsErrorJacobianGen() const

Return the multi-Jacobian generator for the hinge loop constraint error f2fs.

The computed Jacobian and pose gradienat matrices are to all the hinge loop constraint error f2fs from all the coordinates in the subgraph. The matrices are thus (6*n_hlc, nU) in size, where n_hlc denotes the number of hinge-loop constraints. This is used to compute the hinge loop constraint row contributions in the overall Jacobian and pose gradient for the constraint errors wrt to the subgraph coordinates.

Returns:

the multi-jacobian generator

kc::ks_ptr<MultiJacobianGenerator> _nonHingeLoopConstraintsJacobianGen() const

Return the multi-Jacobian generator for the non-hinge loop constraint f2fs.

The computed Jacobian and pose gradienat matrices are to all the non-hinge loop constraint f2fs from all the coordinates in the subgraph. The matrices are thus (6*n_nhlc, nU) in size, where n_nhlc denotes the number of non-hinge-loop constraints. This is used to compute the non-hinge loop constraint row contributions in the overall Jacobian for the constraint velocity errors wrt to the subgraph coordinates.

Returns:

the multi-jacobian generator

km::Mat _getCoordinateConstraintsErrorJacobian() const

return the Jacobian to the scalar error for each of the coordinate constraints wrt all the subgraph coordinates. The Jacobian matrix is thus (n_cc, nU) in size, where n_cc denotes the number of coordinate constraints. This is used to compute the hinge loop constraint row contributions for the overall Jacobian and pose gradient for the constraint errors wrt to the subgraph coordinates.

km::Mat _getNonHingeLoopConstraintsErrorJacobian() const

return the Jacobian to the scalar error for each of the non-hinge loop (convel) constraints wrt all the subgraph coords. The Jacobian matrix is thus (n_nhlc, nU) in size, where n_nhlcc denotes the number of non-hinge loop constraints. This is used to compute the non-hinge loop constraint row contributions to the overall Jacobian for the constraint velocity errors wrt to the subgraph coordinates.

virtual CoordData::CoordOffset _coordOffsets(kc::id_t id) const override

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

assemble the overall velocity Jacobian matrix for all the constraints using numerical approach. All subhinge columns are included.

Parameters:

tolerance – the comparison tolerance to use

Returns:

return true if the Jacobians agree

virtual void _resetSubhingeCharts() override

Reset all physical subhinge chart offsets

virtual void _clearExternals() override

Clear out the external spatial forces, gravity accel and gravity gradient values for all bodies, and all generalized accels and forces. This is used by the StatePropagator within its deriv method before calling the model methods.

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

Discard the provided SubGraph.

Parameters:

base – - Base pointer to the SubGraph to discard.

void _makeConstrainCoordData() const

km::Mat _constraintsQmat() const

Compute the Q constraints matrix for the loop constraints.

The Q matrix defines the overall velocity level constraints for the loop closoure constraints. Its product with the stacked relative spatial velocities across the loop constraints is zero when the velocity constraints are satisfied.

It is used by the TA constraint dynamics algorithm to compute Gamma needed to compute the Lagrange multipliers for the TA forward dynamics correction step.

It is also used to define the “squeeze” matrix for the subgraph.

Returns:

The constraints Q matrix

kc::ks_ptr<CoordinateConstraint> _getEnabledCoordinateConstraint(const std::string &name) const

Look up an enabled coordinate constraint by name. See.

Coordinate constraints section for more info on loop constraints.

Parameters:

name – the coordinate constraint’s name

Returns:

the CoordinateConstraint instance

kc::ks_ptr<LoopConstraintBase> _getEnabledLoopConstraint(const std::string &name) const

Look up an enabled loop constraint by name. See.

Bilateral closure constraints section for more info on loop constraints.

Parameters:

name – the loop constraint’s name

Returns:

the LoopConstraintBase instance

const std::vector<kc::ks_ptr<LoopConstraintBase>> &_enabledLoopConstraints() const

Return the list of enabled loop constraints for the subgraph.

This returns the list of loop constraints that are currently “active” for this supgraph. See Bilateral closure constraints section for more info on enabling/disabling loop constraints.

Returns:

the list of enabled loop constraints

void _enableLoopConstraint(const kc::ks_ptr<LoopConstraintBase> constraint)

Enable a loop constrant for the subgraph.

If not already, add this loop constraint to the enabled list for use in constraint kinematics, and TA dynamics etc algorithms. See Bilateral closure constraints section for more info on enabling/disabling loop constraints.

Parameters:

constraint – the new loop constraint

void _disableLoopConstraint(kc::ks_ptr<LoopConstraintBase> constraint)

Disable a loop constrant for the subgraph.

Remove this loop constraint from the enabled loop constraints list for this subgraph. See Bilateral closure constraints section for more info on enabling/disabling loop constraints.

Parameters:

constraint – the loop constraint

void _enableCoordinateConstraint(const kc::ks_ptr<CoordinateConstraint> constraint)

Enable a coordinate constrant for the subgraph.

If not already, add this coordinate constraint to the enabled list for use in constraint kinematics, and TA dynamics etc algorithms. See Coordinate constraints section for more info on enabling/disabling coordinate constraints.

Parameters:

constraint – the new coordinate constraint

void _disableCoordinateConstraint(kc::ks_ptr<CoordinateConstraint> constraint)

Disable a coordinate constrant for the subgraph.

Remove this coordinate constraint from the enabled coordinate constraints list for this subgraph. See Coordinate constraints section for more info on enabling/disabling coordinate constraints.

Parameters:

constraint – the coordinate constraint

Protected Attributes

kc::RegistryList<LoopConstraintBase> _enabled_loop_constraints_list

Loop constraints enabled for the subgraph. This has both hinge and non-hinge loop constraints

kc::RegistryList<LoopConstraintHinge> _enabled_hinge_loop_constraints_list

Hinge based loop constraints enabled for the subgraph

kc::RegistryList<LoopConstraintConVel> _enabled_nonhinge_loop_constraints_list

Non-hinge loop constraints enabled for the subgraph

kc::RegistryList<CoordinateConstraint> _enabled_coordinate_constraints_list

Compound constraints enabled for the subgraph

size_t _Qrows_loop = 0

the number of rows in the Qmat constraint matrix for loop constraints

size_t _Qrows_coord = 0

the number of rows in the Qmat constraint matrix for coordinate constraints

kc::RegistryList<Node> _loop_constraint_nodes_list

The subset of the loop constraint source/target frames that are loop constraint nodes. Using the registry list will also prevent the same constraint node from being used multiple times by different loop constraints.

kc::RegistryList<PhysicalSubhinge> _coordinate_constraint_subhinges_list

The list of subhinges associated with coordinate constraints. Using the registry list will also prevent the same subhinge from being used multiple times by different coordinate constraints.

mutable kc::ks_ptr<CoordData> _constraint_coord_data = nullptr

CoordData instance to track the coordinates across the loop constraint subhinges in this subgraph

kc::ks_ptr<ConstraintKinematicsSolver> _cks = nullptr

mutable kc::ks_ptr<CoordData> _graph_coord_data = nullptr

merged CoordData instance with the hinge subhinge, body coordinates and hinge loop constraint subhinges. Compared with the _tree_coord_data, this contains the additional loop constraint subhinge coordinates

mutable kc::ks_ptr<MultiJacobianGenerator> _hinge_loops_multi_jacgen = nullptr

MutliJacobianGenerator for hinge-based loop constraints

mutable kc::ks_ptr<MultiJacobianGenerator> _non_hinge_loops_multi_jacgen = nullptr

MutliJacobianGenerator for nonhinge-based loop constraints