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 Mass Property Algorithms section for more information.
- 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.
- 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.
- Returns:
Spatial momentum.
-
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.
- Returns:
Spatial momentum.
-
static km::Mat evalCentroidalMomentumMatrix(SubTree &st)#
Calculate the centroidal momentum matrix for the SubTree.
This returns the centroidal momentum matrx (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.
- Returns:
The 6xnU centroidal momentum matrx (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 Mass Property 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 Mass Property 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 Mass Property 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 Mass Property 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 Mass Property 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 evalTreeForwardDynamics(SubTree &st)#
Evaluate Articulated Body Inertia (ATBI) based forward dynamics for a tree system.
See the Simulation Algorithms section for more information.
- Parameters:
st – SubTree instance
-
static void evalTAForwardDynamics(SubGraph &sg)#
Compute the Tree-Augmented (TA) forward dynamics for a system with loop constraints.
The TA algorithm solves the graph system forward dynamics using recursive SOA recursive algorithms with exact enforcement of the loop constraints. This algorithm does do not require the computation of inversion of the mass matrix.
See the Simulation Algorithms section for more information.
- Parameters:
sg – SubGraph 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 evalTAForwardDynamics 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 Simulation 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 suhinges 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 – SubTreeh 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 evalIndepConstraintCoordIndices() 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 evalIndepConstraintVelCoordIndices() 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.
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 evalIndepConstraintVelCoordIndices() 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::vector<unsigned int> evalIndependentPoseCoordinates(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 indenpendent ones from
- Returns:
The list of independent coordinate indices and the dependent coordinate mapping matrix
-
static std::pair<std::vector<unsigned int>, std::vector<unsigned int>> evalIndepConstraintVelCoordIndices(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 returne 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 indenpendent 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 indenpendent 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 o f 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 cooridates, 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 evalIndepConstraintVelCoordIndices() 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 Mass Property 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::Frame2Frame>> &f2fs, const std::vector<kc::ks_ptr<CoordData>> &cds)#
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 Frame2Frames to compute the Jacobian for
cds – the list of CoordData instances 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 soultion 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 addition mass matrix inverse rows and colums for these subhinge. This is equivalent to compounting 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 mapping between spatial forces and spatial accelerations, the subhinge rows and columns define the mapping between generalized forces and generalized acceelerations. 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 specifed, 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 Mass Property 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 invers constributions to include
- Returns:
The tree OSCM matrix
-
struct ModalAnalysis#
struct for modal frequency domain analysis
-
static double evalKineticEnergy(const SubTree &st)#