Robot Dynamics Library: RobotDynamics Namespace Reference

Namespace for all structures of the RobotDynamics library. More...

Namespaces
	Examples

	HydroUtils
	Namespace that contains optional helper functions.

	Math
	Math types such as vectors and matrices and utility functions.

	Urdf

	Utils
	Namespace that contains optional helper functions.

Classes
struct	DragData

struct	Body
	Describes all properties of a single body. More...

struct	FixedBody
	Keeps the information of a body and how it is attached to another body. More...

struct	ConstraintSet
	Structure that contains both constraint information and workspace memory. More...

class	ReferenceFrameException
	A custom exception for frame operations. More...

class	FrameObject
	An interface that objects with a ReferenceFrame extend to inherit the FrameObject::changeFrame method. More...

struct	Joint
	Describes a joint relative to the predecessor body. More...

struct	CustomJoint
	CustomJoint is a struct used to create a joint with user defined parameters. This is accomplished by overriding the RobotDynamics::Joint::jcalc methods that calculate each joints kinematic parameters. More...

struct	Model
	Contains all information about the rigid body model. More...

class	RdlException
	A custom exception. More...

class	ReferenceFrame
	ReferenceFrame object used to tell what frame objects are expressed in. Every ReferenceFrame has a pointer to its parent ReferenceFrame. This parent frame is NOT allowed to be nullptr. The ONLY ReferenceFrame that is allowed to have parentFrame=nullptr is the world frame. There is only one world frame and it can be accessed by the static method ReferenceFrame::getWorldFrame() which will return a shared_ptr to the world frame. This class and its implementation are an adaptation of ReferenceFrame.java by Jerry Pratt and the IHMC Robotics Group. More...

class	FixedReferenceFrame

Typedefs
typedef std::shared_ptr< Model >	ModelPtr

typedef std::shared_ptr< ReferenceFrame >	ReferenceFramePtr

typedef std::shared_ptr< FixedReferenceFrame >	FixedReferenceFramePtr

typedef std::vector< ReferenceFramePtr >	ReferenceFramePtrV

typedef std::vector< FixedReferenceFramePtr >	FixedReferenceFramePtrV

Enumerations
enum	JointType { JointTypeUndefined = 0 , JointTypeRevolute , JointTypePrismatic , JointTypeRevoluteX , JointTypeRevoluteY , JointTypeRevoluteZ , JointTypeSpherical , JointTypeEulerZYX , JointTypeEulerXYZ , JointTypeEulerYXZ , JointTypeTranslationXYZ , JointTypeFloatingBase , JointTypeFixed , JointType1DoF , JointType2DoF , JointType3DoF , JointType4DoF , JointType5DoF , JointType6DoF , JointTypeCustom }
	General types of joints. More...

Functions
void	solveContactSystemDirect (Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &A, Math::VectorNd &b, Math::VectorNd &x, Math::LinearSolver &linear_solver)
	Solves the full contact system directly, i.e. simultaneously for contact forces and joint accelerations. More...

void	solveContactSystemRangeSpaceSparse (Model &model, Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &K, Math::VectorNd &a, Math::LinearSolver linear_solver)
	Solves the contact system by first solving for the the joint accelerations and then the contact forces using a sparse matrix decomposition of the joint space inertia matrix. More...

void	solveContactSystemNullSpace (Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &Y, Math::MatrixNd &Z, Math::VectorNd &qddot_y, Math::VectorNd &qddot_z, Math::LinearSolver &linear_solver)
	Solves the contact system by first solving for the joint accelerations and then for the constraint forces. More...

void	calcContactJacobian (Model &model, const Math::VectorNd &Q, const ConstraintSet &CS, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the Jacobian for the given ConstraintSet. More...

void	calcContactSystemVariables (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS)

void	forwardDynamicsContactsDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
	Computes forward dynamics with contact by constructing and solving the full lagrangian equation. More...

void	forwardDynamicsContactsRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)

void	forwardDynamicsContactsNullSpace (Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot)

void	computeContactImpulsesDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
	Computes contact gain by constructing and solving the full lagrangian equation. More...

void	computeContactImpulsesRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
	Resolves contact gain using solveContactSystemRangeSpaceSparse() More...

void	computeContactImpulsesNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
	Resolves contact gain using solveContactSystemNullSpace() More...

void	forwardDynamicsApplyConstraintForces (Model &model, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot)
	Compute only the effects of external forces on the generalized accelerations. More...

void	forwardDynamicsAccelerationDeltas (Model &model, ConstraintSet &CS, VectorNd &QDDot_t, const unsigned int body_id, const SpatialForceV &f_t)
	Computes the effect of external forces on the generalized accelerations. More...

void	set_zero (std::vector< SpatialVector > &spatial_values)

void	forwardDynamicsContactsKokkevis (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
	Computes forward dynamics that accounts for active contacts in ConstraintSet. More...

void	inverseDynamics (Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &QDDot, VectorNd &Tau, SpatialForceV *f_ext, bool update_kinematics)

void	gravityEffects (Model &model, Math::VectorNd &Tau)
	Computes the gravity vector. More...

void	coriolisEffects (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
	Computes the coriolis forces. More...

void	calcBodyGravityWrench (Model &model, unsigned int body_id, RobotDynamics::Math::SpatialForce &gravity_wrench)
	Calculate the wrench due to gravity on a body. More...

void	nonlinearEffects (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
	Computes the coriolis forces. More...

void	compositeRigidBodyAlgorithm (Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
	Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm. More...

void	forwardDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
	Computes forward dynamics with the Articulated Body Algorithm. More...

void	forwardDynamicsLagrangian (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::MatrixNd &H, Math::VectorNd &C, Math::LinearSolver linear_solver=Math::LinearSolverColPivHouseholderQR, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
	Computes forward dynamics by building and solving the full Lagrangian equation. More...

void	calcMInvTimesTau (Model &model, const Math::VectorNd &Q, const Math::VectorNd &Tau, Math::VectorNd &QDDot, bool update_kinematics=true)
	Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in linear time. More...

void	forwardHydroDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
	Computes forward dynamics with the Articulated Body Algorithm including hydrodynamics effects. More...

void	compositeRigidBodyAlgorithmHydro (Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
	Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm. The resulting inertia matrix will include the effects of fluid added inertia. More...

void	nonlinearEffectsHydro (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
	Computes the coriolis forces, gravity/buoyancy forces as well as hydroydnamic drag and added mass. More...

void	coriolisEffectsHydro (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
	Computes the coriolis forces. More...

void	inverseDynamicsHydro (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, Math::SpatialForceV *f_ext, bool update_kinematics)

void	buoyancyEffects (Model &model, Math::VectorNd &Tau)
	Computes the buoyancy vector. More...

void	potentialEffects (Model &model, Math::VectorNd &Tau)
	Computes the combined buoyancy+gravity vector. More...

void	dragEffects (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
	Computes the drag vector. More...

void	jcalc (Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot)
	Computes all variables for a joint model and updates the body frame as well as the body's center of mass frame. More...

Math::SpatialTransform	jcalc_XJ (Model &model, unsigned int joint_id, const Math::VectorNd &q)

void	jcalc_X_lambda_S (Model &model, unsigned int joint_id, const VectorNd &q)

void	updateKinematics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
	Updates and computes velocities and accelerations of the bodies. More...

void	updateKinematicsCustom (Model &model, const Math::VectorNd Q, const Math::VectorNd QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
	Selectively updates model internal states of body positions, velocities and/or accelerations. More...

void	updateAccelerations (Model &model, const Math::VectorNd &QDDot)
	Computes only the accelerations of the bodies. More...

void	updateKinematicsCustomParallel (Model &model, const Math::VectorNd Q, const Math::VectorNd QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
	Selectively updates model internal states of body positions, velocities and/or accelerations and spawns threads far each branched chain. More...

void	updateKinematicsParallel (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
	Updates and computes velocities and accelerations of the bodies. More...

void	calcRelativeBodySpatialJacobian (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
	Computes the jacobian of a frame $J_{i}^{k,j}$ with being the "base" frame, being the "relative" frame, and being the "expressed in" frame. Multiplying this jacobian by a vector of joint velocities will result in the spatial motion of the baseFrame w.r.t relativeFrame expressed in expressedInFrame, a.k.a $v_{i}^{k,j} = J_{i}^{k,j}\dot{q}$ . More...

void	calcRelativeBodySpatialJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
	Computes the rate of change of the jacobian, $\dot{J}_{i}^{k,j}$ , with being the "base" frame, being the relative" frame, and \f$k\f$ being the "expressed in" frame. This jacobian is such that the following is true, $a_{i}^{k,j} = J_{i}^{k,j}\ddot{q} + \dot{J}_{i}^{k,j}\dot{q}$ where $a_{i}^{k,j}$ is the spatial acceleration of frame w.r.t frame , expressed in frame . Additionally the jacobian $J_{i}^{k,j}$ can be computed by RobotDynamics::calcRelativeBodySpatialJacobian. More...

void	calcRelativeBodySpatialJacobianAndJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
	Computes both the body spatial jacobian and its time derivative. This function will be a bit more efficient at computing the jacobian and it's time derivative if you need both matrices rather than computing them with separate function calls. More...

void	calcPointJacobian (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the 3D point jacobian for a point on a body. More...

void	calcPointJacobian (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics=true)
	Compute the 3D point jacobian of the origin of a reference frame If a position of a point is computed by a function for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix . More...

void	calcPointJacobian6D (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics=true)
	Compute the 6D point jacobian of the origin of a reference frame If a position of a point is computed by a function for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix . More...

void	calcRelativePointJacobian6D (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Compute the 6D jacobian of the origin of a reference frame relative to the origin of another reference frame and express the result in a third frame. More...

void	calcPointJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the time derivative of the linear components the a point jacobian on a body. More...

void	calcPointJacobian6D (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes a 6-D Jacobian for a point on a body. More...

void	calcPointJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the time derivative of a point jacobian of a point on a body. More...

void	calcRelativePointJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Compute the time derivative of the 6D jacobian of the origin of a reference frame relative to the origin of another reference frame and express the result in a third frame. More...

void	calcRelativePointJacobianAndJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Compute the point Jacobian of the origin of baseFrame, relative to the origin of relativeFrame, expressed in expressedInFrame. Also compute that point Jacobians time derivative. More...

void	calcPointJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the time derivative of a point jacobian of a point on a body. Only computes the linear elemets. More...

void	calcPointJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the 6D time derivative of a point jacobian on a body. More...

void	calcBodySpatialJacobian (Model &model, const Math::VectorNd &Q, unsigned int body_id, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the spatial jacobian for a body. The result will be returned via the G argument and represents the body Jacobian expressed at the origin of the body. The corresponding spatial velocity of the body w.r.t the world frame expressed in body frame can be calculated, for the body, as $v^{i,0}_{i} = G(q) \dot{q}$ . For the spatial jacobian of body , each column j is computed as follows. More...

void	calcBodySpatialJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd &G, const bool update_kinematics=true)
	Computes the time derivative of the spatial jacobian for a body. The result will be returned via the G argument and represents the time derivative of the body Jacobian expressed at the origin of the body. The corresponding spatial acceleration of the body w.r.t the world frame expressed in body frame can be calculated, for the th body, as $a^{i,0}_{i} = G(q) \ddot{q} + \dot{G}(q)\dot{q}$ where is the body jacobian of body . More...

Math::FrameVector	calcPointVelocity (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes the velocity of a point on a body. More...

Math::FrameVectorPair	calcPointVelocity6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes angular and linear velocity of a point that is fixed on a body. More...

Math::FrameVectorPair	calcPointVelocity6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, bool update_kinematics=true)
	Computes angular and linear velocity of the origin of a reference frame relative to world and expressed in world frame. More...

FrameVectorPair	calcPointVelocity6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr baseFrame, RobotDynamics::ReferenceFramePtr relativeFrame, RobotDynamics::ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Computes angular and linear velocity of the origin of a reference frame relative to another reference frame and expressed in a third reference frame. More...

FrameVectorPair	calcPointAcceleration6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Computes linear and angular acceleration of a reference frame, relative to another reference frame and expressed in a third reference frame. More...

Math::FrameVector	calcPointAcceleration (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes the linear acceleration of a point on a body. More...

Math::FrameVectorPair	calcPointAcceleration6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes linear and angular acceleration of a point on a body. More...

Math::SpatialMotion	calcSpatialVelocity (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial velocity of any frame with respect to any other frame, expressed in an arbirtary third frame. The returned RobotDynamicS::Math::SpatialMotion is expressed in body_frame unless th expressedInFrame is provided. Each time RobotDynamics::updateKinematics is called, the spatial velocity of each body with respect to the world, and expressed in body frame is calculated. For body this is written as $v^{i,0}_{i}$ . Given another body, body , the velocity of body relative to body and expressed in body frame is computed by,. More...

Math::SpatialMotion	calcSpatialVelocity (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial velocity of any body with respect to any other body. The returned RobotDynamicS::Math::SpatialMotion is expressed in the body frame of body body_id. Each time RobotDynamics::updateKinematics is called, the spatial velocity of each body with respect to the world, and expressed in body frame is calculated. For body this is written as $v^{i,0}_{i}$ . Given another body, body , the velocity of body relative to body and expressed in body frame is computed by,. More...

Math::SpatialAcceleration	calcSpatialAcceleration (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial acceleration of any frame with respect to any other frame and express the result in an arbitrary third frame. The returned RobotDynamicS::Math::SpatialAcceleration can be expressed in the reference frame of choice by supplying the expressedInFrame argument. If left to the default value of nullptr, the result will be expressed in body_frame. More...

Math::SpatialAcceleration	calcSpatialAcceleration (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial acceleration of any body with respect to any other body and express the result in an arbitrary reference frame. The returned RobotDynamicS::Math::SpatialAcceleration can be expressed in the reference frame of choice by supplying the expressedInFrame argument. If left to the default value of nullptr, the result will be expressed in the reference frame corresponding to body_id. More...

bool	inverseKinematics (Model &model, const VectorNd &Qinit, const std::vector< unsigned int > &body_id, const std::vector< Vector3d > &body_point, const std::vector< Vector3d > &target_pos, VectorNd &Qres, double step_tol, double lambda, unsigned int max_iter)

DragData	operator+ (DragData dragData1, const DragData &dragData2)

bool	inverseKinematics (Model &model, const Math::VectorNd &Qinit, const std::vector< unsigned int > &body_id, const std::vector< Math::Vector3d > &body_point, const std::vector< Math::Vector3d > &target_pos, Math::VectorNd &Qres, double step_tol=1.0e-12, double lambda=0.01, unsigned int max_iter=50)
	Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method) More...

Detailed Description

Namespace for all structures of the RobotDynamics library.

Typedef Documentation

◆ ModelPtr

typedef std::shared_ptr<Model> RobotDynamics::ModelPtr

Enumeration Type Documentation

◆ JointType

enum RobotDynamics::JointType

General types of joints.

Enumerator
JointTypeUndefined
JointTypeRevolute
JointTypePrismatic
JointTypeRevoluteX
JointTypeRevoluteY
JointTypeRevoluteZ
JointTypeSpherical	3 DoF joint using Quaternions for joint positional variables and
JointTypeEulerZYX	3 DoF joint that uses Euler ZYX convention (faster than emulated
JointTypeEulerXYZ	3 DoF joint that uses Euler XYZ convention (faster than emulated
JointTypeEulerYXZ	3 DoF joint that uses Euler YXZ convention (faster than emulated
JointTypeTranslationXYZ
JointTypeFloatingBase	A 6-DoF joint for floating-base (or freeflyer) systems.
JointTypeFixed	Fixed joint which causes the inertial properties to be merged with.
JointType1DoF
JointType2DoF	Emulated 2 DoF joint.
JointType3DoF	Emulated 3 DoF joint.
JointType4DoF	Emulated 4 DoF joint.
JointType5DoF	Emulated 5 DoF joint.
JointType6DoF	Emulated 6 DoF joint.
JointTypeCustom	User defined joints of varying size.

Function Documentation

◆ buoyancyEffects()

void RobotDynamics::buoyancyEffects	(	Model &	model,
		Math::VectorNd &	Tau
	)

Computes the buoyancy vector.

This function computes the buoyancy vector for an RDL model

Parameters

model	rigid body model
Tau	Buoyancy vector. Modified

Note: This function WILL NOT compute any of the kinematics, so you MUST call either updateKinematics or updateKinematicsCustom prior to calling this function or the result will be wrong.

◆ compositeRigidBodyAlgorithmHydro()

void RobotDynamics::compositeRigidBodyAlgorithmHydro	(	Model &	model,
		const Math::VectorNd &	Q,
		Math::MatrixNd &	H,
		bool	update_kinematics = `true`
	)

Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm. The resulting inertia matrix will include the effects of fluid added inertia.

This function computes the joint space inertia matrix from a given model and the generalized state vector: $M(q) = M_{rb}(q) + M_{A}$ where $M(q)_{rb}$ is what you would get from RobotDynamics::compositeRigidBodyAlgorithm and $M(q)_{A}$ is the inertia dependent from the effects of the fluid. Notice that it is not dependent on the joint position variables. It is only geometry dependent.

Parameters

model	rigid body model
Q	state vector of the model
H	a matrix where the result will be stored in
update_kinematics	whether the kinematics should be updated (safer, but at a higher computational cost!)

Note: This function only evaluates the entries of H that are non-zero. Before calling this function one has to ensure that all other values have been set to zero, e.g. by calling H.setZero().

Update the kinematic parameters and calculate the combined(rigid body & hydrodynamic inertia) inertia to a modifiable location

Do a backwards pass over the bodies, populating all elements of the inertia matrix that are not zero.

Calculate the inertia of the subtree rooted at body i assuming that subtree is a single composite rigid body

Main diagonal term

Calculate the non-zero, off diagonal terms

◆ coriolisEffectsHydro()

void RobotDynamics::coriolisEffectsHydro	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::VectorNd &	Tau,
		bool	update_kinematics = `true`
	)

Computes the coriolis forces.

This function computes the generalized forces from given generalized states, and velocities from coriolis effects $\tau = C(q, \dot{q})$

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
Tau	actuations of the internal joints (output)
update_kinematics	whether the kinematics should be updated (safer, but at a higher computational cost!)

Note: If you pass update_kinematics=false, then you must have previously calculated the kinematics from one of the other functions, e.g. updateKinematics or updateKinematicsCustom. If you have not computed kinematics, then passing update_kinematics will give an incorrect result

◆ dragEffects()

void RobotDynamics::dragEffects	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::VectorNd &	Tau,
		bool	update_kinematics = `true`
	)

Computes the drag vector.

This function computes the drag vector for an RDL model

Parameters

model	rigid body model
Tau	Drag vector. Modified

◆ forwardDynamicsAccelerationDeltas()

void RobotDynamics::forwardDynamicsAccelerationDeltas	(	Model &	model,
		ConstraintSet &	CS,
		VectorNd &	QDDot_t,
		const unsigned int	body_id,
		const SpatialForceV &	f_t
	)

Computes the effect of external forces on the generalized accelerations.

This function is essentially similar to ForwardDynamics() except that it tries to only perform computations of variables that change due to external forces defined in f_t.

◆ forwardDynamicsApplyConstraintForces()

void RobotDynamics::forwardDynamicsApplyConstraintForces	(	Model &	model,
		const VectorNd &	Tau,
		ConstraintSet &	CS,
		VectorNd &	QDDot
	)

Compute only the effects of external forces on the generalized accelerations.

This function is a reduced version of ForwardDynamics() which only computes the effects of the external forces on the generalized accelerations.

◆ forwardHydroDynamics()

void RobotDynamics::forwardHydroDynamics	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		Math::VectorNd &	QDDot,
		Math::SpatialForceV *	f_ext = `nullptr`,
		bool	update_kinematics = `true`
	)

Computes forward dynamics with the Articulated Body Algorithm including hydrodynamics effects.

This function computes the generalized accelerations from given generalized states, velocities and forces: $\ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)$ It does this by using the recursive Articulated Body Algorithm that runs in $O(n_{dof})$ with $n_{dof}$ being the number of joints.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
Tau	actuations of the internal joints
QDDot	accelerations of the internal joints (output)
f_ext	External forces acting on the body (optional, defaults to NULL)
update_kinematics	Default true. If false, kinematics will not be computed

Note: If you pass update_kinematics=false, then you must have previously calculated the kinematics from one of the other functions, e.g. updateKinematics or updateKinematicsCustom. If you have not computed kinematics, then passing update_kinematics will give an incorrect result

Forward pass over all bodies to update the kinematics as well as calculate the bias force(s) and add in any external forces if any.

Do a backwards pass over the bodies, calculating the articulated body inertias and articulated bias forces(see RBDA p. 128). Also calculates some intermediate values(U,d,u) to reduce the number of duplicate operations.

Do a final forward pass over the bodies, calculating the accelerations

◆ inverseDynamicsHydro()

void RobotDynamics::inverseDynamicsHydro	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot,
		Math::VectorNd &	Tau,
		Math::SpatialForceV *	f_ext = `nullptr`,
		bool	update_kinematics = `true`
	)

@brief Computes inverse dynamics with the Newton-Euler Algorithm. This method includes hydrodynamic effects from buoyancy, added mass, and drag

This function computes the generalized forces from given generalized states, velocities, and accelerations: $\tau = M(q) \ddot{q} + N(q, \dot{q})$

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
QDDot	accelerations of the internals joints
Tau	actuations of the internal joints (output)
f_ext	External forces acting on the body (optional, defaults to NULL)
update_kinematics	whether the kinematics should be updated (safer, but at a higher computational cost!)

Note: If you pass update_kinematics=false, then you must have previously calculated the kinematics from one of the other functions, e.g. updateKinematics or updateKinematicsCustom. If you have not computed kinematics, then passing update_kinematics will give an incorrect result

◆ inverseKinematics()

bool RobotDynamics::inverseKinematics	(	Model &	model,
		const VectorNd &	Qinit,
		const std::vector< unsigned int > &	body_id,
		const std::vector< Vector3d > &	body_point,
		const std::vector< Vector3d > &	target_pos,
		VectorNd &	Qres,
		double	step_tol,
		double	lambda,
		unsigned int	max_iter
	)

◆ jcalc()

void RobotDynamics::jcalc	(	Model &	model,
		unsigned int	joint_id,
		const Math::VectorNd &	q,
		const Math::VectorNd &	qdot
	)

Computes all variables for a joint model and updates the body frame as well as the body's center of mass frame.

By appropriate modification of this function all types of joints can be modeled. See RobotDynamics::CustomJoint

Parameters

model	the rigid body model
joint_id	the id of the joint we are interested in. This will be used to determine the type of joint and also the entries of $q, \dot{q}$ .
q	joint state variables
qdot	joint velocity variables

◆ jcalc_X_lambda_S()

void RobotDynamics::jcalc_X_lambda_S	(	Model &	model,
		unsigned int	joint_id,
		const VectorNd &	q
	)

◆ jcalc_XJ()

Math::SpatialTransform RobotDynamics::jcalc_XJ	(	Model &	model,
		unsigned int	joint_id,
		const Math::VectorNd &	q
	)

◆ nonlinearEffectsHydro()

void RobotDynamics::nonlinearEffectsHydro	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::VectorNd &	Tau,
		bool	update_kinematics = `true`
	)

Computes the coriolis forces, gravity/buoyancy forces as well as hydroydnamic drag and added mass.

This function computes the generalized forces from given generalized states, and velocities from gravity and coriolis effects including hydrodynamic terms like buoyancy, added mass, and drag: $\tau = N(q, \dot{q})$

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
Tau	actuations of the internal joints (output)
update_kinematics	whether the kinematics should be updated (safer, but at a higher computational cost!)

Note: If you pass update_kinematics=false, then you must have previously calculated the kinematics from one of the other functions, e.g. updateKinematics or updateKinematicsCustom. If you have not computed kinematics, then passing update_kinematics will give an incorrect result

◆ operator+()

DragData RobotDynamics::operator+	(	DragData	dragData1,
		const DragData &	dragData2
	)

inline

◆ potentialEffects()

void RobotDynamics::potentialEffects	(	Model &	model,
		Math::VectorNd &	Tau
	)

Computes the combined buoyancy+gravity vector.

This function computes the buoyancy+gravity vector for an RDL model

Parameters

model	rigid body model
Tau	Vector of torques from gravity and buoyancy. Modified

Note: This function WILL NOT compute any of the kinematics, so you MUST call either updateKinematics or updateKinematicsCustom prior to calling this function or the result will be wrong.

◆ set_zero()

void RobotDynamics::set_zero ( std::vector< SpatialVector > & spatial_values )

inline

Namespaces

Classes

Typedefs

Enumerations

Functions

Detailed Description

Typedef Documentation

◆ ModelPtr

Enumeration Type Documentation

◆ JointType

Function Documentation

◆ buoyancyEffects()

◆ compositeRigidBodyAlgorithmHydro()

◆ coriolisEffectsHydro()

◆ dragEffects()

◆ forwardDynamicsAccelerationDeltas()

◆ forwardDynamicsApplyConstraintForces()

◆ forwardHydroDynamics()

◆ inverseDynamicsHydro()

◆ inverseKinematics()

◆ jcalc()

◆ jcalc_X_lambda_S()

◆ jcalc_XJ()

◆ nonlinearEffectsHydro()

◆ operator+()

◆ potentialEffects()

◆ set_zero()