Robot Dynamics Library
Functions
Hydro Utilities

Functions

void RobotDynamics::HydroUtils::calcCenterOfBuoyancy (Model &model, const Math::VectorNd &q, Math::FramePoint &com, bool update_kinematics=true)
 Computes the Center of Buoyancy (COB) position. More...
 
void RobotDynamics::HydroUtils::calcCenterOfMassAndCenterOfBuoyancy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::Vector3d &com, Math::Vector3d &cob, Math::Vector3d &com_velocity, bool update_kinematics)
 
Math::SpatialForce RobotDynamics::HydroUtils::calcBuoyancyWrenchOnCenterOfMass (Model &model, const Math::VectorNd &q, bool update_kinematics=true)
 Computes the wrench due to buoyancy experienced at the robots center of mass. More...
 
Math::SpatialForce RobotDynamics::HydroUtils::calcDragWrenchOnCenterOfMass (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics=true)
 Computes the wrench due to drag experienced at the robots center of mass. More...
 
double RobotDynamics::HydroUtils::calcSubtreeVolume (Model &model, const unsigned int bodyId)
 Calculates the total volume of the model. More...
 
double RobotDynamics::HydroUtils::calcSubtreeWeightInFluid (Model &model, const unsigned int bodyId)
 Calculate the weight of a kinematic subtree in fluid with density RobotDynamics::Model::fluid_density. More...
 
string RobotDynamics::Utils::getDofName (const Math::SpatialVector &joint_dof)
 get string abbreviation for dof name from spatial vector. More...
 
string RobotDynamics::Utils::getBodyName (const RobotDynamics::Model &model, unsigned int body_id)
 get body name, returns empty string if bodyid is virtual and has multiple child bodies More...
 
std::string RobotDynamics::Utils::getModelHierarchy (const Model &model)
 Creates a human readable overview of the model. More...
 
std::string RobotDynamics::Utils::getModelDOFOverview (const Model &model)
 Creates a human readable overview of the Degrees of Freedom. More...
 
std::string RobotDynamics::Utils::getNamedBodyOriginsOverview (Model &model)
 Creates a human readable overview of the locations of all bodies that have names. More...
 
void RobotDynamics::Utils::calcCenterOfMass (Model &model, const Math::VectorNd &q, Math::Vector3d &com, bool update_kinematics=true)
 Computes the Center of Mass (COM) position. More...
 
void RobotDynamics::Utils::calcCenterOfMass (Model &model, const Math::VectorNd &q, Math::FramePoint &com, bool update_kinematics=true)
 Computes the Center of Mass (COM) position. More...
 
void RobotDynamics::Utils::calcCenterOfMass (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::Vector3d &com, Math::Vector3d *com_velocity=NULL, Math::Vector3d *angular_momentum=NULL, bool update_kinematics=true)
 Computes the Center of Mass (COM) and optionally its linear velocity. More...
 
void RobotDynamics::Utils::calcCenterOfMass (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::Vector3d &com, Math::Vector3d &com_velocity, bool update_kinematics=true)
 Computes the Center of Mass (COM) and its linear velocity. More...
 
void RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::Vector3d &com_vel, bool update_kinematics=true)
 Computes the Center of Mass (COM) velocity in world frame. More...
 
void RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::FrameVector &com_vel, bool update_kinematics=true)
 Computes the Center of Mass (COM) velocity in world frame. More...
 
void RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, Math::Vector3d &com_vel)
 Computes the Center of Mass (COM) velocity in world frame. More...
 
void RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, Math::FrameVector &com_vel)
 Computes the Center of Mass (COM) velocity in world frame. More...
 
double RobotDynamics::Utils::calcPotentialEnergy (Model &model, const Math::VectorNd &q, bool update_kinematics=true)
 Computes the potential energy of the full model. More...
 
double RobotDynamics::Utils::calcKineticEnergy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics=true)
 Computes the kinetic energy of the full model. More...
 
Math::SpatialForce RobotDynamics::Utils::calcGravityWrenchOnCenterOfMass (Model &model, const Math::VectorNd &q, bool update_kinematics=true)
 Computes the gravitational wrench experienced on the robots center of mass. More...
 
void RobotDynamics::Utils::calcCenterOfMassJacobian (Model &model, const Math::VectorNd &q, Math::MatrixNd &jCom, bool update_kinematics=true)
 Computes the matrix $J_{com}$ such that $v_{com} = J_{com} \dot{q} $. More...
 
Math::Vector3d RobotDynamics::Utils::calcSubtreeCenterOfMassScaledByMass (Model &model, const unsigned int bodyId, const Math::VectorNd &q, bool updateKinematics=true)
 Calculates the center of mass of a subtree starting with the body with ID bodyId and scales it by the total mass of the subtree. More...
 
double RobotDynamics::Utils::calcSubtreeMass (Model &model, const unsigned int bodyId)
 Calculates the total mass of the subtree beginning with body bodyId and traversing outwards from there. More...
 
void RobotDynamics::Utils::calcCentroidalMomentumMatrix (Model &model, const Math::VectorNd &q, Math::MatrixNd &A, bool update_kinematics=true)
 Calculates the centroidal momentum matrix, $ A(q) $ for a model. The centroidal momentum matrix is a $ 6 \times N $ matrix such that the 6dof centroidal momentum vector is computed by,. More...
 
void RobotDynamics::Utils::calcCentroidalMomentumMatrix (Model &model, const Math::VectorNd &q, Math::MatrixNd &A, Math::Vector3d com, bool update_kinematics=true)
 Calculates the centroidal momentum matrix, $ A(q) $ for a model. The centroidal momentum matrix is a $ 6 \times N $ matrix such that the 6dof centroidal momentum vector is computed by,. More...
 
void RobotDynamics::Utils::calcCentroidalMomentumMatrixDot (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::MatrixNd &Adot, Math::Vector3d com, Math::Vector3d com_velocity, bool update_kinematics=true)
 Calculates the time derivative of the centroidal momentum matrix, i.e. the matrix computed by RobotDynamics::Utils::calcCentroidalMomentumMatrix and stores it in the Adot argument. More...
 
void RobotDynamics::Utils::calcCentroidalMomentumMatrixDot (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::MatrixNd &Adot, bool update_kinematics=true)
 Calculates the time derivative of the centroidal momentum matrix, i.e. the matrix computed by RobotDynamics::Utils::calcCentroidalMomentumMatrix and stores it in the Adot argument. More...
 
void RobotDynamics::Utils::calcCentroidalMomentumMatrixAndMatrixDot (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::MatrixNd &A, Math::MatrixNd &Adot, Math::Vector3d com, Math::Vector3d com_velocity, bool update_kinematics=true)
 Calculates the centroidal momentum matrix and its time derivative, but in a more efficient way than calling the functions to compute them separately. If you need both matrices it will be more efficient to use this function. More...
 
void RobotDynamics::Utils::calcCentroidalMomentumMatrixAndMatrixDot (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::MatrixNd &A, Math::MatrixNd &Adot, bool update_kinematics=true)
 Calculates the centroidal momentum matrix and its time derivative, but in a more efficient way than calling the functions to compute them separately. If you need both matrices it will be more efficient to use this function. More...
 

Detailed Description

Utility functions are those not necessarily required for kinematics/dynamics, but provide utility by giving model information, calculating useful quantities such as the position/velocity of the center of mass, etc.

Function Documentation

◆ calcBuoyancyWrenchOnCenterOfMass()

Math::SpatialForce RobotDynamics::HydroUtils::calcBuoyancyWrenchOnCenterOfMass ( Model model,
const Math::VectorNd q,
bool  update_kinematics = true 
)

Computes the wrench due to buoyancy experienced at the robots center of mass.

Parameters
model
q
update_kinematics(optional) Defaults to true
Note
The reference frame the resulting force is expressed in is the center of mass reference frame owned by the RobotDynamics::Model object. This means that you must first have the position/orientation of that reference frame updated prior to calling this function otherwise the frame it's expressed in is likely to not be what you want.

◆ calcCenterOfBuoyancy()

void RobotDynamics::HydroUtils::calcCenterOfBuoyancy ( Model model,
const Math::VectorNd q,
Math::FramePoint com,
bool  update_kinematics = true 
)

Computes the Center of Buoyancy (COB) position.

Parameters
modelThe model for which we want to compute the COM
qThe current joint positions
cob(output) location of the Center of Buoyancy of the model in world frame
update_kinematics(optional input) whether the kinematics should be updated (defaults to true)

◆ calcCenterOfMass() [1/4]

void RobotDynamics::Utils::calcCenterOfMass ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::Vector3d com,
Math::Vector3d com_velocity,
bool  update_kinematics = true 
)

Computes the Center of Mass (COM) and its linear velocity.

Parameters
modelThe model for which we want to compute the COM
qThe current joint positions
qdotThe current joint velocities
com(output) location of the Center of Mass of the model in world frame
com_velocitylinear velocity of the COM in world frame
update_kinematics(optional input) whether the kinematics should be updated (defaults to true)

◆ calcCenterOfMass() [2/4]

void RobotDynamics::Utils::calcCenterOfMass ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::Vector3d com,
Math::Vector3d com_velocity = NULL,
Math::Vector3d angular_momentum = NULL,
bool  update_kinematics = true 
)

Computes the Center of Mass (COM) and optionally its linear velocity.

When only interested in computing the location of the COM you can use nullptr as value for com_velocity.

Parameters
modelThe model for which we want to compute the COM
qThe current joint positions
qdotThe current joint velocities
com(output) location of the Center of Mass of the model in world frame
com_velocity(optional output) linear velocity of the COM in world frame
angular_momentum(optional output) angular momentum of the model at the COM in world frame
update_kinematics(optional input) whether the kinematics should be updated (defaults to true)

◆ calcCenterOfMass() [3/4]

void RobotDynamics::Utils::calcCenterOfMass ( Model model,
const Math::VectorNd q,
Math::FramePoint com,
bool  update_kinematics = true 
)

Computes the Center of Mass (COM) position.

Parameters
modelThe model for which we want to compute the COM
qThe current joint positions
com(output) location of the Center of Mass of the model in world frame
update_kinematics(optional input) whether the kinematics should be updated (defaults to true)

◆ calcCenterOfMass() [4/4]

void RobotDynamics::Utils::calcCenterOfMass ( Model model,
const Math::VectorNd q,
Math::Vector3d com,
bool  update_kinematics = true 
)

Computes the Center of Mass (COM) position.

Parameters
modelThe model for which we want to compute the COM
qThe current joint positions
com(output) location of the Center of Mass of the model in world frame
update_kinematics(optional input) whether the kinematics should be updated (defaults to true)
Note
Updates the transforms of Model::comFrame

◆ calcCenterOfMassAndCenterOfBuoyancy()

void RobotDynamics::HydroUtils::calcCenterOfMassAndCenterOfBuoyancy ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::Vector3d com,
Math::Vector3d cob,
Math::Vector3d com_velocity,
bool  update_kinematics 
)

◆ calcCenterOfMassJacobian()

void RobotDynamics::Utils::calcCenterOfMassJacobian ( Model model,
const Math::VectorNd q,
Math::MatrixNd jCom,
bool  update_kinematics = true 
)

Computes the matrix $J_{com}$ such that $v_{com} = J_{com} \dot{q} $.

Parameters
modelThe model for which the COM jacobian will be computed for
qThe current joint positions
jComA 3 x model.qdot_size matrix where the jacobian will be stored.
update_kinematicsIf true, kinematic variables will be computed. Default = true.

◆ calcCenterOfMassVelocity() [1/4]

void RobotDynamics::Utils::calcCenterOfMassVelocity ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::FrameVector com_vel,
bool  update_kinematics = true 
)

Computes the Center of Mass (COM) velocity in world frame.

Parameters
modelThe model for which we want to compute the COM velocity
qThe current joint positions
qdotThe current joint velocities
com_vel(modified) Frame vector where the result vector and frame will be stored
update_kinematics(optional input) whether the kinematics should be updated (defaults to true)

◆ calcCenterOfMassVelocity() [2/4]

void RobotDynamics::Utils::calcCenterOfMassVelocity ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::Vector3d com_vel,
bool  update_kinematics = true 
)

Computes the Center of Mass (COM) velocity in world frame.

Parameters
modelThe model for which we want to compute the COM velocity
qThe current joint positions
qdotThe current joint velocities
com_vel(modified) translational velocity of the Center of Mass of the model in world frame
update_kinematics(optional input) whether the kinematics should be updated (defaults to true)

◆ calcCenterOfMassVelocity() [3/4]

void RobotDynamics::Utils::calcCenterOfMassVelocity ( Model model,
Math::FrameVector com_vel 
)

Computes the Center of Mass (COM) velocity in world frame.

Parameters
modelThe model for which we want to compute the COM velocity
com_vel(modified) Frame vector where the result vector and frame will be stored

◆ calcCenterOfMassVelocity() [4/4]

void RobotDynamics::Utils::calcCenterOfMassVelocity ( Model model,
Math::Vector3d com_vel 
)

Computes the Center of Mass (COM) velocity in world frame.

Parameters
modelThe model for which we want to compute the COM velocity
com_vel(modified) Vector3d where the result vector and frame will be stored

◆ calcCentroidalMomentumMatrix() [1/2]

void RobotDynamics::Utils::calcCentroidalMomentumMatrix ( Model model,
const Math::VectorNd q,
Math::MatrixNd A,
bool  update_kinematics = true 
)

Calculates the centroidal momentum matrix, $ A(q) $ for a model. The centroidal momentum matrix is a $ 6 \times N $ matrix such that the 6dof centroidal momentum vector is computed by,.

\[ h = A(q) \dot{q} \]

Note
It is crucial that the $ A $ matrix be all zeros when this function is called, as the elements will be added to. To be sure, call A.setZero() before calling this function.
Parameters
modelRDL model
qVector of joint positions
A6 by N matrix where the result will be stored
update_kinematicsIf true, calculates kinematic parameters
Note
this function will compute the center of mass. If you already have this quantity it will be more efficient to use the function that takes this as an argument so it doesn't recompute them

◆ calcCentroidalMomentumMatrix() [2/2]

void RobotDynamics::Utils::calcCentroidalMomentumMatrix ( Model model,
const Math::VectorNd q,
Math::MatrixNd A,
Math::Vector3d  com,
bool  update_kinematics = true 
)

Calculates the centroidal momentum matrix, $ A(q) $ for a model. The centroidal momentum matrix is a $ 6 \times N $ matrix such that the 6dof centroidal momentum vector is computed by,.

\[ h = A(q) \dot{q} \]

Note
It is crucial that the $ A $ matrix be all zeros when this function is called, as the elements will be added to. To be sure, call A.setZero() before calling this function.
Parameters
modelRDL model
qVector of joint positions
A6 by N matrix where the result will be stored
comPosition of the center of mass in world coordinates
update_kinematicsIf true, calculates kinematic parameters

◆ calcCentroidalMomentumMatrixAndMatrixDot() [1/2]

void RobotDynamics::Utils::calcCentroidalMomentumMatrixAndMatrixDot ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::MatrixNd A,
Math::MatrixNd Adot,
bool  update_kinematics = true 
)

Calculates the centroidal momentum matrix and its time derivative, but in a more efficient way than calling the functions to compute them separately. If you need both matrices it will be more efficient to use this function.

Parameters
modelrdl model
qvector of position
qdotvector of velocities
Astorage for centroidal momentum matrix
Adotstorage for centroidal momentum matrix dot
update_kinematicsif true, kinematic variables will be computed. Defaults to true
Note
this function will compute the center of mass and its velocity. If you already have these quantities it will be more efficient to use the function that takes these as arguments so it doesn't recompute them

◆ calcCentroidalMomentumMatrixAndMatrixDot() [2/2]

void RobotDynamics::Utils::calcCentroidalMomentumMatrixAndMatrixDot ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::MatrixNd A,
Math::MatrixNd Adot,
Math::Vector3d  com,
Math::Vector3d  com_velocity,
bool  update_kinematics = true 
)

Calculates the centroidal momentum matrix and its time derivative, but in a more efficient way than calling the functions to compute them separately. If you need both matrices it will be more efficient to use this function.

Parameters
modelrdl model
qvector of position
qdotvector of velocities
Astorage for centroidal momentum matrix
Adotstorage for centroidal momentum matrix dot
composition of the center of mass in world coordinates
com_velocityvelocity of the center of mass in world coordinates
update_kinematicsif true, kinematic variables will be computed. Defaults to true

◆ calcCentroidalMomentumMatrixDot() [1/2]

void RobotDynamics::Utils::calcCentroidalMomentumMatrixDot ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::MatrixNd Adot,
bool  update_kinematics = true 
)

Calculates the time derivative of the centroidal momentum matrix, i.e. the matrix computed by RobotDynamics::Utils::calcCentroidalMomentumMatrix and stores it in the Adot argument.

Parameters
model
q
qdot
Adot
update_kinematics
Note
this function will compute the center of mass and its velocity. If you already have these quantities it will be more efficient to use the function that takes these as arguments so it doesn't recompute them

◆ calcCentroidalMomentumMatrixDot() [2/2]

void RobotDynamics::Utils::calcCentroidalMomentumMatrixDot ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
Math::MatrixNd Adot,
Math::Vector3d  com,
Math::Vector3d  com_velocity,
bool  update_kinematics = true 
)

Calculates the time derivative of the centroidal momentum matrix, i.e. the matrix computed by RobotDynamics::Utils::calcCentroidalMomentumMatrix and stores it in the Adot argument.

Parameters
modelrdl model
qvector of position
qdotvector of velocities
Adotstorage for centroidal momentum matrix dot
composition of the center of mass in world coordinates
com_velocityvelocity of the center of mass in world coordinates
update_kinematics

◆ calcDragWrenchOnCenterOfMass()

Math::SpatialForce RobotDynamics::HydroUtils::calcDragWrenchOnCenterOfMass ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
bool  update_kinematics = true 
)

Computes the wrench due to drag experienced at the robots center of mass.

Parameters
model
q
qdot
update_kinematics(optional) Defaults to true
Note
The reference frame the resulting force is expressed in is the center of mass reference frame owned by the RobotDynamics::Model object. This means that you must first have the position/orientation of that reference frame updated prior to calling this function otherwise the frame it's expressed in is likely to not be what you want.

◆ calcGravityWrenchOnCenterOfMass()

Math::SpatialForce RobotDynamics::Utils::calcGravityWrenchOnCenterOfMass ( Model model,
const Math::VectorNd q,
bool  update_kinematics = true 
)

Computes the gravitational wrench experienced on the robots center of mass.

Parameters
model
q
update_kinematics(optional) Defaults to true
Note
The reference frame the resulting force is expressed in is the center of mass reference frame owned by the RobotDynamics::Model object. This means that you must first have the position/orientation of that reference frame updated prior to calling this function otherwise the frame it's expressed in is likely to not be what you want.

◆ calcKineticEnergy()

double RobotDynamics::Utils::calcKineticEnergy ( Model model,
const Math::VectorNd q,
const Math::VectorNd qdot,
bool  update_kinematics = true 
)

Computes the kinetic energy of the full model.

Parameters
model
q
qdot
update_kinematics(optional) Defaults to true
Returns
Kinetic energy

◆ calcPotentialEnergy()

double RobotDynamics::Utils::calcPotentialEnergy ( Model model,
const Math::VectorNd q,
bool  update_kinematics = true 
)

Computes the potential energy of the full model.

Parameters
model
q
update_kinematics(optional) Defaults to true
Returns
Potential energy

◆ calcSubtreeCenterOfMassScaledByMass()

Math::Vector3d RobotDynamics::Utils::calcSubtreeCenterOfMassScaledByMass ( Model model,
const unsigned int  bodyId,
const Math::VectorNd q,
bool  updateKinematics = true 
)

Calculates the center of mass of a subtree starting with the body with ID bodyId and scales it by the total mass of the subtree.

Parameters
modelThe model used for the calculation
bodyIdThe ID of the first body in the desired subtree
qThe current joint positions
updateKinematicsIf true, kinematic variables will be computed. Default = true

◆ calcSubtreeMass()

double RobotDynamics::Utils::calcSubtreeMass ( Model model,
const unsigned int  bodyId 
)

Calculates the total mass of the subtree beginning with body bodyId and traversing outwards from there.

Parameters
modelThe model used for the calculation
bodyIdThe ID of the first body in the desired subtree
Returns
Mass of subtree

◆ calcSubtreeVolume()

double RobotDynamics::HydroUtils::calcSubtreeVolume ( Model model,
const unsigned int  bodyId 
)

Calculates the total volume of the model.

Parameters
modelThe model used for the calculation
bodyIdThe ID of the first body in the desired subtree
Returns
volume of subtree

◆ calcSubtreeWeightInFluid()

double RobotDynamics::HydroUtils::calcSubtreeWeightInFluid ( Model model,
const unsigned int  bodyId 
)

Calculate the weight of a kinematic subtree in fluid with density RobotDynamics::Model::fluid_density.

Parameters
modelRDL Model
bodyIdThe ID of the first body in the desired subtree
Returns
Weight in fluid

◆ getBodyName()

std::string RobotDynamics::Utils::getBodyName ( const RobotDynamics::Model model,
unsigned int  body_id 
)

get body name, returns empty string if bodyid is virtual and has multiple child bodies

◆ getDofName()

std::string RobotDynamics::Utils::getDofName ( const SpatialVector joint_dof)

get string abbreviation for dof name from spatial vector.

◆ getModelDOFOverview()

std::string RobotDynamics::Utils::getModelDOFOverview ( const Model model)

Creates a human readable overview of the Degrees of Freedom.

◆ getModelHierarchy()

std::string RobotDynamics::Utils::getModelHierarchy ( const Model model)

Creates a human readable overview of the model.

◆ getNamedBodyOriginsOverview()

std::string RobotDynamics::Utils::getNamedBodyOriginsOverview ( Model model)

Creates a human readable overview of the locations of all bodies that have names.