Rigid Body Dynamics Library
|
#include <BalanceToolkit.h>
Static Public Member Functions | |
static void | CalculateFootPlacementEstimator (Model &model, Math::VectorNd &q, Math::VectorNd &qdot, Math::Vector3d &pointOnGroundPlane, Math::Vector3d &groundPlaneNormal, FootPlacementEstimatorInfo &fpeInfo, double smallAngularVelocity, bool evaluate_derivatives, bool update_kinematics) |
Definition at line 297 of file BalanceToolkit.h.
|
static |
This function implements the 3D FPE described in Millard et al. and the sensitivity analysis described in the supplementary material of Sloot et al.
Millard M, McPhee J, Kubica E. Foot placement and balance in 3D. Journal of computational and nonlinear dynamics. 2012 Apr 1;7(2).
Sloot LH, Millard M, Werner C, Mombaur K. Slow but Steady: Similar Sit-to-Stand Balance at Seat-Off in Older vs. Younger Adults. Frontiers in sports and active living. 2020;2.
model | The multibody model |
q | The generalized coordinates of the model |
qdot | The generalized velocities of the model |
pointOnGroundPlane | The coordinates of a point on the ground plane in the coordinates of the base frame. |
groundPlaneNormal | The normal direction of the plane, in the coordinates of the base frame. Note that the current version of the FPE assumes that the normal opposes the gravity vector of the model. Under these assumptions the 3D FPE can be evaluated for a flat plane, or a succession of flat planes (like stairs). |
fpeInfo | The struct that contains the FPE location, the numerical accuracy of the solution, the state and parameters of the projected model, the projection error, and all of the derivatives of the FPE which can be used to estimate the accuracy of the FPE for a non-idealized solution. |
smallAngularVelocity | (rad/s) The definition of the n vector requires a non-zero value for the angular momentum about the center-of-mass ground projection point. This results in a numerically undefined quantity if the model is at rest. Instead we let this vector go to zero before that, and set phi to point straight down. Which brings us to the next point: this implementation is not continuous as the system's angular velocity about point P crosses this small angular velocity. |
evaluate_derivatives | The calculations to perform the sensitivity analysis on the solution of the FPE are lengthy scalar equations. Unless you need the derivatives set this option to false to save yourself a bit of computation. |
update_kinematics | Use this to indicate if the model's kinematic transforms need to be updated or not |