416 lines
16 KiB
C++
416 lines
16 KiB
C++
/*
|
|
* RBDL - Rigid Body Dynamics Library
|
|
* Copyright (c) 2011-2018 Martin Felis <martin@fysx.org>
|
|
*
|
|
* Licensed under the zlib license. See LICENSE for more details.
|
|
*/
|
|
|
|
#ifndef RBDL_KINEMATICS_H
|
|
#define RBDL_KINEMATICS_H
|
|
|
|
#include "rbdl/rbdl_math.h"
|
|
#include <assert.h>
|
|
#include <iostream>
|
|
#include "rbdl/Logging.h"
|
|
|
|
namespace RigidBodyDynamics {
|
|
|
|
/** \page kinematics_page Kinematics
|
|
* All functions related to kinematics are specified in the \ref
|
|
* kinematics_group "Kinematics Module".
|
|
*
|
|
* \note Please note that in the Rigid %Body Dynamics Library all angles
|
|
* are specified in radians.
|
|
*
|
|
* \defgroup kinematics_group Kinematics
|
|
* @{
|
|
*
|
|
* \note Please note that in the Rigid %Body Dynamics Library all angles
|
|
* are specified in radians.
|
|
*/
|
|
|
|
/** \brief Updates and computes velocities and accelerations of the bodies
|
|
*
|
|
* This function updates the kinematic variables such as body velocities
|
|
* and accelerations in the model to reflect the variables passed to this function.
|
|
*
|
|
* \param model the model
|
|
* \param Q the positional variables of the model
|
|
* \param QDot the generalized velocities of the joints
|
|
* \param QDDot the generalized accelerations of the joints
|
|
*/
|
|
RBDL_DLLAPI void UpdateKinematics (Model &model,
|
|
const Math::VectorNd &Q,
|
|
const Math::VectorNd &QDot,
|
|
const Math::VectorNd &QDDot
|
|
);
|
|
|
|
/** \brief Selectively updates model internal states of body positions, velocities and/or accelerations.
|
|
*
|
|
* This function updates the kinematic variables such as body velocities and
|
|
* accelerations in the model to reflect the variables passed to this function.
|
|
*
|
|
* In contrast to UpdateKinematics() this function allows to update the model
|
|
* state with values one is interested and thus reduce computations (e.g. only
|
|
* positions, only positions + accelerations, only velocities, etc.).
|
|
|
|
* \param model the model
|
|
* \param Q the positional variables of the model
|
|
* \param QDot the generalized velocities of the joints
|
|
* \param QDDot the generalized accelerations of the joints
|
|
*/
|
|
RBDL_DLLAPI void UpdateKinematicsCustom (Model &model,
|
|
const Math::VectorNd *Q,
|
|
const Math::VectorNd *QDot,
|
|
const Math::VectorNd *QDDot
|
|
);
|
|
|
|
/** \brief Returns the base coordinates of a point given in body coordinates.
|
|
*
|
|
* \param model the rigid body model
|
|
* \param Q the curent genereralized positions
|
|
* \param body_id id of the body for which the point coordinates are expressed
|
|
* \param body_point_position coordinates of the point in body coordinates
|
|
* \param update_kinematics whether UpdateKinematics() should be called
|
|
* or not (default: true)
|
|
*
|
|
* \returns a 3-D vector with coordinates of the point in base coordinates
|
|
*/
|
|
RBDL_DLLAPI Math::Vector3d CalcBodyToBaseCoordinates (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
unsigned int body_id,
|
|
const Math::Vector3d &body_point_position,
|
|
bool update_kinematics = true);
|
|
|
|
/** \brief Returns the body coordinates of a point given in base coordinates.
|
|
*
|
|
* \param model the rigid body model
|
|
* \param Q the curent genereralized positions
|
|
* \param body_id id of the body for which the point coordinates are expressed
|
|
* \param base_point_position coordinates of the point in base coordinates
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not
|
|
* (default: true).
|
|
*
|
|
* \returns a 3-D vector with coordinates of the point in body coordinates
|
|
*/
|
|
RBDL_DLLAPI Math::Vector3d CalcBaseToBodyCoordinates (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
unsigned int body_id,
|
|
const Math::Vector3d &base_point_position,
|
|
bool update_kinematics = true);
|
|
|
|
/** \brief Returns the orientation of a given body as 3x3 matrix
|
|
*
|
|
* \param model the rigid body model
|
|
* \param Q the curent genereralized positions
|
|
* \param body_id id of the body for which the point coordinates are expressed
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not
|
|
* (default: true).
|
|
*
|
|
* \returns An orthonormal 3x3 matrix that rotates vectors from base coordinates
|
|
* to body coordinates.
|
|
*/
|
|
RBDL_DLLAPI Math::Matrix3d CalcBodyWorldOrientation (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
const unsigned int body_id,
|
|
bool update_kinematics = true);
|
|
|
|
/** \brief Computes the point jacobian for a point on a body
|
|
*
|
|
* If a position of a point is computed by a function \f$g(q(t))\f$ for which its
|
|
* time derivative is \f$\frac{d}{dt} g(q(t)) = G(q)\dot{q}\f$ then this
|
|
* function computes the jacobian matrix \f$G(q)\f$.
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param body_id the id of the body
|
|
* \param point_position the position of the point in body-local data
|
|
* \param G a matrix of dimensions 3 x \#qdot_size where the result will be stored in
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
|
|
*
|
|
* The result will be returned via the G argument.
|
|
*
|
|
* \note This function only evaluates the entries of G that are non-zero. One
|
|
* Before calling this function one has to ensure that all other values
|
|
* have been set to zero, e.g. by calling G.setZero().
|
|
*
|
|
*/
|
|
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
|
|
);
|
|
|
|
/** \brief Computes a 6-D Jacobian for a point on a body
|
|
*
|
|
* Computes the 6-D Jacobian \f$G(q)\f$ that when multiplied with
|
|
* \f$\dot{q}\f$ gives a 6-D vector that has the angular velocity as the
|
|
* first three entries and the linear velocity as the last three entries.
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param body_id the id of the body
|
|
* \param point_position the position of the point in body-local data
|
|
* \param G a matrix of dimensions 6 x \#qdot_size where the result will be stored in
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
|
|
*
|
|
* The result will be returned via the G argument.
|
|
*
|
|
* \note This function only evaluates the entries of G that are non-zero. One
|
|
* Before calling this function one has to ensure that all other values
|
|
* have been set to zero, e.g. by calling G.setZero().
|
|
*
|
|
*/
|
|
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
|
|
);
|
|
|
|
/** \brief Computes the spatial jacobian for a body
|
|
*
|
|
* The spatial velocity of a body at the origin of coordinate system of
|
|
* body \f$i\f$ can be expressed as \f${}^i \hat{v}_i = G(q) * \dot{q}\f$.
|
|
* The matrix \f$G(q)\f$ is called the spatial body jacobian of the body
|
|
* and can be computed using this function.
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param body_id the id of the body
|
|
* \param G a matrix of size 6 x \#qdot_size where the result will be stored in
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
|
|
*
|
|
* The result will be returned via the G argument and represents the
|
|
* body Jacobian expressed at the origin of the body.
|
|
*
|
|
* \note This function only evaluates the entries of G that are non-zero. One
|
|
* Before calling this function one has to ensure that all other values
|
|
* have been set to zero, e.g. by calling G.setZero().
|
|
*/
|
|
RBDL_DLLAPI void CalcBodySpatialJacobian (
|
|
Model &model,
|
|
const Math::VectorNd &Q,
|
|
unsigned int body_id,
|
|
Math::MatrixNd &G,
|
|
bool update_kinematics = true
|
|
);
|
|
|
|
/** \brief Computes the velocity of a point on a body
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param QDot velocity vector of the internal joints
|
|
* \param body_id the id of the body
|
|
* \param point_position the position of the point in body-local data
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
|
|
*
|
|
* \returns The cartesian velocity of the point in global frame (output)
|
|
*/
|
|
RBDL_DLLAPI Math::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
|
|
);
|
|
|
|
/** \brief Computes angular and linear velocity of a point that is fixed on a body
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param QDot velocity vector of the internal joints
|
|
* \param body_id the id of the body
|
|
* \param point_position the position of the point in body-local data
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
|
|
*
|
|
* \returns The a 6-D vector for which the first three elements are the
|
|
* angular velocity and the last three elements the linear velocity in the
|
|
* global reference system.
|
|
*/
|
|
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
|
|
);
|
|
|
|
/** \brief Computes the linear acceleration of a point on a body
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param QDot velocity vector of the internal joints
|
|
* \param QDDot velocity vector of the internal joints
|
|
* \param body_id the id of the body
|
|
* \param point_position the position of the point in body-local data
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
|
|
*
|
|
* \returns The cartesian acceleration of the point in global frame (output)
|
|
*
|
|
* The kinematic state of the model has to be updated before valid
|
|
* values can be obtained. This can either be done by calling
|
|
* UpdateKinematics() or setting the last parameter update_kinematics to
|
|
* true (default).
|
|
*
|
|
* \warning If this function is called after ForwardDynamics() without
|
|
* an update of the kinematic state one has to add the gravity
|
|
* acceleration has to be added to the result.
|
|
*/
|
|
RBDL_DLLAPI
|
|
Math::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
|
|
);
|
|
|
|
/** \brief Computes linear and angular acceleration of a point on a body
|
|
*
|
|
* \param model rigid body model
|
|
* \param Q state vector of the internal joints
|
|
* \param QDot velocity vector of the internal joints
|
|
* \param QDDot velocity vector of the internal joints
|
|
* \param body_id the id of the body
|
|
* \param point_position the position of the point in body-local data
|
|
* \param update_kinematics whether UpdateKinematics() should be called or not (default: true)
|
|
*
|
|
* \returns A 6-D vector where the first three elements are the angular
|
|
* acceleration and the last three elements the linear accelerations of
|
|
* the point.
|
|
*
|
|
* The kinematic state of the model has to be updated before valid
|
|
* values can be obtained. This can either be done by calling
|
|
* UpdateKinematics() or setting the last parameter update_kinematics to
|
|
* true (default).
|
|
*
|
|
* \warning If this function is called after ForwardDynamics() without
|
|
* an update of the kinematic state one has to add the gravity
|
|
* acceleration has to be added to the result.
|
|
*/
|
|
RBDL_DLLAPI
|
|
Math::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
|
|
);
|
|
|
|
/** \brief Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method)
|
|
*
|
|
* \param model rigid body model
|
|
* \param Qinit initial guess for the state
|
|
* \param body_id a vector of all bodies for which we we have kinematic target positions
|
|
* \param body_point a vector of points in body local coordinates that are
|
|
* to be matched to target positions
|
|
* \param target_pos a vector of target positions
|
|
* \param Qres output of the computed inverse kinematics
|
|
* \param step_tol tolerance used for convergence detection
|
|
* \param lambda damping factor for the least squares function
|
|
* \param max_iter maximum number of steps that should be performed
|
|
* \returns true on success, false otherwise
|
|
*
|
|
* This function repeatedly computes
|
|
* \f[ Qres = Qres + \Delta \theta\f]
|
|
* \f[ \Delta \theta = G^T (G^T G + \lambda^2 I)^{-1} e \f]
|
|
* where \f$G = G(q) = \frac{d}{dt} g(q(t))\f$ and \f$e\f$ is the
|
|
* correction of the body points so that they coincide with the target
|
|
* positions. The function returns true when \f$||\Delta \theta||_2 \le\f$
|
|
* step_tol or if the error between body points and target gets smaller
|
|
* than step_tol. Otherwise it returns false.
|
|
*
|
|
* The parameter \f$\lambda\f$ is the damping factor that has to
|
|
* be chosen carefully. In case of unreachable positions higher values (e.g
|
|
* 0.9) can be helpful. Otherwise values of 0.0001, 0.001, 0.01, 0.1 might
|
|
* yield good results. See the literature for best practices.
|
|
*
|
|
* \warning The actual accuracy might be rather low (~1.0e-2)! Use this function with a
|
|
* grain of suspicion.
|
|
*/
|
|
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.0e-12,
|
|
double lambda = 0.01,
|
|
unsigned int max_iter = 50
|
|
);
|
|
|
|
RBDL_DLLAPI Math::Vector3d CalcAngularVelocityfromMatrix (
|
|
const Math::Matrix3d &RotMat);
|
|
|
|
struct RBDL_DLLAPI InverseKinematicsConstraintSet {
|
|
enum ConstraintType {
|
|
ConstraintTypePosition = 0,
|
|
ConstraintTypeOrientation,
|
|
ConstraintTypeFull
|
|
};
|
|
|
|
InverseKinematicsConstraintSet();
|
|
|
|
Math::MatrixNd J; /// the Jacobian of all constraints
|
|
Math::MatrixNd G; /// temporary storage of a single body Jacobian
|
|
Math::VectorNd e; /// Vector with all the constraint residuals.
|
|
|
|
unsigned int num_constraints; //size of all constraints
|
|
double lambda; /// Damping factor, the default value of 1.0e-6 is reasonable for most problems
|
|
unsigned int num_steps; // The number of iterations performed
|
|
unsigned int max_steps; // Maximum number of steps (default 300), abort if more steps are performed.
|
|
double step_tol; // Step tolerance (default = 1.0e-12). If the computed step length is smaller than this value the algorithm terminates successfully (i.e. returns true). If error_norm is still larger than constraint_tol then this usually means that the target is unreachable.
|
|
double constraint_tol; // Constraint tolerance (default = 1.0e-12). If error_norm is smaller than this value the algorithm terminates successfully, i.e. all constraints are satisfied.
|
|
double error_norm; // Norm of the constraint residual vector.
|
|
|
|
// everything to define a IKin constraint
|
|
std::vector<ConstraintType> constraint_type;
|
|
std::vector<unsigned int> body_ids;
|
|
std::vector<Math::Vector3d> body_points;
|
|
std::vector<Math::Vector3d> target_positions;
|
|
std::vector<Math::Matrix3d> target_orientations;
|
|
std::vector<unsigned int> constraint_row_index;
|
|
|
|
// Adds a point constraint that tries to get a body point close to a
|
|
// point described in base coordinates.
|
|
unsigned int AddPointConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos);
|
|
// Adds an orientation constraint that tries to align a body to the
|
|
// orientation specified as a rotation matrix expressed in base
|
|
// coordinates.
|
|
unsigned int AddOrientationConstraint (unsigned int body_id, const Math::Matrix3d &target_orientation);
|
|
// Adds a constraint on both location and orientation of a body.
|
|
unsigned int AddFullConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &target_pos, const Math::Matrix3d &target_orientation);
|
|
// Clears all entries of the constraint setting
|
|
unsigned int ClearConstraints();
|
|
};
|
|
|
|
RBDL_DLLAPI bool InverseKinematics (
|
|
Model &model,
|
|
const Math::VectorNd &Qinit,
|
|
InverseKinematicsConstraintSet &CS,
|
|
Math::VectorNd &Qres
|
|
);
|
|
|
|
/** @} */
|
|
|
|
}
|
|
|
|
/* RBDL_KINEMATICS_H */
|
|
#endif
|