Kinematics APIs

Methods related to the kinematic calculations are provided in this section. Note that some methods are for use in the serial systems (unconstrained), while the others are specifically for use in the parallel (closed-loop) systems.

get_num_joints returns the degrees of freedom of the model

calc_EndEffectorJacobian computes the point Jacobian for a point on a body

calc_JacobianConstrained computes the Jacobian for the given constraint set

calc_EndEffectorVel computes the cartesian velocity of a point on a body

calc_InverseKinematics computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method

calc_ForwardKinematics computes the cartesian position of a point on a body relative to the base (world)

---

get_num_joints

Returns model's number of joints.

Specification:

returns: unsigned int N number of joints

cal_EndEffectorJacobian

Computes the point jacobian for a point on a body.

Specification:

input parameters:

const VectorNd &Q state vector of the internal joints

unsigned int body_id the id of the body

const Vector3d &body_point_position the position of the point in body-local data

returns: Math::MatrixNd J a matrix of dimensions 3 x #qdot_size where the result will be stored in

calc_JacobianConstrained

Computes the Jacobian for the given ConstraintSet.

Specification:

input parameters:

const VectorNd &Q state vector of the internal joints

ConstraintSet &CS the constraint set for which the Jacobian should be computed

returns: Math::MatrixNd J matrix where the output will be stored in

calc_EndEffectorVel

Computes the cartesian velocity of a point on a body (can be used for calculating end effector's cartesian velocity).

Specification:

input parameters:

const VectorNd &Q state vector of the internal joints

const VectorNd &QDot Angular velocity vector of the internal joints

unsigned int body_id the id of the body

const Vector3d &body_point_position the position of the point in body-local data

returns: Vector3d V Cartesian velocity of a point relative to the base frame (world )

calc_InverseKinematics

Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method).

Specification:

input parameters:

const VectorNd &Qinit initial guess for the state

const std::vector< unsigned int > &body_id a vector of all bodies for which we have kinematic target positions

const Vector3d &body_point_position a vector of points in body local coordinates that are to be matched to target positions

const std::vector<Vector3d> &target_pos a vector of target positions

returns: VectorNd &Qres output of the computed inverse kinematics

calc_ForwardKinematics

Method for calculating the cartesian position of a point on a body relative to the base (world).

Specification:

input parameters:

const VectorNd &Q state vector of the internal joints

unsigned int body_id the id of the body

const Vector3d &body_point_position the position of the point in body-local data

returns: Vector3d P vector representing the cartesian position of the body point with respect to the base frame (world).