KrisLibrary
1.0.0
|
Class defining kinematic and dynamic state of a robot, with commonly used functions. Inherits from RobotKinematics3D. More...
#include <RobotDynamics3D.h>
Public Member Functions | |
void | Initialize (int numBodies) |
void | InitializeRigidObject () |
void | Merge (const std::vector< RobotDynamics3D * > &robots) |
void | Subset (const RobotDynamics3D &robot, const std::vector< int > &subset) |
void | UpdateDynamics () |
This calls UpdateFrames() and Update_J() | |
void | Update_J () |
Updates JO and JP. | |
void | Update_dB_dq () |
Updates dB_dq. Called internally by GetCoriolis*() | |
bool | InVelocityLimits (const Vector &dq) const |
Check physical limits. | |
bool | InTorqueLimits (const Vector &torques) const |
bool | InPowerLimits (const Vector &dq, const Vector &torques) const |
bool | IsUnactuatedLink (int i) const |
Query helpers. | |
bool | IsActuatedLink (int i) const |
bool | GetJacobianDt (const Vector3 &pi, int i, int j, Vector3 &dtheta_dt, Vector3 &dp_dt) const |
void | GetWorldAcceleration (const Vector3 &pi, int i, const Vector &ddq, Vector3 &dw, Vector3 &dv) const |
Given some ddq, gets the acceleration of point pi on link i. | |
void | GetResidualAcceleration (const Vector3 &pi, int i, Vector3 &dw, Vector3 &dv) const |
Given no joint acceleration, gets the acceleration of point pi on link i. | |
Vector3 | GetLinearMomentum (int i) const |
Linear momentum of link i. | |
Vector3 | GetLinearMomentum () const |
Linear momentum of robot. | |
Vector3 | GetAngularMomentum (int i) const |
Angular momentum of link i. | |
Vector3 | GetAngularMomentum () const |
Angular momentum of robot. | |
Real | GetKineticEnergy (int i) const |
Kinetic energy of link i. | |
Real | GetKineticEnergy () const |
Kinetic energy of robot. | |
void | GetKineticEnergyMatrix (Matrix &B) const |
Computes the kinetic energy matrix B. | |
Real | GetKineticEnergyDeriv (int i, int j, int z) const |
Computes dBij/dqz. | |
void | GetKineticEnergyMatrixDeriv (int z, Matrix &dB) const |
Computes the derivative of B with respect to q(z) | |
void | GetKineticEnergyMatrixTimeDeriv (Matrix &dB) const |
Computes dB/dt. | |
void | GetCoriolisForceMatrix (Matrix &C) |
Computes the coriolis force matrix C (such that C*dq = coriolis forces) | |
void | GetCoriolisForces (Vector &Cdq) |
Computes the coriolis forces. | |
void | CalcAcceleration (Vector &ddq, const Vector &fext) |
void | CalcTorques (const Vector &ddq, Vector &fext) |
Public Member Functions inherited from RobotKinematics3D | |
virtual std::string | LinkName (int i) const |
void | Initialize (int numLinks) |
Initializer: blank robot. | |
void | InitializeRigidObject () |
Initializer: rigid object. | |
void | Merge (const std::vector< RobotKinematics3D * > &robots) |
Initializer: merge some robots into one big robot. | |
void | Subset (const RobotKinematics3D &robot, const std::vector< int > &subset) |
Initializer: select a subset of the robot links as DOFs. | |
void | UpdateFrames () |
based on the values in q, update the frames T | |
void | UpdateSelectedFrames (int link, int root=-1) |
based on the values in q, update the frames of link T up to the root | |
void | UpdateConfig (const Config &q) |
sets the current config q and updates frames | |
bool | InJointLimits (const Config &q) const |
returns true if q is within joint limits | |
void | NormalizeAngles (Config &q) const |
normalizes angles in q to be within the range [qmin,qmax] | |
Real | GetTotalMass () const |
Vector3 | GetCOM () const |
Vector3 | GetCOM (const Config &q) |
Matrix3 | GetTotalInertia () const |
void | GetCOMJacobian (Matrix &Jc) const |
void | GetCOMHessian (Matrix &Hx, Matrix &Hy, Matrix &Hz) const |
void | GetGravityTorques (const Vector3 &g0, Vector &G) const |
Real | GetGravityPotentialEnergy (const Vector3 &g0, Real refHeight=Zero) const |
void | GetWorldPosition (const Vector3 &pi, int i, Vector3 &p) const |
in the following, pi is a point in the local frame of body i | |
const Matrix3 & | GetWorldRotation (int i) const |
void | GetWorldRotation_Moment (int i, Vector3 &m) const |
void | GetWorldVelocity (const Vector3 &pi, int i, const Vector &dq, Vector3 &dp) const |
gets the world velocity/angular velocity of pi, given dq/dt | |
void | GetWorldAngularVelocity (int i, const Vector &dq, Vector3 &omega) const |
bool | GetWorldRotationDeriv (int i, int j, Matrix3 &dR) const |
derivative of Ri w.r.t. qj | |
bool | GetWorldRotationDeriv_Moment (int i, int j, Vector3 &dm) const |
bool | GetWorldRotationDeriv_Moment (int i, int j, const Vector3 &m, Vector3 &dm) const |
same as above, but m s.t. Ri=e^[m] is specified | |
bool | GetJacobian (const Vector3 &pi, int i, int j, Vector3 &dw, Vector3 &dv) const |
gets the jacobian of pi w.r.t qj | |
bool | GetOrientationJacobian (int i, int j, Vector3 &dw) const |
bool | GetPositionJacobian (const Vector3 &pi, int i, int j, Vector3 &dv) const |
void | GetFullJacobian (const Vector3 &pi, int i, Matrix &J) const |
void | GetPositionJacobian (const Vector3 &pi, int i, Matrix &J) const |
rows 3-5 of the above | |
void | GetWrenchTorques (const Vector3 &torque, const Vector3 &force, int i, Vector &F) const |
for a wrench w=(torque,force) on link i, returns joint torques F = J^t w | |
void | AddWrenchTorques (const Vector3 &torque, const Vector3 &force, int i, Vector &F) const |
void | GetForceTorques (const Vector3 &f, const Vector3 &pi, int i, Vector &F) const |
for a force f at pi on link i, returns joint torques F = J^t f | |
void | AddForceTorques (const Vector3 &f, const Vector3 &pi, int i, Vector &F) const |
bool | GetJacobianDeriv (const Vector3 &pm, int m, int i, int j, Vector3 &ddtheta, Vector3 &ddp) const |
void | GetJacobianDeriv_Fast (const Vector3 &pm, int m, int i, int j, Vector3 &ddtheta, Vector3 &ddp) const |
assumes i,j<=m | |
void | GetJacobianDeriv (const Vector3 &pm, int m, Matrix *Htheta[3], Matrix *Hp[3]) const |
void | GetPositionHessian (const Vector3 &pm, int m, Matrix *Hp[3]) const |
void | GetDirectionalHessian (const Vector3 &pm, int m, const Vector3 &v, Matrix &Hpv) const |
Real | PointDistanceBound (const Vector3 &pi, int i, const Config &q1, const Config &q2) const |
Real | PointDistanceBound2 (const Vector3 &pi, int i, const Config &q1, const Config &q2) |
A closer upper bound than the previous. Overwrites robot's state. | |
Real | SphereDistanceBound (const Vector3 &ci, Real r, int i, const Config &q1, const Config &q2) |
Public Member Functions inherited from Chain | |
bool | HasValidOrdering () const |
bool | IsAncestor (int n, int p) const |
Returns true if p is an ancestor of n. | |
bool | IsDescendent (int n, int c) const |
Returns true if c is a descendant of n. | |
int | LCA (int a, int b) const |
Least common ancestor. | |
void | GetChildList (std::vector< std::vector< int > > &children) const |
Returns a vector where element i is a vectors of the children of link i. | |
void | GetAncestors (int k, std::vector< bool > &ancestors) const |
Returns a vector where element i is true if it is an ancestor of n. | |
void | GetDescendants (int k, std::vector< bool > &descendants) const |
Returns a vector where element i is true if it is an descendant of n. | |
Public Attributes | |
Vector | dq |
current velocity | |
Vector | velMin |
Vector | velMax |
velocity limits | |
Vector | torqueMax |
torque limits | |
Vector | powerMax |
Power=|torque||velocity| limits. | |
Array2D< Vector3 > | JP |
derivative of cm[i] w.r.t. q(j) | |
Array2D< Vector3 > | JO |
derivative of orientation[i] w.r.t. q(j) | |
std::vector< Matrix > | dB_dq |
derivative of the kinetic energy matrix w.r.t q(i) | |
Public Attributes inherited from RobotKinematics3D | |
std::vector< RobotLink3D > | links |
Config | q |
current configuration | |
Vector | qMin |
Vector | qMax |
joint limits | |
Public Attributes inherited from Chain | |
std::vector< int > | parents |
Topologically sorted list of parents. | |
Additional Inherited Members | |
Public Types inherited from Chain | |
enum | { NoParent = -1 } |
Class defining kinematic and dynamic state of a robot, with commonly used functions. Inherits from RobotKinematics3D.
The current state is given by configuration q, velocity dq. The dynamic equations are
* q' = dq, * B(q)*q'' + C(q,dq) + G(q) = f, *
where B is the kinetic energy matrix, C is the coriolis forces, G is the generalized gravity vector, and f is the generalized torque vector.
Forward and inverse dynamics methods use the slow O(n^3) full matrix method. For faster solutions, use the NewtonEulerSolver class.
All methods defined here use the current setting of dq as the state of the robot.
The GetKineticEnergyMatrix*(),GetKineticEnergyDeriv(), GetCoriolis*() CalcAcceleration(), and CalcTorques() methods additionally require UpdateDynamics() to have been called beforehand.
bool RobotDynamics3D::GetJacobianDt | ( | const Vector3 & | pi, |
int | i, | ||
int | j, | ||
Vector3 & | dtheta_dt, | ||
Vector3 & | dp_dt | ||
) | const |
Computes the time derivative of dpi/dqj. That is, the jacobian of pi on link i with respect to qj.
Referenced by IsUnactuatedLink().