Reference

1. Introduction

robot can be described as a single body or a set of bodies with different forces applied. a robot arm moving in free space is driven by actuator forces on joints, a legged robot is additionally driven by contact forces on feet and a flying vehicle is driven by aerodynamic forces

category of robot:

a list of math notations

2. Kinematics

description of the motion of points, bodies, and systems of bodies, only consider how to move but not why

2.1. Position, rotation, and velocity

for a moving point, need a position vector and its derivatives; for a rigid body, a rotation is additionally needed

to numerically express the components of the position vector, need to define a reference frame with an orho-normal basis

so position vector is given by

for two frame with same origin point O, and a point P in space, the position vector can be expressed in two frame:

basis vectors of frame can be expressed in frame by

then the relationship between two position vector is

where is rotation matrix and columns of it are orthogonal unit vector, so rotation matrix is orthogonal and belongs to special (comes from the det of matrix is 1 not −1) orthonormal group

rotations have two different interpretations:

composition of rotations is given by

representation of rotation use parameter vector

ZXZ Euler angles (propery Euler angles)

ZYX Euler angles (Tait-Bryan angles, yaw-pitch-roll)

XYZ Euler angles (Cardan angles)

angular velocity is the limit of rotational motion of w.r.t :

relationship between angular velocity and rotation matrix is defined by

angular velocity in different frame is expressed in

the relationship between time derivatives of rotation parameterizations and angular velocity is given by [1] 2.5.1, used to update rotation parameter (orientation) with angular

2.2. Velocity in a moving body

let be the inertial fixed frame, is the frame fixed on a moving body, we have:

the time derivative of a position vector expressed in a frame is not always equal to the time derivative of coordinates of a position in a frame , in other word,

unless frame is an inertial frame

position of point P is written as

differentiating w.r.t time results in

point P is on body, the position vector expressed in frame is constant, frome the relationship between angular velocity and rotation matrix, , yielding

which is rigid body velocity formulation, for a point P on rigid body B, the absolute velocity is

for acceleration, result is

vector differentiation in moving frame is given by Euler differentiation rule, to get absolute velocity of P in frame from position vector in frame , we must transform into frame , then differentiating it, then transform back to frame

2.3. Kinematics of system of bodies

robotic system can be model as open kinematic structure composed of links connected by joints (prismatic or revolute, one dof), two connected bodies related by a transformation

for fixed base system, frame attached to root link is world fixed (inertial) frame

generalized coordinate vector is used to describe configuration of robot:

in most cases, each generalized coordinate corresponds to a dof, without additional kinematic constriants, minimal set of generalized coordinates is called minimal coordinates, the choice of generalized coordinates is not unique

configuration of end-effector is described by relative position and orientation w.r.t a reference frame (inertial or root frame)

end-effector operates in operational space which depends on geometry and structure of robot, is described by

which are independent coordinates, is a minimal selection of end-effector configuration and

forward kinematics: mapping between generalized joint coordinates and end-effector configuration

for a serial linkage system

for fixed base robot, first frame of link 0 is not moving w.r.t inertial frame and end-effector is rigidly connected to last body, then and are constant transformation

differential kinematics is given by linearization of forward kinematics:

is a analytical Jacobian matrix, represents an approximation in context of finite differences and exact relation between generalized velocity and time-derivatives of end-effector configuration parameters:

analytical Jacobian is divided into position and rotation Jacobian

geometric Jacobian is a unique Jacobian than relates generalized velocity and velocity of end-effector

the velocity of link k is given by

where is origin of frame , for numerical addition, all vectors are expressed in same coordinate system

denoting base (inertial) frame as 0 and end-effector frame as , origin of frame (is the body frame of link k) is at joint k and the end-effector velocity is given by

frame of link k is aligned with joint k, then

which results in

then geometric Jacobian is given by

we need to define this w.r.t a basis like , then

the rotation axis is given by

analytical Jacobian matrix and geometric Jacobian matrix is related by

2.4. Kinematic Control Methods

inverse differential Kinematics

geometric Jacobian performs a mapping from joint space velocity to end-effector velocity, but in many cases, given a desired end-effector velocity, how to get corresponding joint velocity, one way is take the inverse or pseudo-inverse of Jacobian

there are many kinds of pseudo-inverse methods and solution is not unique

when a configuration that the rank of Jacobian is smaller than the number of operational space coordinates (number of controllable end-effector DOFs), the configuration is called singular. In a singular configuration, for a desired end-effector velocity , there may not exist a corresponding generalized velocity , by taking Moore-Penrose pseudo inverse, the solution of minimizes the least square error . However, small desired velocity in singular direction will lead to extremely high joint velocity, which is problematic for inverse kinematics algorithms

to deal with the bad condition of singular Jacobian, a common approach is to use damped version of Moore-Penrose pseudo-inverse

the larger damping parameter , the more stable the solution, but the slower the convergence

when a configuration that the rank of Jacobian is smaller than the number of joints , the configuration is called redundant. the Moore-Penrose pseudo-inverse

minimizes while , then redundancy means there are infinite solutions

where is null-space projection matrix of fulfilling , the generalized velocity could be modified by any choice of without changing end-effector velocity

to determine the null-space projection matrix, the simplest way is given by

inverse kinematics is to find a joint configuration as a function of a given end-effector configuration

analytical solutions

numerical solution: the differences in joint space coordinates can be mapped to differences in end-effector coordinates by analytical Jacobian

given a start configuration and a desired end-effector configuration , the inverse kinematics problem can be solved iteratively

trajectory control: to follow a predefined task-space trajectory, pure inverse differential kinematics is insufficient, a weighted tracking error (pose error) feedback need to be added

for position tracking with a given predefined position and velocity , the trajectory controller is

for orientation tracking with predefined orientations and angular velocity , selection of parameterization leads to different trajectories, best approach is to follow the shortest rotation approach

2.5. Floating base kinematics

floating-based robot is described by unactuated base coordinates and actuated joint coordinates , the unactuated base is free in translation and rotation

generalized velocity and acceleration vector (not time-derivatives of parameterization) is

the position vector of a point Q which is fixed at the end of kinematic chain, w.r.t inertial frame is given by

time differentiation of position vector gives

then the mapping from generalized velocity to operational space twist of frame is given by spatial Jacobian

contact can be modeled as kinematic constriants, every contact point imposes three constriants

the velocity and acceleration constriants can be expressed as a function of generalized velocity and acceleration using contact point Jacobian (position part of spatial Jacobian)

for active contacts, contact Jacobians are stacked to

stacked Jacobian can be split into

where is the relation between base motion and contact constriants, if the rank of it is full (6), base motion can be controlled from joint motion

for point contacts of quadruped, each point point feet imposes three independent constriants. In case of two point contacts, the rank of stacked contact Jacobian is 6 but the base part is only 5 (robot cannot change orientation around the line of support). In case of three point contacts, rank of stacked contact Jacobian is 9 and the base part is 6, which means base position and orientation is fully controllable by joints and there are three internal constriants

for extended feet of humanoid, additional constriants are required to limit foot rotation. a common way is to assign multiple contact points on same link

applying inverse kinematics to floating base system is to achieve task-space motion without violating contact constriants. a multi-task approach with priorization where contact constriants are considered to have higher priority

3. Dynamic

3.1. Classical Mechanics

Newton’s second law to a particle with (infinitesimal) mass

variation of a quantity describes: for a fixed instant in time, all possible (admissible) directions the quantity may move while adhering to constriants

variation of a position vector is called virtual displacement

for a single body, position, velocity, acceleration vector of a point mass on body is given by

where is absolute position of body point S, is relative position of w.r.t S and and are absolute velocities and accelerations of point S.

motion is described using generalized coordinates

virtual displacement of body element is expressed as

where is the variation of infinitesimal rotation of local body-fixed frame defined at point S, the variation is not taken w.r.t an orientation quantity

for a multi-body system, the motion is described using generalized coordinates , virtual displacements are

principle of virtual work + d’Alambert’s principle

3.2. Newton-Euler method

use principle of virtual work to a single body, define as body mass, as S is center of geometry, is inertia matrix/tensor around S, we get

definitions about linear and angular momentum and their changes are

which results in Newton-Euler equation

for numerical calculation, the terms of changes in momentum must be expressed in same coordinate system, and for inertia tensor,

to use Newton-Euler method for multi-body system, an approach is to separate all bodies at joints and consider every body as a single body, then apply constriant forces as external forces, in a general 3D and fixed base case, there joints and free bodies, so a system consists of dofs and motion constriants

another approach is to describe motion as a function of generalized coordinates

3.3. Lagrange method

three concepts:

kinetic energy of a system is defined as

velocities of each body is function of generalized velocities given by geometric Jacobian

then kinetic energy is expressed in generalized coordinates by

potential energy is from gravity and elastic energy

some constriants are defined by velocities are motion constriants, which are expressed as linear combinations of general velocities

for motion constriants, motion constriant Jacobian matrix is

some constriants are defined as function of configuration are configuration constriants,

constrianed Euler-Lagrange (CEL) equation (a.k.a Euler-Lagrange of the first kind) is given by:

where is vector of Lagrangian multipliers for motion constriants, is vector of Lagrangian multipliers for configuration constriants

3.4. Projected Newton-Euler method

a combination of classical Newton-Euler equation for dynamic equilibrium in Cartesian coordinates and constriant compliant Lagrange formulation using generalized coordinates

change of linear and angular momentum can be rewritten using generalized coordinates

virtual work principle can be rewritten using generalized coordinates

combining above two equations yields

some terms are defined and computed as

there are external forces and external torques applied on system, based on virtual work, we have

there are actuator forces / torques applied on joint k, which are imposed on two bodies equally and in opposite directions

total generalized forces is combination of joint actuators and body links:

with contact forces, resulting equations of motion is

a multi-body dynamics formulation for fixed-based robots is

where is generalized mass matrix, are generalized position, velocity and acceleration vectors, is Coriolis and centrifugal force terms, is gravitational terms, is external generalized forces (applied on bodies and joints), is external Cartesian forces (from contacts), is geometric Jacobian corresponding to external forces

3.5. Dynamics of floating base system

a multi-body dynamics formulation for floating-based robots is

where is generalized coordinates consists of actuated joint coordinates and unactuated base coordinates , corresponding velocities are and . selection matrix selects joint velocity by

to control unactuated base coordinates, external forces are necessary, they can come from contacts with environment (e.g. legged robots) or from aerodynamics (e.g. flying robots)

for legged robots, equation of motion is written as

where is force that robot exerts on environment

there two different methods to model contact force:

where equation of motion is (90)

substituting contact force (90) into equation of motion (90) results in

then dynamically consistent support null-space matrix/projector is defined as

impact occurs when bodies collide with each other in a very short duration and high peak forces that results in energy dissipation and large acceleration

ignore all kinds of forces except contact impuse force, the equation of motion is integrated over impulse duration, results in

for a perfect inelastic collision with a Newtonian collision law, all contact points instantaneously come to rest i.e. , then impulsive force is give by

where is the inertia that is seen at support so called end-effector inertia, the post-impact generalized velocity is given by

the pre-impact generalized velocity is projected onto support consistent manifold

the kinetic energy loss is give by

3.6. Joint-space dynamic control

for a torque controllable actuator, joint feedback control law is

when applying this control law to a robot arm (without contact),

there is a steady-state tracking error if

to compensate this steady-state offset, a common approach is

where is estimated gravity effects

inverse dynamics control is given by

where are estimates. applying control law for a exact model results in

a common approach to select desired acceleration is

which corresponds to a mass-spring-damper system with eigenfrequcy and damping value

3.7. Task-space dynamics control

in most cases, we want move end-effector in task-space i.e. in world-fixed frame, the generalized acceleration is given by geometric Jacobian

multi-task control is similar to kinematic multi-tasks control

end-effector dynamics is given by

relationship between joint torque and end-effector force is given by

then task-space equations of motion is

where the lhs consists of end-effector inertia, centrifugal/Coriolis, and gravitational terms

end-effector motion control is given by

where desired acceleration is

where is end-effector rotation error

when robot should apply force in some directions and move in other directions, we need operational space control

3.8. Inverse dynamics for floating-base system

given a desired acceleration, the (109) is inverted into

depending on structure of , there is a null-space to modify torque

there are many approaches to solve problem of simultaneously controlling different operational-space objectives, which involve motion at selected locations (e.g. end-effector, CoG), desired contact forces, or joint torques

a method is to take operational space control as sequential least square optimization problem of linear objectives. problems with same priority are stacked in and , is the highest priority, the hierarchical least square optimization of problems is to solve each task in a least square sense

without influencing task of higher priority, methods includes iterative null-space projection and solving a sequence of constrianed quadratic programs using standard numerical solvers