CYBERTRONICS

Advanced Cybernetics Solutions

sym2code knowledge base

Sym2Code Example

Check the control design via simulation before to deploy the final code.

Control Law Design

Let us consider the system depicted in picture.
The position of the three points of interest can be easily found as:
pa=(ϵ0) p_a = \begin{pmatrix} \epsilon \\ 0\end{pmatrix}
pb=pa+Rα(l10),Rα=(cosαsinαsinαcosα) p_b = p_a + R_\alpha \begin{pmatrix} l_1 \\ 0\end{pmatrix}, \qquad R_\alpha = \begin{pmatrix} \cos \alpha & -\sin \alpha \\ \sin \alpha & \cos \alpha \end{pmatrix}
pc=pb+RαRβ(l20),Rβ=(cosβsinβsinβcosβ) p_c = p_b + R_\alpha R_\beta \begin{pmatrix} l_2 \\ 0\end{pmatrix}, \qquad R_\beta = \begin{pmatrix} \cos \beta & -\sin \beta \\ \sin \beta & \cos \beta \end{pmatrix}
Considering the joint variables:
q=(ϵ,α,β) q = \begin{pmatrix} \epsilon, & \alpha, & \beta \end{pmatrix}^\top
we can formulate the control problem of finding the control law to reduce asymptotically the error:
p~=pdpc(q)\tilde{p} = p_d – p_c(q)
between a fixed position pdR2p_d \in \mathbb{R}^2 such that p˙d=0\dot{p}_d = 0 and the result of the direct kinematics represented by the function pc(q):R3R2p_c(q): \mathbb{R}^3 \rightarrow \mathbb{R}^2.
Let us consider a Lyapunov function:
V(p~)=12p~p~0p~0V(\tilde{p}) = \frac{1}{2} \tilde{p}^\top \tilde{p} \succ 0 \quad \forall \quad \tilde{p} \neq 0
computing the time derivative we obtain:
V˙(p~)=p~(J(q)q˙),J(q)=pc(q)q \dot{V}(\tilde{p}) = \tilde{p}^\top (-J(q)\dot{q}), \qquad J(q) =\frac{\partial p_c(q)}{\partial q}
where J(q)J(q) is the analytical Jacobian.
As the choice of the control action should enforce negative definiteness for the Lyapunov function time derivative, a possible choice can be immediately identified by: q˙=KJ(q)(pdpc(q)) \dot{q} = K J^\top(q) \left( p_d – p_c(q) \right)
It is well known that kinematics singularities might affect the control results as the matrix J(q)KJ(q)J(q) K J^\top(q) is responsible of the quadratic form: V˙(p~)=p~J(q)KJ(q)p~0 \dot{V}(\tilde{p}) = -\tilde{p}^\top J(q) K J^\top(q) \tilde{p} \prec 0
A good indicator of the singularity proximity is given by the manipulability computed as: μ=det(J(q)J(q)) \mu =det(J(q)J^\top(q))

Let us now convert our math results into runnable code with Sym2Code

Code Generation

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
% Joint variables
q = SymVectorCol('q',{'epsilon','alpha','beta'}, 'Joint State');
 
% Arms length
L = SymVectorCol('l',1:2, 'Link Length');
 
% Rotation matrices
R_alpha = [cos(q(2)), -sin(q(2)); sin(q(2)), cos(q(2))];
R_beta = [cos(q(3)), -sin(q(3)); sin(q(3)), cos(q(3))];
 
% Direct kinematics of the points of interest
p_a = Sym('p_a',[q(1);0],'Position A');
p_b = Sym('p_b',p_a + R_alpha*[L(1);0],'Position B');
p_c = Sym('p_c',simplify(p_b + R_alpha*R_beta*[L(2);0]),'Position C');
 
Sym2Code({p_a, p_b, p_c}, 'PRRKine', {q, L});
 
% Lyapunov control law
p_d = SymVectorCol('p_d',{'x','y'}, 'Desired Position');
K = SymVectorCol('K',{'x','alpha', 'beta'}, 'Gain');
J = Sym('J', simplify(jacobian(p_c,q)), 'Analytic Jacobian');
q_dot_c = Sym('u', simplify(diag(K)*J'*(p_d-p_c)), 'Kinematic Control');
 
Sym2Code({q_dot_c, J}, 'PRRCtrl', {q, L, p_d, K});

Resulting Code

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
function [p_a,p_b,p_c, bError] = PRRKine(q,l)
%-----------------------------------------------------------------------------
% This code has been automatically generated using Sym2Code Toolbox
% Sym2Code M Target
%-----------------------------------------------------------------------------
% WARNING: Modifying the following code is not recommended.
%-----------------------------------------------------------------------------
% For more information:
% - visit our webpage www.cybertronics.cloud
% - write an email to sym2code@cybertronics.cloud
%-----------------------------------------------------------------------------
% Inputs:
% q is a 3x1: Joint State */
% l is a 2x1: Link Length */
% Output:
% p_a is a 2x1: Position A */
% p_b is a 2x1: Position B */
% p_c is a 2x1: Position C */
%-----------------------------------------------------------------------------
 
% The module is named PRRKine and has 2 inputs and 3 outputs
 
p_a = zeros(2,1); % Output n. 1 is: Position A
p_b = zeros(2,1); % Output n. 2 is: Position B
p_c = zeros(2,1); % Output n. 3 is: Position C
bError = true;
 
XTmp = zeros(1,12); % Optimal Transition Variables
 
% Check input data
 
% Input n. 1 is: Joint State
if(isempty(q)), return; end
if(size(q,1)~=3), return; end
if(size(q,2)~=1), return; end
if(~isempty(q(isnan(q)==1))), return; end
 
% Input n. 2 is: Link Length
if(isempty(l)), return; end
if(size(l,1)~=2), return; end
if(size(l,2)~=1), return; end
if(~isempty(l(isnan(l)==1))), return; end
 
% Perform main calculation
 
% Computing cos(q_alpha) as
XTmp(1) = cos(q(2,1));
% Computing l_1*cos(q_alpha) as
XTmp(2) = (XTmp(1)*l(1,1));
% Computing q_epsilon + l_1*cos(q_alpha) as
XTmp(3) = (XTmp(2)+q(1,1));
% Computing sin(q_alpha) as
XTmp(4) = sin(q(2,1));
% Computing l_1*sin(q_alpha) as
XTmp(5) = (XTmp(4)*l(1,1));
% Computing q_alpha + q_beta as
XTmp(6) = (q(2,1)+q(3,1));
% Computing cos(q_alpha + q_beta) as
XTmp(7) = cos(XTmp(6));
% Computing l_2*cos(q_alpha + q_beta) as
XTmp(8) = (XTmp(7)*l(2,1));
% Computing q_epsilon + l_2*cos(q_alpha + q_beta) + l_1*cos(q_alpha) as
XTmp(9) = (XTmp(3)+XTmp(8));
% Computing sin(q_alpha + q_beta) as
XTmp(10) = sin(XTmp(6));
% Computing l_2*sin(q_alpha + q_beta) as
XTmp(11) = (XTmp(10)*l(2,1));
% Computing l_2*sin(q_alpha + q_beta) + l_1*sin(q_alpha) as
XTmp(12) = (XTmp(5)+XTmp(11));
 
% Generating optimal output for p_a which is p_a with comment: Position A
% as q_epsilon
p_a(1,1) = q(1,1);
% as 0
p_a(2,1) = 0.000000e+00;
 
% Generating optimal output for p_b which is p_b with comment: Position B
% as q_epsilon + l_1*cos(q_alpha)
p_b(1,1) = XTmp(3);
% as l_1*sin(q_alpha)
p_b(2,1) = XTmp(5);
 
% Generating optimal output for p_c which is p_c with comment: Position C
% as q_epsilon + l_2*cos(q_alpha + q_beta) + l_1*cos(q_alpha)
p_c(1,1) = XTmp(9);
% as l_2*sin(q_alpha + q_beta) + l_1*sin(q_alpha)
p_c(2,1) = XTmp(12);
 
% Return a valid state
bError = false;
end

Simulating System

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
% Time and number of samples
Ts  = 0.01;
Tf  = 5.0;
t   = 0:Ts:Tf;
N   = numel(t);
 
% Joint state, manipulability index and points of interest
q   = zeros(3,N);
mu  = zeros(1,N);
p_a = zeros(2,N);
p_b = zeros(2,N);
p_c = zeros(2,N);
 
% Initial condition: vertical position
q(:,1) = [0.0; pi/2; 0];
 
% Arms length
L      = [0.5; 0.25];
 
% Gain and desired position
K      = [0.4; 1.0; 5.0];
p_d    = [1;0.5];
 
% Simulate kinematics control
figure(1);
subplot(1,2,1)
plot(p_d(1),p_d(2), 'ok', 'LineWidth',4), hold on;
for k=1:N
     
    % Compute the location of the points of interest
    [p_a(:,k), p_b(:,k), p_c(:,k)] = PRRKine(q(:,k), L);
     
    % Compute control action
    [q_dot_c, J] = PRRCtrl(q(:,k), L, p_d, K);
     
    % Evaluate the manipulability index
    mu(k) = det(J*J');
     
    % Plot trajectory: red to blue over time
    if k==1 || mod(k,floor(N/4))==0
        Color = [1;0;0]*(N-k)/N + [0;0;1]*(k/N);
        % Plot the three points
        plot(p_a(1,k),p_a(2,k),'o','Color',Color), hold on;
        plot(p_b(1,k),p_b(2,k),'o','Color',Color)
        plot(p_c(1,k),p_c(2,k),'^','Color',Color)
         
        % Plot the two links
        plot([p_a(1,k),p_b(1,k)],...
            [p_a(2,k),p_b(2,k)],'-', 'LineWidth',2,'Color',Color);
        plot([p_b(1,k),p_c(1,k)],...
            [p_b(2,k),p_c(2,k)],'-', 'LineWidth',2,'Color',Color);
    end
     
    % Integrate with forward Euler
    q(:,k+1) = q(:,k) + Ts*q_dot_c;
end
grid on, hold off;
xlim([0,1]); ylim([0,1])
xlabel('x [m]'), ylabel('y [m]');
 
% Plot manipulability index over time
subplot(1,2,2)
plot(t, mu, 'k', 'LineWidth',2);
grid on; hold off;
xlim([0,Tf]),
xlabel('Time [s]'), ylabel('\mu')

Get in Touch

By continuing to use the site, you agree to the use of cookies. more information

The cookie settings on this website are set to "allow cookies" to give you the best browsing experience possible. If you continue to use this website without changing your cookie settings or you click "Accept" below then you are consenting to this.

Close