GravityTorques-3Links-2D

Geometry and joints torques of a 3-links system in a gravity field in 2D

Usage

• (Clone or) download the repository
• On your computer :
  • Open and run main.m with matlab
  • Open and run scilab/main.sce with scilab

Geometry

The graphical representation below shows a 3-links kinematic chain.

With more details, we decide that :

• Links are rigid bodies fully defined by :
  • L_i : Total length (meter)
  • c_i : location of the centre of mass from the root of the link (percentage)
  • m_i : mass (kilogram)
  • x_i y_i : absolute coordinates of the root of the link (real)
    
• Link orientation are defined by :
  • θ_i : relative to the previous link
    NOTE : this is the convention for a kinematic chain, where angles (θ_i) are counter clockwise from previous segment.
  • α_i : relative to the gravity field
    NOTE : α_i = θ₁ + ... + θ_i

Torque

We pedagogically describe the logic for a kinematic chain with 1 to N links.

For a single link:

To compute the torque for a single link, we define :

• W_i : gravitation force (newton)
  • W_i = g m_i
• D_i : lever arm of W_i (meter)
  • D_i = (L_i c_i) cos(α_i)
• α_i : angle relative to the gravity field (radian)
• T_i : torque due to gravity (newton meter)
  • T_i = D_i W_i = g m_i c_i cos(α_i)

For two links:

To compute the torque for two links, we have to take care that the kinematic chain has two axes of rotation, as illustrated in the figure below.

We define :

• J_i : joint (axis of rotation) of link_i.
  • In a kinematic chain, the joint J_i articulates link_i-1 to link_i
  • Joint_i is located at (x_i, y_i)
  • Joint₁ is the root of the kinematic chain
• T_Joint,Link : torque due to link L at joint J (newton meter)
  For example (see figure below):
  • T_1,2 : torque due to link 2 around the root joint
  • T_1,12 : torque due to links 1+2 around the root joint
  • T_2,2 : torque due to link 2 around joint 2

With these notations, we get :

• Torque around axis 1 (root):
  • T_1,1 = g m₁ D_1,1
    • with D_1,1= L₁ c₁ cos(α₁)
  • T_1,2 = g m₂ D_1,2,
    • with D_1,2 = L₂ c₂ cos(α₂) + L₁ cos(α₁)
  • T_0,12 = T_0,1 + T_0,2
• Torque around axis 2 (partial torque at (x₂ y₂)):
  • T_2,2 = g m₂ D_1,2
    • with D_2,2 = L₂ c₂ cos(α₂)

Lever arms D_Joint,Link and gravity forces W_Link are illustrated below.

For three links:

Generalisation from the previous simpler cases gives:

• Distances (lever arm of the weight):
  D_1,1 = L₁ c₁ cos(α₁)
  D_1,2 = L₂ c₂ cos(α₂) + L₁ cos(α₁)
  D_1,3 = L₃ c₃ cos(α₃) + L₂ cos(α₂) + L₁ cos(α₁)

• Torques :
  T_1,1 = g m₁ D_0,1
  T_1,2 = g m₂ D_0,2
  T_1,3 = g m₃ D_0,3

• Total torque :
  T_1,123 = T_0,1 + T_0,2 + T_0,3

• Partial torques:
  The torques of interest are that of the distal part of the kinematic chain, so to get the gravitational torque at each joint.

  • Last link acting on the last joint:
    T_3,3 = g m₃ D_2,3, with D_3,3 = L₃ c₃ cos(α₃)
  • Last two links acting on joint 1 (the root is joint 0):
    T_2,23 = T_2,2 + T_2,3
    T_2,23 = g m₂ D_2,2 + g m₃ D_2,3, with
    • D_2,2 = L₂ c₂ cos(α₂)
    • D_2,3 = L₂ cos(α₂) + L₃ c₃ cos(α₃)

The figure below illustrates such a 3 links system, showing the α_i and D_{Joint, Link}

For N links:

To compute the torque for N links, we generalize the previous by exploiting the regularities in the equations.

• Constraints on formulas (i.e., this is a kinematic chain)
  • Link must be distal relative to joint (L >= J) in:
    • T_Joint,Link
    • D_Joint,Link
• Regularities in lever arm formula:
  The lever arm of link L relative to joint J is the sum of lever arms along the corresponding part of the kinematic chain.
  D_Joint,Link = Σ D_J,i with i varying from J to L
  • Last link (most distal part of the kinematic chain): D_J,last = L_last c_last cos(α_last)
  • Other link (not distal part of the kinematic chain): D_J,i = L_i cos(α_i)
• Regularities in torque formula:
  • Invariance overs links and joints.
    • T_J,J...L = Σ T_J,i with i varying from J to L
    • T_J,i = g m_i D_J,i

Implementation in matlab:

Here, we will keep data structure as straightforward as possible, and we will not use object programming.

Data to describe the kinematic chain

Each link is totally described once we know : total length (meter), angle from horizontal (degrees, easier for humans eyes), position of the CoM (percent, from link root) and mass (kg). Below are plausible data values for a human body (profile view) :

%      trunk    arm   forearm
L = [  0.75,   0.35,    0.25  ]; % total length (meter)
a = [    90,    -70,       0  ]; % alpha from horizontal (degrees, easier for humans eyes)
d = [  0.45,   0.45,    0.55  ]; % position of CoM (percent)
m = [    20,    1.1,     0.9  ]; % mass (kg)

alpha = a .* pi ./ 180;          % alpha in radian (for computations)

This allows for calls such as:
D1_3 = L(3) .* c(3) .* cos( alpha(3) ) + L(1:2) .* alpha(1:2) ;
to implement
D_1,3 = L₃ c₃ cos(α₃) + L₂ cos(α₂) + L₁ cos(α₁)

Relations between angles in the kinematic chain

Angles θ and α have relations that we can exploit. By definition:

• θ is the relative orientation of one segment (relative to the previous)
• α is the absolute orientation of one segment (relative to horizontal)

Hence:

• θ is the difference in α (change/derivative of α)
• α is the sum of all previous θ (sum/integration of θ)

% Relations between angles: 
theta = [alpha(1) diff(alpha)];  % difference in alpha 
alpha = cumsum(theta);           % sum of previous theta

Cartesian coordinates of the links

Once defined the coordinate of the root of the kinematic chain, the distal coordinate of each segment is given by L cos(α).

xEndLink = cumsum(L .* cos(alpha));  % end of all links 
yEndLink = cumsum(L .* sin(alpha));	
xEndEffector = xEndLink(end);        % end effector position
yEndEffector = yEndLink(end); 
x = xEndLink(1:end-1);               % end of previous link = beg of current link
y = yEndLink(1:end-1); 
x = [0, x];                          % add first link
y = [0, y]; 
x = xRoot + x;                       % shift all by Root coordinate
y = yRoot + y; 

Lever arm computation

The lever arm from joint j to the CoM of link l is given by the sum of :

• the distance from the axis of rotation to the root of the link : Σ L cos(α)
• the distance from the root of the link to the CoM : L c cos(α)

ShiftLnk = L .* cos(alpha); 
ShiftCoM = L .* c .* cos(alpha); 
LA = sum(ShiftLnk(j:l-1)) + ShiftCoM(l);

Torque computation

The torque due to gravity at joint is the sum of the torques due to the distal links.

for j = 1:NbLinks                                 % for each joint... 	
    T(j) = 0;                                     % init torque at this joint
    for l = j:NbLinks                             % for the distal links...		
    	LA = sum(ShiftLnk(j:l-1)) + ShiftCoM(l);  % lever arm (of each link)
    	Tl = LA .* g .* m(l);                     % torque (of each link)
    	T(j) = T(j) + Tl;                         % sum torques (of each link)
    end
end 

Implementation logic

To ease the manipulation of the previous calculations, we organize things a bit. We define a structure that stores all data needed to describe the "posture" of the system. As we do not use object programming, we create functions to manipulate this data

• Posture related functions:

  • To create the posture (from the minimal information):
    P = Posture_set(L, a, c, m, xRoot, yRoot)

  • To conpute the gravity torques (knowing the configuration of the links):
    P = Posture_setGravityTorques(P)

  • To move the posture to a novel configuration (with a novel theta1):
    P = Posture_moveTheta1(P, theta1)

  • To display the posture
    Posture_plot(P)

  • To compute the torques
    P = Posture_setGravityTorques(P)

• Definition of the kinematic chain
  We manually specify the minimal information necessary to display the chain and compute gravitational torques.

	% define root position 
	xRoot = 10;  yRoot = 0;           
	% define links 
	%      first  second    third
	L = [  0.75,   0.35,    0.35   ]; % length in meter
	a = [    90,    -70,       0   ]; % angle from horizontal in degrees
	c = [  0.45,   0.45,    0.55   ]; % position of COM in percent
	m = [    20,    1.1,     0.9   ]; % mass in kg

• Initialisation of the posture struct
  From the minimal definition, we compute the coordinates of the links and the torques, and we store that into a "posture" struct.

	P = Posture_set(L, a, c, m, xRoot, yRoot)
		% L : length of links (meter) 
		% a : angle of links (degree) 
		% m : mass of links (kilogram) 
		% c : distance of center of mass (percentage)
		% xRoot, yRoot : coordinates of the root of the chain
	
	P = 
	    Length: [0.7500 0.3500 0.4500]
	       CoM: [0.4500 0.4500 0.5500]
	      Mass: [20 1.1000 0.9000]
	         x: [10 10 10.1197 10.5697]
	         y: [0 0.7500 0.4211 0.4211]
	     theta: [1.5708 -2.7925 1.2217]
	    nLinks: 3
	    Torque: [3.8234 3.8234 2.1852]

• Modification of the first link orientation
  Without changing the end effector position, we can change the orientation of the first link .

	P = Posture_moveTheta1(P, theta1)
		% P : a posture struct (as provided by Posture_set) 
		% theta1 : angle of the first link (degree) 

• Display of a posture struct
  Accurate plot of a posture is necessary for visualisation

	Posture_plot(P)
		% P : a posture struct (as provided by Posture_set) 

Todo

Adding a mass to the endpoint:

This might be done by adding a 4th link, following the very same logic.
This link 4 would have non-zero mass, but zero for other dimensions:
L₄ = 0, α₄ = 0, c₄ = 0 but m₄ is non-zero.

Target position of end effector

End-effector (endpoint) position is fixed at the target position, but the user can interactively modify the angle of the first link.

• A clic (or a slider) can change α₁
  • the geometric constraints are pictured
  • α₁ should stay in a range allowing the reach