Rigid Body Dynamics Library

Namespace for all structures of the RigidBodyDynamics library. More...
Namespaces  
Addons  
Math  
Math types such as vectors and matrices and utility functions.  
Utils  
Namespace that contains optional helper functions.  
Data Structures  
struct  Body 
Describes all properties of a single body. More...  
struct  ConstraintSet 
Structure that contains both constraint information and workspace memory. More...  
struct  CustomConstraint 
Interface to define generalpurpose constraints. More...  
struct  CustomJoint 
struct  FixedBody 
Keeps the information of a body and how it is attached to another body. More...  
struct  Joint 
Describes a joint relative to the predecessor body. More...  
struct  Model 
Contains all information about the rigid body model. More...  
Enumerations  
enum  JointType { JointTypeUndefined = 0, JointTypeRevolute, JointTypePrismatic, JointTypeRevoluteX, JointTypeRevoluteY, JointTypeRevoluteZ, JointTypeSpherical, JointTypeEulerZYX, JointTypeEulerXYZ, JointTypeEulerYXZ, JointTypeTranslationXYZ, JointTypeFloatingBase, JointTypeFixed, JointTypeHelical, JointType1DoF, JointType2DoF, JointType3DoF, JointType4DoF, JointType5DoF, JointType6DoF, JointTypeCustom } 
General types of joints. More...  
Functions  
void  SolveLinearSystem (const MatrixNd &A, const VectorNd &b, VectorNd &x, LinearSolver ls) 
unsigned int  GetMovableBodyId (Model &model, unsigned int id) 
RBDL_DLLAPI void  SolveConstrainedSystemDirect (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...  
RBDL_DLLAPI void  SolveConstrainedSystemRangeSpaceSparse (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...  
RBDL_DLLAPI void  SolveConstrainedSystemNullSpace (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...  
RBDL_DLLAPI void  CalcConstraintsPositionError (Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics=true) 
Computes the position errors for the given ConstraintSet. More...  
RBDL_DLLAPI void  CalcConstraintsJacobian (Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::MatrixNd &G, bool update_kinematics=true) 
Computes the Jacobian for the given ConstraintSet. More...  
RBDL_DLLAPI void  CalcConstraintsVelocityError (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics=true) 
Computes the velocity errors for the given ConstraintSet. More...  
RBDL_DLLAPI void  CalcConstrainedSystemVariables (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, std::vector< Math::SpatialVector > *f_ext=NULL) 
Computes the terms , , and of the constrained dynamic problem and stores them in the ConstraintSet. More...  
RBDL_DLLAPI bool  CalcAssemblyQ (Model &model, Math::VectorNd QInit, ConstraintSet &CS, Math::VectorNd &Q, const Math::VectorNd &weights, double tolerance=1e12, unsigned int max_iter=100) 
Computes a feasible initial value of the generalized joint positions. More...  
RBDL_DLLAPI void  CalcAssemblyQDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotInit, ConstraintSet &CS, Math::VectorNd &QDot, const Math::VectorNd &weights) 
Computes a feasible initial value of the generalized joint velocities. More...  
RBDL_DLLAPI void  ForwardDynamicsConstraintsDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext=NULL) 
Computes forward dynamics with contact by constructing and solving the full lagrangian equation. More...  
RBDL_DLLAPI void  ForwardDynamicsConstraintsRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext) 
RBDL_DLLAPI void  ForwardDynamicsConstraintsNullSpace (Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext) 
RBDL_DLLAPI void  ComputeConstraintImpulsesDirect (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...  
RBDL_DLLAPI void  ComputeConstraintImpulsesRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus) 
Resolves contact gain using SolveContactSystemRangeSpaceSparse() More...  
RBDL_DLLAPI void  ComputeConstraintImpulsesNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus) 
Resolves contact gain using SolveContactSystemNullSpace() More...  
RBDL_DLLAPI void  ForwardDynamicsApplyConstraintForces (Model &model, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot) 
Compute only the effects of external forces on the generalized accelerations. More...  
RBDL_DLLAPI void  ForwardDynamicsAccelerationDeltas (Model &model, ConstraintSet &CS, VectorNd &QDDot_t, const unsigned int body_id, const std::vector< SpatialVector > &f_t) 
Computes the effect of external forces on the generalized accelerations. More...  
void  set_zero (std::vector< SpatialVector > &spatial_values) 
RBDL_DLLAPI 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...  
RBDL_DLLAPI void  InverseDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext=NULL) 
Computes inverse dynamics with the NewtonEuler Algorithm. More...  
RBDL_DLLAPI void  NonlinearEffects (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, std::vector< Math::SpatialVector > *f_ext=NULL) 
Computes the coriolis forces. More...  
RBDL_DLLAPI 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...  
RBDL_DLLAPI void  ForwardDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext=NULL) 
Computes forward dynamics with the Articulated Body Algorithm. More...  
RBDL_DLLAPI void  ForwardDynamicsLagrangian (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::LinearSolver linear_solver=Math::LinearSolverColPivHouseholderQR, std::vector< Math::SpatialVector > *f_ext=NULL, Math::MatrixNd *H=NULL, Math::VectorNd *C=NULL) 
Computes forward dynamics by building and solving the full Lagrangian equation. More...  
RBDL_DLLAPI 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...  
RBDL_DLLAPI void  jcalc (Model &model, unsigned int joint_id, const Math::VectorNd &q, const Math::VectorNd &qdot) 
Computes all variables for a joint model. More...  
RBDL_DLLAPI Math::SpatialTransform  jcalc_XJ (Model &model, unsigned int joint_id, const Math::VectorNd &q) 
RBDL_DLLAPI void  jcalc_X_lambda_S (Model &model, unsigned int joint_id, const VectorNd &q) 
RBDL_DLLAPI 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...  
RBDL_DLLAPI void  UpdateKinematicsCustom (Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot, const Math::VectorNd *QDDot) 
Selectively updates model internal states of body positions, velocities and/or accelerations. More...  
RBDL_DLLAPI Vector3d  CalcBodyToBaseCoordinates (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &body_point_position, bool update_kinematics=true) 
Returns the base coordinates of a point given in body coordinates. More...  
RBDL_DLLAPI Vector3d  CalcBaseToBodyCoordinates (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &base_point_position, bool update_kinematics=true) 
Returns the body coordinates of a point given in base coordinates. More...  
RBDL_DLLAPI Matrix3d  CalcBodyWorldOrientation (Model &model, const Math::VectorNd &Q, const unsigned int body_id, bool update_kinematics=true) 
Returns the orientation of a given body as 3x3 matrix. More...  
RBDL_DLLAPI 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 point jacobian for a point on a body. More...  
RBDL_DLLAPI 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 6D Jacobian for a point on a body. More...  
RBDL_DLLAPI 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. More...  
RBDL_DLLAPI Vector3d  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...  
RBDL_DLLAPI Math::SpatialVector  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...  
RBDL_DLLAPI Vector3d  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...  
RBDL_DLLAPI SpatialVector  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...  
RBDL_DLLAPI 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.0e12, double lambda=0.01, unsigned int max_iter=50) 
Computes the inverse kinematics iteratively using a damped LevenbergMarquardt method (also known as Damped Least Squares method) More...  
Variables  
struct RBDL_DLLAPI  CustomConstraint 
Namespace for all structures of the RigidBodyDynamics library.
This abstract class represents a mathematical function that calculates a value of arbitrary type based on M real arguments. The output type is set as a template argument, while the number of input components may be determined at runtime. The name "Function" (with no trailing _) may be used as a synonym for Function_<double>.
Subclasses define particular mathematical functions. Predefined subclasses are provided for several common function types: Function_<T>::Constant, Function_<T>::Linear, Function_<T>::Polynomial, and Function_<T>::Step. You can define your own subclasses for other function types. The Spline_ class also provides a convenient way to create various types of Functions.
This is a low level Quintic Bezier curve class that contains functions to design continuous sets of 'C' shaped Bezier curves, and to evaluate their values and derivatives. A set in this context is used to refer to 2 or more quintic Bezier curves that are continuously connected to eachother to form one smooth curve, hence the name QuinticBezierSet.
In the special case when this class is being used to generate and evaluate 2D Bezier curves, that is x(u) and y(u), there are also functions to evaluate y(x), the first six derivatives of y(x), and also the first integral of y(x).
This class was not designed to be a stand alone Quintic Bezier class, but rather was developed out of necessity to model muscles. I required curves that, when linearly extrapolated, were C2 continuous, and by necessity I had to use quintic Bezier curves. In addition, the curves I was developing were functions in x,y space, allowing many of the methods (such as the evaluation of y(x) given that x(u) and y(u), the derivatives of y(x), and its first integral) to be developed, though in general this can't always be done.
I have parcelled all of these tools into their own class so that others may more easily use and develop this starting point for their own means. I used the following text during the development of this code:
Mortenson, Michael E (2006). Geometric Modeling Third Edition. Industrial Press Inc., New York. Chapter 4 was quite helpful.
Future Upgrades
Analytical Inverse to x(u): I think this is impossible because it is not possible, in general, to find the roots to a quintic polynomial, however, this fact may not preclude forming the inverse curve. The impossibility of finding the roots to a quintic polynomial was proven by Abel (Abel's Impossibility Theorem) and Galois
http://mathworld.wolfram.com/QuinticEquation.html
At the moment I am approximating the curve u(x) using cubic splines to return an approximate value for u(x), which I polish using Newton's method to the desired precision.
Note as of Nov 2015** > The cubic spline approximation of the inverse curve has been removed. Since there is no spline class in RBDL (and I'm not motivated to port it over from SimTK) this functionality does not work. In addition, I've since found that this nice inverse only saves a few Newton iterations over a calculated guess. It's not worth the effort to include.
Analytical Integral of y(x): This is possible using the Divergence Theorem applied to 2D curves. A nice example application is shown in link 2 for computing the area of a closed cubic Bezier curve. While I have been able to get the simple examples to work, I have failed to successfully compute the area under a quintic Bezier curve correctly. I ran out of time trying to fix this problem, and so at the present time I am numerically computing the integral at a number of knot points and then evaluating the spline to compute the integral value.
a. http://en.wikipedia.org/wiki/Divergence_theorem b. http://objectmix.com/graphics/133553areaclosedbeziercurve.html
Note as of Nov 2015** > The splined numeric integral of the curve has been removed. There is not an errorcontrolled numerical integrator in RBDL and so it is not straight forward to include this feature. > For later: discuss options with Martin.
Computational Cost Details All computational costs assume the following operation costs:
Operation Type : #flops +,,=,Boolean Op : 1 / : 10 sqrt: 20 trig: 40
These relative weightings will vary processor to processor, and so any of the quoted computational costs are approximate.
RBDL Port Notes
The port of this code from OpenSim has been accompanied by a few changes:
This class contains the quintic Bezier curves, x(u) and y(u), that have been created by SmoothSegmentedFunctionFactory to follow a physiologically meaningful muscle characteristic. A SmoothSegmentedFunction cannot be created directly,you must use SmoothSegmentedFunctionFactory to create the muscle curve of interest.
Computational Cost Details All computational costs assume the following operation costs:
Operation Type : #flops +,,=,Boolean Op : 1 / : 10 sqrt: 20 trig: 40
These relative weightings will vary processor to processor, and so any of the quoted computational costs are approximate.
RBDL Port Notes The port of this code from OpenSim has been accompanied by a few changes:
The function name .printMuscleCurveToFile(...) has been changed to .printCurveToFile().
This is a class that acts as a user friendly wrapper to QuinticBezerCurveSet to build specific kinds of physiologically plausible muscle curves using C2 continuous sets of quintic Bezier curves. This class has been written there did not exist a set of curves describing muscle characteristics that was:
For example, the curves employed by Thelen (Thelen DG(2003). Adjustment of Muscle Mechanics Model Parameters to Simulate Dynamic Contractions in Older Adults. ASME Journal of Biomechanical Engineering (125).) are parameterized in a physically meaningful manner, making them easy to use. However there are many shortcomings of these curves:
The muscle curves used in the literature until 2012 have been hugely influenced by Thelen's work, and thus similar comments can easily be applied to just about every other musculoskeletal simulation.
Another example is from Miller (Miller,RH(2011).Optimal Control of Human Running. PhD Thesis). On pg 149 a physiolgically plausible force velocity curve is specified that gives the user the ability to change the concentric curvature to be consistent with a slow or a fast twitch musle. This curve is not C2 continuous at the origin, but even worse, it contains singularities in its parameter space. Since these parameters do not have a physical interpretation this model is difficult to use without accidentically creating a curve with a singularity.
With this motivation I set out to develop a class that could generate muscle characteristic curves with the following properties:
These goals were surprisingly time consuming achieve, but these goals have been achieved using sets of C2 continuous quintic Bezier curves. The resulting muscle curve functions in this class take parameters that would be intuitive to biomechanists who simulate human musculoskeletal systems, and returns a SmoothSegmentedFunction which is capable of evaluating the value, and derivatives of the desired function (or actually relation as the case may be).
Each curve is made up of one or more C2 quintic Bezier curves x(u), and y(u), with linearily extrapolated ends as shown in the figure below. These quintic curves span 2 points, and achieve the desired derivative at its end points. The degree of curviness can be varied from 0 to 1 (0, 0.75 and 1.0 are shown in the figure in grey, blue and black respectively), and will make the curve approximate a line when set to 0 (grey), and approximate a curve that hugs the intersection of the lines that are defined by the end points locations and the slopes at the end of each curve segment (red lines). Although you do not need to set all of this information directly, for some of the curves it is useful to know that both the slope and the curviness parameter may need to be altered to achieve the desired shape.
Computational Cost Details All computational costs assume the following operation costs:
Operation Type : #flops *,+,,=,Boolean Op : 1 / : 10 sqrt: 20 trig: 40
These relative weightings will vary processor to processor, and so any of the quoted computational costs are approximate.
RBDL Port Notes The port of this code from OpenSim has been accompanied by a few changes:
enum JointType 
General types of joints.
RBDL_DLLAPI void RigidBodyDynamics::ForwardDynamicsAccelerationDeltas  (  Model &  model, 
ConstraintSet &  CS,  
VectorNd &  QDDot_t,  
const unsigned int  body_id,  
const std::vector< SpatialVector > &  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.
Definition at line 1311 of file Constraints.cc.
References Model::a, Model::d, ConstraintSet::d_a, ConstraintSet::d_multdof3_u, ConstraintSet::d_pA, ConstraintSet::d_u, JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, Model::mJoints, Model::multdof3_Dinv, Model::multdof3_S, Model::multdof3_U, Model::S, Model::U, Model::X_base, and Model::X_lambda.
RBDL_DLLAPI void RigidBodyDynamics::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.
Definition at line 1139 of file Constraints.cc.
References Model::a, SpatialTransform::apply(), Model::c, RigidBodyDynamics::Math::crossf(), Model::d, Model::dof_count, ConstraintSet::f_ext_constraints, Model::gravity, Model::I, Model::IA, JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, Model::mJoints, Model::multdof3_Dinv, Model::multdof3_S, Model::multdof3_U, Model::multdof3_u, Model::pA, Model::S, Model::U, Model::u, Model::v, Model::X_base, and Model::X_lambda.
unsigned int GetMovableBodyId  (  Model &  model, 
unsigned int  id  
) 
Definition at line 1674 of file Constraints.cc.
References Model::fixed_body_discriminator, Model::IsFixedBodyId(), and Model::mFixedBodies.
RBDL_DLLAPI void jcalc  (  Model &  model, 
unsigned int  joint_id,  
const Math::VectorNd &  q,  
const Math::VectorNd &  qdot  
) 
Computes all variables for a joint model.
By appropriate modification of this function all types of joints can be modeled. See RBDA Section 4.4 for details.
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  joint state variables 
qdot  joint velocity variables 
Definition at line 21 of file Joint.cc.
References Model::c_J, Joint::custom_joint_index, Model::GetQuaternion(), CustomJoint::jcalc(), jcalc_X_lambda_S(), jcalc_XJ(), JointTypeCustom, JointTypeEulerXYZ, JointTypeEulerYXZ, JointTypeEulerZYX, JointTypeHelical, JointTypeRevoluteX, JointTypeRevoluteY, JointTypeRevoluteZ, JointTypeSpherical, JointTypeTranslationXYZ, Model::mCustomJoints, Model::mJoints, Model::multdof3_S, Model::S, Quaternion::toMatrix(), Model::v_J, Model::X_J, Model::X_lambda, Model::X_T, RigidBodyDynamics::Math::Xrotx(), RigidBodyDynamics::Math::Xroty(), and RigidBodyDynamics::Math::Xrotz().
Definition at line 271 of file Joint.cc.
References Joint::custom_joint_index, SpatialTransform::E, Model::GetQuaternion(), CustomJoint::jcalc_X_lambda_S(), jcalc_XJ(), JointTypeCustom, JointTypeEulerXYZ, JointTypeEulerYXZ, JointTypeEulerZYX, JointTypeHelical, JointTypeRevoluteX, JointTypeRevoluteY, JointTypeRevoluteZ, JointTypeSpherical, JointTypeTranslationXYZ, Model::mCustomJoints, Model::mJoints, Model::multdof3_S, Model::S, Quaternion::toMatrix(), Model::X_lambda, Model::X_T, RigidBodyDynamics::Math::Xrotx(), RigidBodyDynamics::Math::Xroty(), and RigidBodyDynamics::Math::Xrotz().
RBDL_DLLAPI Math::SpatialTransform jcalc_XJ  (  Model &  model, 
unsigned int  joint_id,  
const Math::VectorNd &  q  
) 
Definition at line 222 of file Joint.cc.
References JointTypeCustom, JointTypeHelical, JointTypePrismatic, JointTypeRevolute, Model::mJoints, RigidBodyDynamics::Math::Xrot(), and RigidBodyDynamics::Math::Xtrans().

inline 
Definition at line 1439 of file Constraints.cc.
Definition at line 1638 of file Constraints.cc.
References LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, and LinearSolverPartialPivLU.