A class with basic model descriptions and functionalities of the multi-link rigid-body robot platform. More...

Inheritance diagram for RobotLinks:
[legend]

Public Member Functions

 RobotLinks (char config,struct base, load_path)
 The class constructor function. More...
 
function obj = addContact (ContactFrame contact,struct fric_coef,struct geometry, load_path)
 Adds contact constraints for the robot manipulator. More...
 
function obj = addFixedJoint (cellstr fixed_joints,char load_path)
 enforces fixed joints as holonomic constraints of the dynamical system of the robot manipulator More...
 
function obj = removeContact (cellstr contact)
 Remove contact constraints defined for the system. More...
 
function base_link = findBaseLink (RigidJoint joints)
 Determines the base link of an array of rigid joints. More...
 
function terminals = findEndEffector (RigidJoint joints)
 Determines the end effector link of an array of rigid joints. More...
 
function indices = getLinkIndices (link_names)
 Returns the indices of links specified by the name string. More...
 
function indices = getJointIndices (joint_names)
 Returns the indices of joints specified by the name string. More...
 
function obj = cconfigure (config, base, load_path)
 
function obj = configureKinematics (struct dofs,struct links)
 Compute the forward kinematics of the robots using the coordinate configurations. More...
 
function obj = configureDynamics (varargin varargin)
 Set the symbolic expressions of the dynamics of the rigid body system. More...
 
function obj = configureActuator (cell dofs,struct actuators)
 Configure the actuator information of the actuated joints. More...
 
function ang = getRelativeEulerAngles (cell frame,matrix R,matrix p)
 Returns the symbolic representation of the Euler angles of a rigid link. More...
 
function varargoutgetCartesianPosition (cell frame,matrix p)
 Returns the symbolic representation of the Cartesian positions of a rigid point specified by a coordinate frame and an offset. More...
 
function varargoutgetEulerAngles (cell frame,matrix p)
 Returns the symbolic representation of the Euler angles of a rigid link. More...
 
function varargoutgetBodyJacobian (cell frame,matrix p)
 Returns the symbolic representation of the body jacobians of coordinate frames. More...
 
function varargoutgetSpatialJacobian (cell frame,matrix p)
 Returns the symbolic representation of the spatial jacobian of the point that is rigidly attached to the link with a given offset. More...
 
function mass = getTotalMass ()
 Return the total mass of the robot model. More...
 
function pos = getComPosition ()
 Returns the symbolic representation of the robot manipulator. More...
 
function bounds = getLimits ()
 Return the boundary values (limits) of the joint position/velocity variables, and torques. More...
 
function obj = loadDynamics (char file_path,logical skip_load_vf,cellstr extra_fvecs)
 Load the symbolic expression of dynamical equations from a previously save MX files. More...
 
- Public Member Functions inherited from ContinuousDynamics
 ContinuousDynamics (char type,char name)
 The class construction function. More...
 
function obj = addState (SymVariable x,SymVariable dx,SymVariable ddx)
 overload the superclass addState method with fixed state fields More...
 
function sol = simulate (t0, x0, tf, controller, params, logger, eventnames, options, solver)
 Simulate the dynamical system. More...
 
function [ value , isterminal , direction ] = checkGuard (t, x, controller, params, eventfuncs)
 Detect the guard condition (event trigger) More...
 
function obj = setMassMatrix (M)
 Set the mass matrix M(x) of the system. More...
 
function obj = setDriftVector (vf)
 Set the drift vector field Fvec(x) or Fvec(x,dx) of the system. More...
 
function obj = appendDriftVector (vf)
 Append additional vf to the drift vector field Fvec(x) or Fvec(x,dx) of the system. More...
 
function M = calcMassMatrix (x)
 calculates the mass matrix Mmat(x) More...
 
function f = calcDriftVector (x, dx)
 calculates the mass matrix Fvec(x) or Fvec(x,dx) More...
 
function [ xdot , extra ] = calcDynamics (t, x, controller, params, logger)
 calculate the dynamical equation dx More...
 
function [ xdot , extra ] = firstOrderDynamics (t, x, controller, params, logger)
 calculate the dynamical equation of the first order dynamical system More...
 
function [ xdot , extra ] = secondOrderDynamics (t, x, controller, params, logger)
 calculate the dynamical equation of the second order dynamical system More...
 
function obj = compile (export_path, varargin)
 export the symbolic expressions of the system dynamics matrices and vectors and compile as MEX files. More...
 
function obj = addEvent (constr)
 Adds event function for the dynamical system states and inputs. More...
 
function obj = removeEvent (name)
 Remove event functions defined for the system. More...
 
function obj = addHolonomicConstraint (constr, load_path)
 Adds a holonomic constraint to the dynamical system. More...
 
function obj = removeHolonomicConstraint (name)
 Remove holonomic constraints defined for the system. More...
 
function obj = addUnilateralConstraint (constr)
 Adds unilateral (inequality) constraints on the dynamical system states and inputs. More...
 
function obj = removeUnilateralConstraint (name)
 Remove unilateral constraints defined for the system. More...
 
function obj = addVirtualConstraint (constr)
 Adds a set of virtual constraints to the dynamical system. More...
 
function obj = removeVirtualConstraint (name)
 Remove virtual constraints defined for the system. More...
 
function obj = saveExpression (export_path, varargin)
 save the symbolic expression of system dynamical equations to a MX binary files More...
 
function obj = loadDynamics (file_path, vf_names, skip_load_vf)
 load the symbolic expression of system dynamical equations from MX binary files for fast loading More...
 
function obj = clearKernel (varargin)
 
- Public Member Functions inherited from DynamicalSystem
function obj = addState (varargin)
 Add state variables of the dynamical system. More...
 
function obj = addInput (category, name, var, gf, varargin)
 Add input variables of the dynamical system. More...
 
function obj = removeInput (category, name)
 Remove input variables of the dynamical system. More...
 
function obj = addParam (varargin)
 Add parameter variables of the dynamical system. More...
 
function obj = removeParam (param_name)
 Remove parameter variables of the dynamical system. More...
 
function obj = setParamValue (varargin)
 set the actual value of the system parameters More...
 
function obj = compile (export_path, varargin)
 export the symbolic expressions of the system dynamics matrices and vectors and compile as MEX files. More...
 
function obj = loadDynamics (export_path, varargin)
 
function obj = saveExpression (export_path, varargin)
 save the symbolic expression of system dynamical equations to a MX binary files More...
 
 DynamicalSystem (char type,char name)
 The class construction function. More...
 
function obj = setName (name)
 set the name of the dynamical system More...
 
function ret = isParam (char name)
 
function var_group = validateVarName (char name)
 Validate the group and category of the variables specified by the input name More...
 
function obj = setType (char type)
 Sets the type of the dynamical system. More...
 

Public Attributes

char ConfigFile
 The full path of the model configuration file. More...
 
struct Links
 An array of rigid links. More...
 
RigidJoint Joints
 An array of rigid joints. More...
 
- Public Attributes inherited from ContinuousDynamics
function_handle PreProcess
 pre-process function handle of the object before the simulation More...
 
function_handle PostProcess
 pre-process function handle of the object after the simulation More...
 
function_handle UserNlpConstraint
 A handle to a function called by a trajectory optimization NLP to enforce system specific constraints. More...
 
struct Flow
 The flow (trajectory) from the dynamical system simulation. More...
 
SymFunction Mmat
 The mass matrix Mmat(x) More...
 
cell Fvec
 The cell array of the drift vector fields Fvec(x) SymFunction. More...
 
struct HolonomicConstraints
 The holonomic constraints of the dynamical system. More...
 
struct UnilateralConstraints
 The unilateral constraints of the dynamical system. More...
 
struct VirtualConstraints
 The virtual constraints of the dynamical system. More...
 
struct EventFuncs
 The event functions. More...
 
SymFunction MmatDx
 The left hand side of the dynamical equation: M(x)dx or M(x)ddx. More...
 
- Public Attributes inherited from DynamicalSystem
char Name
 The unique name identification of the system. More...
 
function_handle ExternalInputFun
 Returns the external input defined on the dynamical system. More...
 
char Type
 The highest order of the state derivatives of the system. More...
 
struct States
 A structure that contains the symbolic representation of state variables. More...
 
double numState
 The total number of system states. More...
 
struct Inputs
 A structure that contains the symbolic representation of input variables. More...
 
struct Params
 The parameters of the system. More...
 
struct Gvec
 The struct of input vector fields Gvec(x,u) SymFunction. More...
 
struct Gmap
 The struct of input map Gmap(x) SymFunction. More...
 
- Public Attributes inherited from handle
 addlistener
 Creates a listener for the specified event and assigns a callback function to execute when the event occurs. More...
 
 notify
 Broadcast a notice that a specific event is occurring on a specified handle object or array of handle objects. More...
 
 delete
 Handle object destructor method that is called when the object's lifecycle ends. More...
 
 disp
 Handle object disp method which is called by the display method. See the MATLAB disp function. More...
 
 display
 Handle object display method called when MATLAB software interprets an expression returning a handle object that is not terminated by a semicolon. See the MATLAB display function. More...
 
 findobj
 Finds objects matching the specified conditions from the input array of handle objects. More...
 
 findprop
 Returns a meta.property objects associated with the specified property name. More...
 
 fields
 Returns a cell array of string containing the names of public properties. More...
 
 fieldnames
 Returns a cell array of string containing the names of public properties. See the MATLAB fieldnames function. More...
 
 isvalid
 Returns a logical array in which elements are true if the corresponding elements in the input array are valid handles. This method is Sealed so you cannot override it in a handle subclass. More...
 
 eq
 Relational functions example. See details for more information. More...
 
 transpose
 Transposes the elements of the handle object array. More...
 
 permute
 Rearranges the dimensions of the handle object array. See the MATLAB permute function. More...
 
 reshape
 hanges the dimensions of the handle object array to the specified dimensions. See the MATLAB reshape function. More...
 
 sort
 ort the handle objects in any array in ascending or descending order. More...
 

Additional Inherited Members

- Protected Member Functions inherited from DynamicalSystem
function value = getValue (vars)
 returns the variables (vars) value that are stored during computing the dynamics More...
 
function v_type = validateSystemType (type)
 validate if it is valid system type More...
 
- Protected Attributes inherited from DynamicalSystem
double t_
 The time. More...
 
struct states_
 The states. More...
 
struct inputs_
 The inputs. More...
 
struct params_
 The parameters. More...
 
struct GmapName_
 The name of the Gmap functions. More...
 
struct GvecName_
 The name of the Gvec functions. More...
 

Detailed Description

A class with basic model descriptions and functionalities of the multi-link rigid-body robot platform.

References
The descriptions of rigid body model is based on but not limited to the following resrouces
Author
Ayonga Hereid
Date
2016-09-26
New in 2.1:
(Ayonga Hereid, 2017-03-26) Change RobotManipulator to RobotLinks
New in 2.0:
(Ayonga Hereid, 2017-03-26) Change RigidBodyModel to RobotManipulator
New in 1.0:
(Ayonga Hereid, 2016-09-26) Migrated from old modelConfig class

Copyright (c) 2016, AMBER Lab All right reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted only in compliance with the BSD 3-Clause license, see http://www.opensource.org/licenses/bsd-license.php

Constructor & Destructor Documentation

◆ RobotLinks()

RobotLinks.RobotLinks ( char  config,
struct  base,
  load_path 
)
inline

The class constructor function.

Parameters
configthe configuration of the joints and links
basethe configuration of the floating base coordinates

Member Function Documentation

◆ addContact()

function obj = RobotLinks.addContact ( ContactFrame  contact,
struct  fric_coef,
struct  geometry,
  load_path 
)

Adds contact constraints for the robot manipulator.

Parameters
contactthe contact frame object
fric_coefthe friction coefficient
geometrythe geometry constant of the contact
Required fields of contact:
Optional fields of geometry:
  • la —  the distance from the origin to the rolling edge along the positive x-axis
  • lb —  the distance from the origin to the rolling edge along the negative x-axis
  • La —  the distance from the origin to the rolling edge along the negative y-axis
  • Lb —  the distance from the origin to the rolling edge along the positive y-axis
  • RefFrame —  The frame describing the ground contact zero configuration.
Optional fields of fric_coef:
  • mu —  the (static) coefficient of friction.
  • gamma —  the coefficient of torsional friction

◆ addFixedJoint()

function obj = RobotLinks.addFixedJoint ( cellstr  fixed_joints,
char  load_path 
)

enforces fixed joints as holonomic constraints of the dynamical system of the robot manipulator

Parameters
fixed_jointsthe list of fixed joints
load_paththe path from which the symbolic expressions for the input map can be loaded
Required fields of fixed_joints:

◆ cconfigure()

function obj = RobotLinks.cconfigure (   config,
  base,
  load_path 
)

◆ configureActuator()

function obj = RobotLinks.configureActuator ( cell  dofs,
struct  actuators 
)

Configure the actuator information of the actuated joints.

The actuator configuration may include the inertia and gear ratio of the actuator.

Parameters
dofsan object array or name cellstr of actuated joints
actuatorsa structure array of actuator configuration
Required fields of dofs:

◆ configureDynamics()

function obj = RobotLinks.configureDynamics ( varargin  varargin)

Set the symbolic expressions of the dynamics of the rigid body system.

Parameters
vararginextra options
Note
!!! The minus sign !!! Fvec appears at the right hand side (typically -C(q,dq)dq - G(q) appears at the left hand side of the Euler-Lagrangian Equation)

References eval_math_fun().

◆ configureKinematics()

function obj = RobotLinks.configureKinematics ( struct  dofs,
struct  links 
)

Compute the forward kinematics of the robots using the coordinate configurations.

This function computes the kinematic tree structure of the multibody system, homogeneous transformation, and twists of the each coordinate frames.

Parameters
dofsa structure array of coordinate configuration
linksa structure array of link configuration
Generated fields of obj:

◆ findBaseLink()

function base_link = RobotLinks.findBaseLink ( RigidJoint  joints)

Determines the base link of an array of rigid joints.

The base link is the link that is not a child link of any joints.

Note
if joints argument is not provided, it uses the default Joints property of the object.
Parameters
jointsan array of rigid joints
Return values
basethe name of the base link
Required fields of joints:

◆ findEndEffector()

function [ rowvec terminals , cell terminal_links ] = RobotLinks.findEndEffector ( RigidJoint  joints)

Determines the end effector link of an array of rigid joints.

The end-effector link is the link that is not a parent link of any joints.

Note
if joints argument is not provided, it uses the default Joints property of the object.
Parameters
jointsan array of rigid joints
Return values
terminalsthe indices of the end effector coordinates
terminal_linksthe indices of the end effector links
Required fields of joints:

◆ getBodyJacobian()

function SymExpression J = RobotLinks.getBodyJacobian ( cell  frame,
matrix  p 
)

Returns the symbolic representation of the body jacobians of coordinate frames.

Each coordiante

Note
Syntax for ont point

jac = getBodyJacobian(obj,{Link1,[0,0,0.1]})

Note
Syntax for multiple points

[jac1,jac2] = getBodyJacobian(obj,{Link1,[0,0,0.1]},{Link2,[0.2,0,0.1]})

Parameters
framethe list of coordinate frame of the point
pthe offset of the point from the origin of the frame
Return values
Jthe 6xnDof Jacobian matrix of a rigid coordinate frame

References eval_math_fun().

◆ getCartesianPosition()

function SymExpression pos = RobotLinks.getCartesianPosition ( cell  frame,
matrix  p 
)

Returns the symbolic representation of the Cartesian positions of a rigid point specified by a coordinate frame and an offset.

Note
Syntax for ont point

getCartesianPosition(obj,pf,offset)

Note
Syntax for multiple points (offset should be np*3 matrix)

getCartesianPosition(obj,pfarray, offset)

Parameters
framethe list of coordinate frame of the point
pthe offset of the point from the origin of the frame
Return values
posthe 3-Dimensional SO(3) position vectors of the fixed rigid points

◆ getComPosition()

function SymExpression pos = RobotLinks.getComPosition ( )

Returns the symbolic representation of the robot manipulator.

Return values
posthe 3-Dimensional SO(3) position vector of the CoM of the system

References eval_math_fun().

◆ getEulerAngles()

function SymExpression ang = RobotLinks.getEulerAngles ( cell  frame,
matrix  p 
)

Returns the symbolic representation of the Euler angles of a rigid link.

Note
Syntax for ont point

getEulerAngles(obj,pf,offset)

Note
Syntax for multiple points (offset should be np*3 matrix)

getEulerAngles(obj,pfarray, offset)

Parameters
framethe list of coordinate frame of the point
pthe offset of the point from the origin of the frame
Return values
angthe 3-dimensional Euler angles (roll,pitch,yaw) vector of the CoM of the system

◆ getJointIndices()

function indices = RobotLinks.getJointIndices (   joint_names)

Returns the indices of joints specified by the name string.

See also
getLinkIndices, joints
Parameters
joint_namesa cell array of strings of the DoF name
Return values
indicesposition indices of joints in the obj.joints

References str_index().

◆ getLimits()

function struct bounds = RobotLinks.getLimits ( )

Return the boundary values (limits) of the joint position/velocity variables, and torques.

Return values
boundsthe boundaries
Generated fields of bounds:

◆ getLinkIndices()

function indices = RobotLinks.getLinkIndices (   link_names)

Returns the indices of links specified by the name string.

Parameters
link_namesa cell array of strings of the link name
Return values
indicesposition indices of joints in the obj.joints

◆ getRelativeEulerAngles()

function SymExpression ang = RobotLinks.getRelativeEulerAngles ( cell  frame,
matrix  R,
matrix  p 
)

Returns the symbolic representation of the Euler angles of a rigid link.

Note
Syntax for ont point

getEulerAngles(obj,pf,offset)

Note
Syntax for multiple points (offset should be np*3 matrix)

getEulerAngles(obj,pfarray, offset)

Parameters
framethe list of coordinate frame of the point
Rang is computed as the relative Euler angles from frame to R
pthe offset of the point from the origin of the frame
Return values
angthe 3-dimensional Euler angles (roll,pitch,yaw) vector of the CoM of the system

◆ getSpatialJacobian()

function SymExpression J = RobotLinks.getSpatialJacobian ( cell  frame,
matrix  p 
)

Returns the symbolic representation of the spatial jacobian of the point that is rigidly attached to the link with a given offset.

Note
Syntax for ont point

getSpatialJacobian(obj,pf,offset)

Note
Syntax for multiple points (offset should be np*3 matrix)

getSpatialJacobian(obj,pfarray, offset)

Parameters
framethe list of coordinate frame of the point
pthe offset of the point from the origin of the frame
Return values
Jthe 6xnDof Jacobian matrix of a rigid point

◆ getTotalMass()

function double mass = RobotLinks.getTotalMass ( )

Return the total mass of the robot model.

Return values
massthe total mass of the robot

◆ loadDynamics()

function obj = RobotLinks.loadDynamics ( char  file_path,
logical  skip_load_vf,
cellstr  extra_fvecs 
)

Load the symbolic expression of dynamical equations from a previously save MX files.

Parameters
file_paththe path to export the file
skip_load_vfskip loading the drift vectors
extra_fvecslist of extra drift vectors

◆ removeContact()

function obj = RobotLinks.removeContact ( cellstr  contact)

Remove contact constraints defined for the system.

Parameters
contactthe name of the contact
Required fields of contact:

Member Data Documentation

◆ ConfigFile

RobotLinks.ConfigFile

The full path of the model configuration file.

Note
This property has non-standard access specifiers: SetAccess = Protected, GetAccess = Public
Matlab documentation of property attributes.

◆ Joints

RobotLinks.Joints

An array of rigid joints.

Note
This property has non-standard access specifiers: SetAccess = Protected, GetAccess = Public
Matlab documentation of property attributes.

◆ Links

RobotLinks.Links

An array of rigid links.

Note
This property has non-standard access specifiers: SetAccess = Protected, GetAccess = Public
Matlab documentation of property attributes.

The documentation for this class was generated from the following files:
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/RobotLinks.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/addContact.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/addFixedJoint.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/configureActuator.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/configureDynamics.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/configureKinematics.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/findBaseLink.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/findEndEffector.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getBodyJacobian.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getCartesianPosition.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getComPosition.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getEulerAngles.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getJointIndices.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getLimits.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getLinkIndices.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getRelativeEulerAngles.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getSpatialJacobian.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/getTotalMass.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/loadDynamics.m
  • /home/ayonga/.dropboxes/business/Dropbox/research/dzopt/frost/matlab/robotics/@RobotLinks/removeContact.m