Class Algorithms#
Defined in File Algorithms.h
Nested Relationships#
Nested Types#
Class Documentation#
-
class Algorithms#
Class with static methods for various system level algorithms.
This is a container class for static methods to compute various multibody algorithms. See Dynamics Computations section for more information.
Public Static Functions
-
static double evalKineticEnergy(const SubTree &st)#
Calculate the overall kinetic energy of the component bodies in the SubTree.
See the System Properties Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
Kinetic energy.
-
static km::Vec3 evalCmLocation(const SubTree &st)#
Return the location of the sub-tree CM from the virtual root.
Return the current location of the center-of-mass frame for the whole sub-tree with respect to the virtual root.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
The position of the CM with respect to the virtual root.
-
static km::SpatialVector evalSpatialMomentum(const SubTree &st)#
Calculate the overall spatial momentum for the SubTree.
The returned value is by default is referenced about, and represented in the virtual root frame. Note that this quantity is conserved in the absence of external forces on the system.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
Spatial momentum.
-
static km::SpatialVector evalExternalSpatialForce(const SubTree &st)#
Calculate the overall external spatial force for for the SubTree about its CM.
The computed value includes external force contributions from all of the force nodes on all the bodies (including active contact nodes) as well as any constraint forces from constraint nodes. The returned value is referenced about, and represented in the sub-tree’s center of mass frame.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
The overall external spatial force.
-
static km::SpatialVector evalCentroidalMomentum(SubTree &st)#
Calculate the centroidal spatial momentum for the SubTree.
This is the same as the sub-tree’s spatial momentum (computed by evalSpatialMomentum()), except that this value is referenced about the sub-tree’s center of mass location, though still represented in the virtual root frame. Note that this quantity is conserved in the absence of external forces on the system.
This method does not support deformable bodies.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
Spatial momentum.
-
static km::Mat evalCentroidalMomentumMatrix(SubTree &st)#
Calculate the centroidal momentum matrix for the SubTree.
This returns the centroidal momentum matrix (CMM) that defines the mapping between the system generalized velocities and the system’s centroidal momentum. Note that this matrix is configuration dependent. This matrix is used in the context the control of legged robots and managing the motion of their CM for stable walking.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
The 6xnU centroidal momentum matrix (CMM).
-
static km::Mat evalTreeMassMatrixCRB(SubTree &st, kc::ks_ptr<CoordData> coord_data = nullptr)#
Calculate the tree system mass matrix using CRB approach.
This method uses the composite rigid body inertias (CRB) recursive method for computing the mass matrix
See the System Properties Algorithms section for more information.
This method does not allow internal 6 dof hinges to avoid ambiguities about whether mass properties should be coupled across such hinges. Also the prescribed motion flag for any subhinge that has been enabled is reset. This is to ensure that the mass matrix computed from this method are consistent with the results from the alternative dynamics based computation methods.
-
static km::Mat evalTreeMassMatrixInvDyn(SubTree &st, kc::ks_ptr<CoordData> coord_data = nullptr)#
Calculate the tree system mass matrix using Newton-Euler inverse dynamics.
Calculate the tree system mass matrix using NE inverse dynamics. A CoordData argument can be specified to set the specific order for the rows and columns in the computed mass matrix.
This method does not currently support deformable bodies.
This method does not allow internal 6 dof hinges to avoid ambiguities about whether mass properties should be coupled across such hinges. Also the prescribed motion flag for any subhinge that has been enabled is reset. This is to ensure that the mass matrix computed from this method are consistent with the results from the alternative dynamics based computation methods.
See the System Properties Algorithms section for more information.
-
static km::Mat evalTreeMassMatrixInvFwdDyn(SubTree &st, kc::ks_ptr<CoordData> coord_data)#
Calculate the tree system mass matrix inverse using forward dynamics.
Calculate the tree system mass matrix inverse using ATBI forward dynamics. A CoordData argument can be specified to set the specific order for the rows and columns in the computed mass matrix inverse. This method currently supports physical rigid and flexible bodies, but not compound bodies.
This method does not allow internal 6 dof hinges to avoid ambiguities about whether mass properties should be coupled across such hinges. Also the prescribed motion flag for any subhinge that has been enabled is reset. This is to ensure that the mass matrix computed from this method are consistent with the results from the alternative dynamics based computation methods.
See the System Properties Algorithms section for more information.
-
static km::Mat evalSerialChainMassMatrixInverse(SubTree &st, kc::ks_ptr<CoordData> coord_data = nullptr)#
Calculate serial-chain system mass matrix inverse using {{ SOA }} operator-based decomposition expressions.
This method does not require the mass matrix or matrix inversions to compute the mass matrix inverse! The algorithm instead takes advantage of an analytical operator based decomposition of the mass matrix inverse to carry out the computation much lower cost.
A CoordData argument can be specified to set the specific order for the rows and columns in the computed mass matrix inverse. This method cannot be used for systems containing deformable bodies. This mass matrix inverse computation can also be done via the more general evalTreeMassMatrixInverse() which also works for deformable bodies. We however also have this method since it is considerably simpler for the simpler serial-chain topology.
This method does not allow internal 6 dof hinges to avoid ambiguities about whether mass properties should be coupled across such hinges. Also the prescribed motion flag for any subhinge that has been enabled is reset. This is to ensure that the mass matrix computed from this method are consistent with the results from the alternative dynamics based computation methods.
See the System Properties Algorithms section for more information.
-
static km::Mat evalTreeMassMatrixInverse(SubTree &st, kc::ks_ptr<CoordData> coord_data = nullptr)#
Calculate the tree system mass matrix inverse using {{ SOA }} decomposition expressions.
This method does not require the mass matrix or matrix inversions to compute the mass matrix inverse! The algorithm instead takes advantage of an analytical operator based decomposition of the mass matrix inverse to carry out the computation much lower cost. For serial-chain system, the mass matrix inverse can also be computed using the evalSerialChainMassMatrixInverse() specifically for such systems.
A CoordData argument can be specified to set the specific order for the rows and columns in the computed mass matrix inverse. This method is currently limited to physical and rigid bodies.
This method supports both rigid and deformable bodies.
This method does not allow internal 6 dof hinges to avoid ambiguities about whether mass properties should be coupled across such hinges. Also the prescribed motion flag for any subhinge that has been enabled is reset. This is to ensure that the mass matrix computed from this method are consistent with the results from the alternative dynamics based computation methods.
See the System Properties Algorithms section for more information.
-
static void evalInverseDynamics(SubTree &st)#
Evaluate Newton-Euler inverse dynamics.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
-
static void evalForwardDynamics(SubTree &st)#
Compute the forward dynamics for a system.
This uses tree-augmented forward dynamics if there are with loop constraints, and tree forward dynamics if there are no loop constraints.
The tree-augmented algorithm solves the graph system forward dynamics using recursive {{ SOA }} recursive algorithms with exact enforcement of the loop constraints. This algorithm does not require the computation of inversion of the mass matrix.
The tree forward dynamics uses Articulated Body Inertia (ATBI) based forward dynamics.
See the Forward Dynamics Algorithms section for more information.
- Parameters:
st – SubTree instance
-
static void evalBaumgarteForwardDynamics(SubGraph &sg, double stiffness, double damping)#
Compute the forward dynamics for a system with loop constraints using Baumgarte method.
This Baumgarte method is included for completeness, however the evalForwardDynamics is the more accurate and better choice for closed-chain dynamics. The Baumgarte method does not enforce the loop constraints exactly, and requires tuning of the stiffness and damping parameters.
See the Forward Dynamics Algorithms section for more information.
- Parameters:
sg – SubGraph instance
stiffness – the Baumgarte stiffness parameter
damping – the Baumgarte damping parameter
-
static km::Vec evalComputedTorque(SubTree &st)#
Calculate the computed torque using NE algorithm.
Method to compute the “computed torque”, i.e. the generalized forces needed - usually for a specific generalized acceleration inputs. This assumes that the state (generalized coordinates (Q) and velocities (U) have been set to the desired values in the multibody system, as have the generalized accelerations (Udot).
This method should not to be used for systems with deformable bodies. Systems with deformable bodies are under-actuated, while computed torque only applies to fully actuated systems. Prescribed subhinges and constraints are ignored in this computation.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
The computed T generalized forces
-
static km::Vec evalGravityCompensation(SubTree &st)#
Compute the gravity compensation torque for the system.
Compute the generalized forces needed to support the gravitational load on the system. This method automatically takes into account additional bodies that are attached or detached from the system.
See the Embedded Control Algorithms section for more information.
- Parameters:
st – SubTree instance
- Returns:
The vector of generalized forces.
-
static km::Mat evalVelocityConstraintMatrix(SubGraph &sg, bool with_constraints)#
Compute the velocity constraint matrix.
Compute the velocity constraint matrix, Gc, such that Gc * U = 0, i.e. the matrix whose null space contains the generalized velocity U values that satisfy the loop and coordinate constraints on the SubGraph.
If “with_constraints” is true, then Gc’s columns include ones for the loop hinge constraint coordinates, and the block row for each constraint contain the mapping to their constraint errors.
On the other hand, if “with_constraints” is false, Gc’s columns exclude ones for the constraint coordinates, and the block row for each loop hinge constraint maps to the residual vector for each constraint. In the latter case, the number of columns and rows shrink equally by the number of constraint coordinates.
Note that the returned matrix may not have full row rank when the constraints are not all independent. The evalIndepVelConstraintCoordIndices() method can be used to check for the rows that are independent.
The SubGraph::constraintErrorAt() and the SubGraph::constraintResidualAt() methods and can be used to look up the constraint instance associated with any specific row index in the returned matrix (based on whether with_constraint is ‘true’ or ‘false’ respectively). Similarly, the SubGraph::coordAt() method can be used to look up the coordinate instance for a specific column index in the returned matrix.
See the Constraints Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
with_constraints – If true, columns for the hinge loop constraint coords are included
- Returns:
The velocity constraint matrix.
-
static std::vector<unsigned int> evalIndepPoseCoordIndices(SubGraph &sg, const std::vector<unsigned int> &available_indices = {})#
Compute the best indices for independent Q coordinates for the bilateral constraints on the system.
For a SubGraph with constraints, only a subset of the Q pose-level coordinates are independent. This method computes the indices of the coordinates in the Q vector that are best choice for independent coordinates (at the current configuration). A non-empty available_indices list can be used to restrict the pool of indices to pick the independent coordinates from.
Since the pose and velocity level constraints may not be the same, the results from this method may not match the constraint and coordinate indices returned by the evalIndepVelConstraintCoordIndices() method.
See the Constraints Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
available_indices – If non-empty, the indices to pick the independent coordinates from
- Returns:
a pair consisting of a list of independent constraint indices, and a list of independent coordinate indices based on the pose-level constraints
-
static std::vector<unsigned int> evalIndepPoseConstraintIndices(SubGraph &sg)#
Compute the indices for best independent bilateral constraints on the system based on the pose gradients matrix map.
For a SubGraph with constraints, only a subset of the Q pose-level constraints may be independent. This method computes the indices of the independent constraints.
Since the pose and velocity level constraints may not be the same, the results from this method may not match the constraint and coordinate indices returned by the evalIndepVelConstraintCoordIndices() method.
See the Constraints Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
- Returns:
a pair consisting of a list of independent constraint indices, and a list of independent coordinate indices based on the pose-level constraints
-
static std::pair<std::vector<unsigned int>, std::vector<unsigned int>> evalIndepVelConstraintCoordIndices(SubGraph &sg, const std::vector<unsigned int> &available_indices = {})#
Compute the best indices for independent constraints and velocity coordinates using the velocity constraint matrix.
For a SubGraph with constraints, only a subset of the U velocity coordinates are independent (and not all constraints may be independent). This method computes the indices of the independent constraints, and the indices of the coordinates in the U vector that are best choice for independent velocity coordinates (at the current configuration).
Since the pose and velocity level constraints may not be the same, the results from this method may not match the constraint and coordinate indices returned by the evalIndepPoseCoordIndices() and evalIndepPoseConstraintIndices() methods.
See the Constraints Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
available_indices – the list of indices to pick the independent ones from
- Returns:
a pair consisting of a list of independent constraint indices, and a list of independent coordinate indices based on the velocity-level constraints
-
static std::pair<std::vector<unsigned int>, km::Mat> evalVelCoordinatePartitioning(SubGraph &sg, const std::vector<unsigned int> &available_indices = {})#
Compute the best indices for independent and dependent velocity coordinates matrix map.
For a SubGraph with constraints, only a subset of the velocity coordinates are independent. This method computes the indices of the coordinates in the U vector that are best choice for independent velocity coordinates (at the current configuration). It also returns a matrix X that can be used to compute the dependent velocity coordinates, U_d, for a set of independent velocity coordinates, U_i, via U_d = X * U_i,
See the Constraints Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
available_indices – the list of indices to pick the independent ones from
- Returns:
The list of independent coordinate indices and the dependent coordinate mapping matrix
-
static km::Mat evalDependentVelCoordinatesMatrix(SubGraph &sg, const std::vector<unsigned int> &indep_indices, bool full = false)#
Compute the dependent velocity coordinates matrix map for the specified set of independent coordinates.
For a SubGraph with constraints, only a subset of the velocity coordinates are independent. This method computes the matrix X that can be used to compute the dependent velocity coordinates, U_d, for the specified set of independent velocity coordinates, U_i, via U_d = X * U_i,
See the Constraints Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
indep_indices – the list of independent U indices
full – if true, the matrix also includes columns for the independent coordinates, else only for the dependent coordinates
- Returns:
the dependent coordinates mapping matrix
-
static km::Mat evalSqueezeMatrix(SubGraph &sg)#
Return a matrix for computing squeeze generalized forces.
Compute the squeeze matrix, S, whose column space consists of “squeeze” generalized forces, i.e. generalized forces that only effect internal forces within the constrained multibody and do not induce any motion. Thus, the generalized force, S*x, for any vector x will not induce any motion. The row dimension is nU(), while the column dimension is the size of the constraints on the system.
Note that the returned matrix may not have full column rank when the constraints are not all independent. The evalIndepVelConstraintCoordIndices() method can be used to check for the columns that are independent.
See the Constraints Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
- Returns:
The squeeze matrix.
-
static km::StateSpace stateSpaceGenerator(SubGraph &sg, size_t n_inputs, size_t n_outputs, std::function<void(const km::Vec &x, const km::Vec &u)> u_fn, km::lin_fn y_fn)#
Create generator for linearized state space representation of the system.
Method to create a StateSpace object for creating (A,B,C,D) state space linear system matrices for sub-tree multibody system. The returned state space generation object’s generate(x,u) can be called to compute the state space matrices at the state and input vector set point values about which the linearization should be carried out.
The first two inputs are the size of the inputs and outputs for the state space system. The u_fn argument is a user-defined function that takes (x,u) arguments and sets the appropriate forces, gravity etc values for the in the multibody system for dynamics computations. The y_fn argument takes (x, u, y) arguments and computes the outputs y for the (x, u) and state and input values.
See the Guidance and Control Related Algorithms section for more information.
- Parameters:
sg – SubGraph instance
n_inputs – number of inputs
n_outputs – number of outputs
u_fn – function to evaluate the state derivative value for an input value
y_fn – function to evaluate the output value for a given state
- Returns:
State space generator instance
-
static km::SpatialInertia evalSpatialInertia(const SubTree &st, kc::ks_ptr<kf::Frame> ref_frame = nullptr)#
Calculate the overall spatial inertia for a sub-tree.
Return the overall spatial inertia of all the bodies in the sub-tree about the reference frame. The multibody virtual root serves as the default reference frame.
See the System Properties Algorithms section for more information.
- Parameters:
st – SubTree instance
ref_frame – reference frame for the spatial inertia (default: the virtual root)
- Returns:
The overall tree spatial inertia
-
static kc::ks_ptr<MultiJacobianGenerator> jacobianGenerator(const std::vector<kc::ks_ptr<kf::FrameToFrame>> &f2fs, kc::ks_ptr<CoordData> cd)#
Return a Jacobian generator for the subtree.
Create a Jacobian generator object for the specified SubGraph and its coordinates and the list source/target frame pairs in the list of frame2frames. Note that the resulting jacobian and pose gradient matrices will exclude columns for any coordinates frozen in the coord data of the subtree.
See the Kinematics Algorithms section for more information.
- Parameters:
f2fs – list of FrameToFrames to compute the Jacobian for
cd – the CoordData that specify the columns in the computed Jacobian
- Returns:
The Jacobian generator instance
-
static kc::ks_ptr<ConstraintKinematicsSolver> constraintKinematicsSolver(SubGraph &sg)#
Return a constraint/inverse kinematics solver instance.
Creates a constraint kinematics (CK) solver for the specified SubGraph and its coordinates and loop constraints. The CK solver uses the CoordData for the SubGraph for the coordinates to solver for. Coordinates can be frozen during the solution process. Only the subhinge coordinates are solved for, and deformation coordinates - if any - are not used.
See the Kinematics Algorithms section for more information.
- Parameters:
sg – SubGraph instance
- Returns:
The constraint kinematics solver instance
-
static ModalAnalysis modalAnalysis(SubTree &st, std::function<void()> deriv_fn, double tol = 1e-3)#
Perform a modal analysis for the sub-tree.
This creates a state space model of the system about the current configuration, and then solves an eigenproblem to get the frequencies and mode shapes.
See the Guidance and Control Related Algorithms section for more information.
- Parameters:
st – The subtree whose bodies should be used for the modal analysis.
deriv_fn – The derivative function to use when computing the StateSpace model used to compute the eigenproblem.
tol – The tolerance used to eliminate rigid-body modes.
- Returns:
the eigen problem solution for the state space modal
-
static km::Mat evalFramesOSCM(SubTree &st, const std::vector<kc::ks_ptr<kf::Frame>> &body_frames, const std::vector<kc::ks_ptr<PhysicalSubhinge>> &subhinges = {})#
Calculate the tree system operational space compliance matrix (OSCM)
The OSCM is defined at J*M^{-1}*J^*, where J is the Jacobian matrix, and M is the mass matrix of the system. This method does not use this brute force expression. Instead it uses the {{ SOA }} recursive methods to do this computation without ever computing the mass matrix or its inverse, or even the Jacobian matrix itself.
In addition, one or more subhinges can also be provided. The returned matrix will include additional mass matrix inverse rows and columns for these subhinges. This is equivalent to computing the OSCM for the “extended Jacobian” defined as [J \ E], where E is a selector matrix that picks out a subset of the subhinges in the system. While the rows and columns associated with the frames define a mapping between spatial forces and spatial accelerations, the subhinge rows and columns define the mapping between generalized forces and generalized accelerations. The off-diagonal sub-blocks define cross-term mappings.
If no subhinges are specified, the returned matrix is the regular OSCM, and if no frames are specified, the returned matrix is the sub-block of the mass matrix inverse with rows and columns for the specified subhinges. This more general form of the OSCM allows users to properly handle a mix of constraints from the task and joint spaces.
See the System Properties Algorithms section for more information.
- Parameters:
st – SubTree instance
body_frames – list of Frame instances to compute the OSCM for
subhinges – list of physical subhinge mass matrix inverse contributions to include
- Returns:
The tree OSCM matrix
-
struct ModalAnalysis#
struct for modal frequency domain analysis
-
static double evalKineticEnergy(const SubTree &st)#