Namespaces | Functions
Kinematics.cc File Reference
#include <boost/bind.hpp>
#include <iostream>
#include <limits>
#include <cstring>
#include <assert.h>
#include "rdl_dynamics/FramePoint.hpp"
#include "rdl_dynamics/Kinematics.h"
#include "rdl_dynamics/rdl_mathutils.h"
Include dependency graph for Kinematics.cc:

Go to the source code of this file.

Namespaces

 RobotDynamics
 Namespace for all structures of the RobotDynamics library.
 

Functions

void RobotDynamics::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 $ ith $ body, as $ v^{i,0}_{i} = G(q) \dot{q} $. For the spatial jacobian of body $i$, each column j is computed as follows. More...
 
void RobotDynamics::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 $i$th body, as $ a^{i,0}_{i} = G(q) \ddot{q} + \dot{G}(q)\dot{q} $ where $G(q)$ is the body jacobian of body $i$. More...
 
Math::FrameVector RobotDynamics::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 RobotDynamics::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::FrameVectorPair RobotDynamics::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...
 
void RobotDynamics::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 RobotDynamics::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 $g(q(t))$ for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix $G(q)$. More...
 
void RobotDynamics::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 $g(q(t))$ for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix $G(q)$. More...
 
void RobotDynamics::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 RobotDynamics::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 RobotDynamics::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 RobotDynamics::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 RobotDynamics::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...
 
Math::FrameVector RobotDynamics::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 RobotDynamics::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 RobotDynamics::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...
 
Math::FrameVectorPair RobotDynamics::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...
 
void RobotDynamics::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 $i$ being the "base" frame, $j$ being the "relative" frame, and $k$ 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 RobotDynamics::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 RobotDynamics::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 $i$ being the "base" frame, $j$ 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 $i$ w.r.t frame $j$, expressed in frame $k$. Additionally the jacobian $J_{i}^{k,j}$ can be computed by RobotDynamics::calcRelativeBodySpatialJacobian. More...
 
void RobotDynamics::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 RobotDynamics::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 RobotDynamics::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...
 
Math::SpatialAcceleration RobotDynamics::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...
 
Math::SpatialAcceleration RobotDynamics::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::SpatialMotion RobotDynamics::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 $ i $ this is written as $ v^{i,0}_{i} $. Given another body, body $ j $, the velocity of body $ i $ relative to body $ j $ and expressed in body frame $ i $ is computed by,

\[ v^{i,j}_{i} = v^{i,0}_{i} - ^{i}X_{j} v^{j,0}_{j} \]

. More...

 
Math::SpatialMotion RobotDynamics::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 $ i $ this is written as $ v^{i,0}_{i} $. Given another body, body $ j $, the velocity of body $ i $ relative to body $ j $ and expressed in body frame $ i $ is computed by,

\[ v^{i,j}_{i} = v^{i,0}_{i} - ^{i}X_{j} v^{j,0}_{j} \]

. More...

 
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)
 
void RobotDynamics::updateAccelerations (Model &model, const Math::VectorNd &QDDot)
 Computes only the accelerations of the bodies. More...
 
void RobotDynamics::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 RobotDynamics::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 RobotDynamics::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 RobotDynamics::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...
 

rdl_dynamics
Author(s):
autogenerated on Tue Apr 20 2021 02:25:28