function

RigidBody.bodyVelocityRate

Body-velocity component of the rigid-body vector field

Public declarations

Explicit source declarations; dimensions, modifications and defaults are shown as written, without evaluation.

Role	Name	Type	Dimensions / attributes / default
input	`state`	`RigidBody.State`
input	`wrench`	`RigidBody.Wrench`
input	`parameters`	`RigidBody.Parameters`
output	`rate`	`Real`	`[3]`

Modelica declarations and implementation

Show parameters, interfaces and source

Parameters, inputs, outputs, defaults and equations from the source file.

function bodyVelocityRate "Body-velocity component of the rigid-body vector field"
  input RigidBody.State state;
  input RigidBody.Wrench wrench;
  input RigidBody.Parameters parameters;
  output Real rate[3];
protected
  Real gravityBody[3];
algorithm
  assert(parameters.mass > 0.0, "Rigid-body mass must be positive");
  gravityBody := transpose(LieGroups.SO3.Quat.to_DCM(state.attitude))
    * {0.0, 0.0, -parameters.gravity};
  rate := wrench.bodyForce / parameters.mass + gravityBody
    - cross(state.bodyAngularVelocity, state.bodyVelocity);
end bodyVelocityRate;