BodyTree

This class represents a serial modular mechanical system [1]. A BodyTree consists of N joints and bodies serially interconneted.

The dynamic terms are computed through the Generalized Inverse Dynamics (GID) and Actuation Inverse Dynamics (AID) algorithms of [1].

class BodyTree

Bases: handle

Class modeling a mechanical system of generic joints and bodies serially interconnected.

Constructor Summary
BodyTree(Joints, Bodies)

Construct a BodyTree consisting of joints and bodies.

Parameters:

  • Joints ({Joint}) – Cell array of joints of the tree

  • Bodies ({Body}) – Cell array of bodies of the tree

Property Summary
Bodies

Bodies of the kinematic tree organized in a cell array.

Joints

Joints of the kinematic tree organized in a cell array.

MassConditionNumber

Remove

Type:

TODO

MaxBodiesNumber

Maximum number of bodies and joints, required for code generation.

N_B

Total number of joints/bodies in the kinematic tree.

T0

Orientation of the base frame with respect to the world frame.

g

Gravity force in the base frame.

n

Total number of DoF of the kinematic tree.

Method Summary
ApparentForce(q, dq)

Evaluate the generalized apparent force.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • dq ([double], [sym]) – First-order time derivative of configuration variables

ApparentMatrix(q, dq)

Evaluate the apparent matrix.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • dq ([double], [sym]) – First-order time derivative of configuration variables

BodyJacobian(q, idx)

Evaluate the body Jacobian of the i-th body. If i is not specified, the method returns the body Jacobian of each body of the chain.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • idx ([int]) – Array of body indexes indicating the bodies for which the jacobian has to be evaluated

Returns:

length(idx)*6 x n body Jacobian with angular and linear components for each body specified by idx

Return type:

{[double], [sym]}

D(q, dq)

Evaluate the generalized damping force.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • dq ([double], [sym]) – First-order time derivative of configuration variables

DirectKinematics(q, idx)

Evaluate the direct kinematics.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • idx ([int]) – Ordered array of body indexes indicating the bodies for which the direct kinematics has to be evaluated

Returns:

Homogeneous transformation matrices for the body specified by idx.

Return type:

([double], [sym])

Energy(q, dq, q_ref)

Evaluate the system energy. It is assumed that the elastic force, if any is linear.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • dq ([double], [sym]) – First-order time derivative of configuration variables

  • q_ref ([double], [sym]) – Reference configuration variables for the elastic and gravitational energy

EquilibriumConfiguration(q0, tau)

Find an equilibrium configuration by solving numerically the equilibrium equations.

Parameters:

  • q0 ([double]) – Initial guess for the equlibrium

  • tau ([double]) – Generalized actuation force

ForwardDynamics(q, dq, tau)

Forward dynamics through the inverse dynamics algorithm.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • dq ([double], [sym]) – First-order time derivative of the configuration variables

  • tau ([double], [sym]) – Generalized actuation force

GravityForce(q)

Evaluate the generalized gravitational force.

Parameters:

q ([double], [sym]) – Configuration variables

InverseDynamics(q, dq, ddq)

Evaluate the inverse dynamics using the Generalized Inverse Dynamics (GID) algorithm.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • dq ([double], [sym]) – First-order time derivative of configuration variables

  • ddq ([double], [sym]) – Second-order time derivative of configuration variables

InverseKinematics(T, options)

Evaluate the inverse kinematics numerically using a Newton-Rapson iteration scheme.

Parameters:

  • T ([double, double]) – Target transformation matrices vertically stacked

  • q0 ([double, double]) – Initial guess, the default value is q0 = zeros(n, 1)

  • idx ([double]) – Vector of indexes identifing the bodies in the chain for which T has to be found

  • N (double) – Maximum number of iterations

  • task_flags ([double, bool]) – Vector of flags specifying for each indexed body what components of the task vector should be considered

Returns:

Homogeneous transformation matrices for each body.

Return type:

{[double], [sym]}

K(q)

Evaluate the generalized elastic force.

Parameters:

q ([double], [sym]) – Configuration variables

MassMatrix(q)

Evaluate the mass matrix.

Parameters:

q ([double], [sym]) – Configuration variables

StateSpaceForwardDynamics(t, x, u)

Evaluate the forward dynamics in state space form.

Parameters:

  • t (double) – Time

  • x ([double], [sym]) – State variables consisting of configuration variables and time derivative of the configuration variables

  • u ([double], [sym]) – Generalized actuation force

TreeUpdate(q, dq, ddq, options)

Update the state of the BodyTree.

Parameters:

  • q ([double], [sym]) – Configuration variables

  • dq ([double], [sym]) – First-order time derivative of configuration variables

  • ddq ([double], [sym]) – Second-order time derivative of configuration variables

getBodyConfigurationIndex(idx)
Get the indexes of the configuration vector for the bodies

specified by idx.

Parameters:

  • idx ([double, int]) – Bodies for which the configuration

  • computed. (indexes have to be)

Returns:

length(idx) x 2 matrix where each row specifies the start and end index of the corresponding body in the idx vector. If idx is not specified, then the function returns the indexes for all the bodies in the tree.

Return type:

{[double], [sym]}

getJointConfigurationIndex(idx)
Get the indexes of the configuration vector for the joints

specified by idx.

Parameters:

  • idx ([double, int]) – Joints for which the configuration

  • computed. (indexes have to be)

Returns:

length(idx) x 2 matrix where each row specifies the start and end index of the corresponding joint in the idx vector. If idx is not specified, then the function returns the indexes for all the joints in the tree.

Return type:

{[double], [sym]}