Solve equilibrium equations in MATLAB for a quadruped robot

Question

idan 2025-3-2

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2174680-solve-equilibrium-equations-in-matlab-for-a-quadruped-robot

评论： Paul 2025-3-29

采纳的回答： idan

untitled333.m

在 MATLAB Online 中打开

Hi,

I'm trying to solve a equilibrium equations for quadroped robot,

I assum having all leg's tips location, the weight and the COM location in 3D space and the forces F1 and F2 as given input.

I have writed the 6 sets of equations (linear equations) and when trying to solve it with the solver it says that the rank of A is smaller then 6 (5). I have make sure all tips are not symmetric or geometrically (to not loosing possible answers)

how can I solve this problem?

In the script there is example of values I have tried to solve with

adding the code and photo :)

% Known forces and weight

F1 = [1; 1; 1];

F2 = [2; 3; 5];

W = [1; 1; 10];

x1=50;

y1=31.65;

z1=16.09;

x2=44.68;

y2=-27.26;

z2=-24.34;

x3=-4.72;

y3=-24.4;

z3=-27.21;

x4=0;

y4=25;

z4=-21.65;

xCOM=2.53;

yCOM=1.1;

zCOM=-4.47;

% Position vectors relative to leg 2

r1 = [x1 - x2; y1 - y2; z1 - z2];

r3 = [x3 - x2; y3 - y2; z3 - z2];

r4 = [x4 - x2; y4 - y2; z4 - z2];

rCOM = [xCOM - x2; yCOM - y2; zCOM - z2];

% Unknown forces

syms F3x F3y F3z F4x F4y F4z

F3 = [F3x; F3y; F3z];

F4 = [F4x; F4y; F4z];

% Force equilibrium

eq1 = F1 + F2 + F3 + F4 + W == 0

eq1 =

% Moment equilibrium

M1 = cross(r1, F1);

M3 = cross(r3, F3);

M4 = cross(r4, F4);

MCOM = cross(rCOM, W);

eq2 = M1 + M3 + M4 + MCOM == 0

eq2 =

%eq2 = cross(r1, F1) + cross(r3, F3) + cross(r4, F4) + cross(rCOM, W) == 0;

[A, B] = equationsToMatrix([eq1; eq2], [F3x, F3y, F3z, F4x, F4y, F4z]);

rank_A = rank(A);

null(A)

ans =

rank([A,B])

ans = 6

disp(A);

disp(rank_A);

5

% Solve for F3 and F4

solutions = vpasolve([eq1; eq2], [F3x, F3y, F3z, F4x, F4y, F4z]);

disp(solutions)

F3x: [0x1 sym] F3y: [0x1 sym] F3z: [0x1 sym] F4x: [0x1 sym] F4y: [0x1 sym] F4z: [0x1 sym]

2 个评论
显示无隐藏无

idan 2025-3-4

移动：Sam Chak 2025-3-4

Another option that I was thinking about is to trying to solve with some kind of assessment and check for a minimum force and moments normal (the overall error), then set the value of f3 and f4 as the value of initial starting point in fsolve.

What do you think about it?

William Rose 2025-3-4

@idan,

What do you mean by "some kind of assessment"? You suggest that we "check for a minimum force and moments normal (the overall error)". What is normal to what? How is "the overall error" defined? If you give fsolve a set of inconsistent equations, it will not find a solution.

You may have a very good idea. I just don't understand it.

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

idan 2025-3-29

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2174680-solve-equilibrium-equations-in-matlab-for-a-quadruped-robot#answer_1562804

Hi,

So the main mistake that I had is when I tried to set by randomize F1 inside the friction cone. the solution for this problem has 3 DOF and by setting F1 in a space which is not linear wuth the solution from this type:

So when I set another 3d vector of F1 as unkown the solution brings the coefficient C1,C2 and C3.

*The forces has inner forces that cancles each other and thats the reason I can't determinate one force in the cone of friction regardeless the this assumption

Now all I have to do is to check if for any combination of those coefficient there is a particular solution for the problem.

adding the script of the solution for those who wants to run it

clc; clear; close all;

% Known forces and weight

F2 = [4; 5; 1]; % User's input force

W = [0; 0; -10]; % Increased weight for better force balance

% Positions of leg tips and center of mass (ensuring asymmetry)

x1 = 0; y1 = 0; z1 = 0; %F1 - Unkown force

x2 = 1; y2 = 0; z2 = 0; %F2 - user's input force

x3 = 0; y3 = 1; z3 = 0; %F3 - unkown force

x4 = 0.85; y4 = 0.9; z4 = 0.15; %F4 - unkown force

xCOM = 0.2; yCOM = 0.2; zCOM = 0.5; %COM location

% Position vectors relative to leg 2 - the users input force

r1 = [x1 - x2; y1 - y2; z1 - z2];

disp("r1= ")

disp(r1)

r2 = [x2 - x2; y2 - y2; z2 - z2];

disp("r2= ")

disp(r2)

r3 = [x3 - x2; y3 - y2; z3 - z2];

disp("r3= ")

disp(r3)

r4 = [x4 - x2; y4 - y2; z4 - z2];

disp("r4= ")

disp(r4)

rCOM = [xCOM - x2; yCOM - y2; zCOM - z2];

disp("rCOM= ")

disp(rCOM)

% Define unknown forces

syms F1x F1y F1z F3x F3y F3z F4x F4y F4z

F1 = [F1x; F1y; F1z];

F3 = [F3x; F3y; F3z];

F4 = [F4x; F4y; F4z];

% Force equilibrium equation (sum of forces must be zero)

eq1 = F1(1) + F2(1) + F3(1) + F4(1) + W(1) == 0; %Fx

disp("eq1= ")

disp(eq1)

eq2 = F1(2) + F2(2) + F3(2) + F4(2) + W(2) == 0; %Fy

disp("eq2= ")

disp(eq2)

eq3 = F1(3) + F2(3) + F3(3) + F4(3) + W(3) == 0; %Fz

disp("eq3= ")

disp(eq3)

% Moment equilibrium equation (sum of moments must be zero)

M1 = cross(r1, F1);

disp("M1= ")

disp(M1)

M2 = cross(r2, F2);

disp("M2= ")

disp(M2)

M3 = cross(r3, F3);

disp("M3= ")

disp(M3)

M4 = cross(r4, F4);

disp("M4= ")

disp(M4)

MCOM = cross(rCOM, W);

disp("MCOM= ")

disp(MCOM)

eq4 = M1 + M3 + M4 + MCOM == 0;

disp("eq4= ")

disp(eq4)

% Convert equations to matrix form A * X = B

[A, B] = equationsToMatrix([eq1; eq2; eq3; eq4], [F1x, F1y, F1z, F3x, F3y, F3z, F4x,F4y, F4z]);

% Display the system matrix and RHS vector

disp('Matrix A:');

disp(A);

disp('Vector B:');

disp(B);

disp("NULLL")

disp(null(A));

NULL=null(A);

% Check the rank of A

rank_A = rank(A);

disp('rank of A:');

disp(rank_A);

% Display the system matrix and RHS vector

disp('Matrix A:');

disp(A);

disp('Vector B:');

disp(B);

% Solve the system

if rank_A == size(A, 1) % Full rank, unique solution exists

X = linsolve(A, B);

disp('Xp Solution for F3 and F4 :');

disp(X);

disp('Xh Solution for F3 and F4 :');

disp("C1* ")

disp(NULL(:,1));

disp("C2* ")

disp(NULL(:,2));

disp("C3* ")

disp(NULL(:,3));

force_norm = norm(A * X - B); % ||Ax - B|| should be small if the solution is good

moment_norm = norm(cross(r1, X(1:3)) + cross(r3, X(4:6))+ cross(r4, X(7:9))+MCOM);

disp('Norm of force residuals:');

disp(round(force_norm, 5));

disp('Norm of moment residuals:');

disp(round(moment_norm, 5));

disp("Done")

end

4 个评论
显示 2更早的评论隐藏 2更早的评论

Paul 2025-3-29

在 MATLAB Online 中打开

Hi idan,

A few comments on this code to consider.

This code comment: " % Full rank, unique solution exists" is not correct. As you showed, the equations have an inifinite set of solutions.

The Xp solution includes F1, as well as F3 and F4.

The equation for force_norm doesn't look right. That equation, A*X - B, includes moment terms, does it not. Seems like force_norm should just be X(1:3) + X(4:6) + X(7:9) + F2 = 0;

Running the code

% Known forces and weight
F2 = [4; 5; 1];   % User's input force
W = [0; 0; -10];  % Increased weight for better force balance
% Positions of leg tips and center of mass (ensuring asymmetry)
x1 = 0;   y1 = 0;  z1 = 0; %F1 - Unkown force
x2 = 1;   y2 = 0; z2 = 0; %F2 - user's input force
x3 = 0;  y3 = 1; z3 = 0; %F3 - unkown force
x4 = 0.85;    y4 = 0.9;  z4 = 0.15; %F4 - unkown force
xCOM = 0.2;  yCOM = 0.2;  zCOM = 0.5; %COM location
% Position vectors relative to leg 2 - the users input force
r1 = [x1 - x2; y1 - y2; z1 - z2];
r2 = [x2 - x2; y2 - y2; z2 - z2];
r3 = [x3 - x2; y3 - y2; z3 - z2];
r4 = [x4 - x2; y4 - y2; z4 - z2];
rCOM = [xCOM - x2; yCOM - y2; zCOM - z2];
% Define unknown forces
syms F1x F1y F1z F3x F3y F3z F4x F4y F4z
F1 = [F1x; F1y; F1z];
F3 = [F3x; F3y; F3z];
F4 = [F4x; F4y; F4z];
% Force equilibrium equation (sum of forces must be zero)
eq1 = F1(1) + F2(1) + F3(1) + F4(1) + W(1) == 0; %Fx
eq2 = F1(2) + F2(2) + F3(2) + F4(2) + W(2) == 0; %Fy
eq3 = F1(3) + F2(3) + F3(3) + F4(3) + W(3) == 0; %Fz
% Moment equilibrium equation (sum of moments must be zero)
M1 = cross(r1, F1);
M2 = cross(r2, F2);
M3 = cross(r3, F3);
M4 = cross(r4, F4);
MCOM = cross(rCOM, W);
eq4 = M1 + M3 + M4 + MCOM == 0;
% Convert equations to matrix form A * X = B
[A, B] = equationsToMatrix([eq1; eq2; eq3; eq4], [F1x, F1y, F1z, F3x, F3y, F3z, F4x,F4y, F4z]);
% Display the system matrix and RHS vector
NULL=null(A);
% Check the rank of A
rank_A = rank(A);
% Solve the system
if rank_A == size(A, 1)  % Full rank, unique solution exists
    X = linsolve(A, B);
    %{
    disp('Xp Solution for F3 and F4 :');
    disp(X);
    disp('Xh Solution for F3 and F4 :');
    disp("C1* ")
    disp(NULL(:,1));
    disp("C2* ")
    disp(NULL(:,2));
    disp("C3* ")
    disp(NULL(:,3));
    force_norm = norm(A * X - B); % ||Ax - B|| should be small if the solution is good
    moment_norm = norm(cross(r1, X(1:3)) + cross(r3, X(4:6))+ cross(r4, X(7:9))+MCOM);
    disp('Norm of force residuals:');
    disp(round(force_norm, 5));
    disp('Norm of moment residuals:');
    disp(round(moment_norm, 5));
    disp("Done")
    %}
    
end
Warning: Solution is not unique because the system is rank-deficient.

It seems like the requirement is to find a particular solution for [F1;F3;F4] that's orthogonal to the null space of A. However, when the solution to A*X = b is not unique, which it is not for this problem, linsolve makes no guarantees about the solution it returns. In fact, this solution for X does not meet that requirement

C = NULL.'*X

C =

This "particular" solution does include components along basis vectors of the null space.

This solution, discussed in this comment

X1 = pinv(A)*B

X1 =

is orthogonal to the null space

NULL.'*X1

ans =

Keep in mind that symbolic null does not return an orthonormal basis

NULL.'*NULL

ans =

Switching to double null, which does return an orthonormal basis

NULL = null(double(A));
C = NULL.'*double(X);

We see that subtracting from X its components that live in the null space of A recovers the particular solution X1.

double([X1,(X - NULL*C)])
ans = 9×2
   -5.2463   -5.2463
   -0.2749   -0.2749
    7.5448    7.5448
    0.9570    0.9570
   -0.2749   -0.2749
    0.2277    0.2277
    0.2893    0.2893
   -4.4502   -4.4502
    1.2275    1.2275
<mw-icon class=""></mw-icon>
<mw-icon class=""></mw-icon>
norm(diff(ans,1,2))
ans = 4.1336e-15

idan 2025-3-29

编辑：Walter Roberson 2025-3-29

在 MATLAB Online 中打开

so does the next code fixs the all comment you have write?

thank you for your answer

% Define known forces and weight
F2 = [4; 5; 1];   % User's input force
W = [0; 0; -10];  % Increased weight for better force balance
% Define positions of leg tips and COM
x1 = 0;   y1 = 0;  z1 = 0; %F1 - Unknown force
x2 = 1;   y2 = 0;  z2 = 0; %F2 - User's input force
x3 = 0;   y3 = 1;  z3 = 0; %F3 - Unknown force
x4 = 0.85; y4 = 0.9; z4 = 0.15; %F4 - Unknown force
xCOM = 0.2; yCOM = 0.2; zCOM = 0.5; % COM location
% Compute position vectors relative to leg 2
r1 = [x1 - x2; y1 - y2; z1 - z2];
r2 = [x2 - x2; y2 - y2; z2 - z2];
r3 = [x3 - x2; y3 - y2; z3 - z2];
r4 = [x4 - x2; y4 - y2; z4 - z2];
rCOM = [xCOM - x2; yCOM - y2; zCOM - z2];
% Define unknown forces
syms F1x F1y F1z F3x F3y F3z F4x F4y F4z
F1 = [F1x; F1y; F1z];
F3 = [F3x; F3y; F3z];
F4 = [F4x; F4y; F4z];
% Force equilibrium equations
eq1 = F1(1) + F2(1) + F3(1) + F4(1) + W(1) == 0;
eq2 = F1(2) + F2(2) + F3(2) + F4(2) + W(2) == 0;
eq3 = F1(3) + F2(3) + F3(3) + F4(3) + W(3) == 0;
% Moment equilibrium equations
M1 = cross(r1, F1);
M3 = cross(r3, F3);
M4 = cross(r4, F4);
MCOM = cross(rCOM, W);
eq4 = M1 + M3 + M4 + MCOM == 0;
% Convert equations to matrix form A * X = B
[A, B] = equationsToMatrix([eq1; eq2; eq3; eq4], [F1x, F1y, F1z, F3x, F3y, F3z, F4x,F4y, F4z]);
% Compute null space
NULL = null(double(A));  % Ensures orthonormal basis
rank_A = rank(A);
% Solve system
if rank_A < size(A,2)  % Rank-deficient system, infinite solutions
    X1 = pinv(A) * double(B);  % Find a particular solution
    X = linsolve(A, B);  % MATLAB's default solution (not guaranteed to be null-space free)
    
    % Compute null space contribution
    C = NULL.' * double(X);
    X_corrected = X - NULL * C;  % Remove null space component
    
    % Check correction
    norm_diff = norm(double([X1, X_corrected] - X1), 'fro');
    
    % Compute force residual norm
    force_norm = norm(X(1:3) + X(4:6) + X(7:9) + F2);
    
    % Display results
    disp('Particular Solution X1:');
    disp(X1);
    
    disp('Null Space Contribution C:');
    disp(C);
    
    disp('Corrected Solution:');
    disp(X_corrected);
    
    disp('Norm of difference between corrected solution and X1:');
    disp(norm_diff);
    
    disp('Force Residual Norm:');
    disp(force_norm);
end

idan 2025-3-29

Have you edited somthing in yout answer?

Paul 2025-3-29

If you're asking me, I didn't edit anything. Looks like Walter Roberson edited you comment to use code formatting for the code. When in text mode, you can enter code formatting by clicking the leftmost icon in the Code section of the ribbon, or alt-enter (at least on Windows). When in a code block, click the leftmost icon on the Text section of the ribbon to go back to text mode, or alt-enter (at least on Windows).

请先登录，再进行评论。

Answer 2

William Rose 2025-3-2

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2174680-solve-equilibrium-equations-in-matlab-for-a-quadruped-robot#answer_1560946

@idan,

The same problem arises in a simple example: compute the weight on each wheel of an automobile. If you assume the car, including tires, is a rigid body, then the system is underdetermined, because there are three linear equations and four unknowns. In real life, the fact that the car (especially the tires) is not a rigid body solves the problem. I suspect that if you add one or more stiff, but not infinitely stiff, elements to the system, you will be able to solve it.

7 个评论
显示 5更早的评论隐藏 5更早的评论

William Rose 2025-3-3

@idan, The constraining equaitions are

moment about x-axis (or an axis parallel to x-axis)=0,

moment about y-axis (or an axis parallel to y-axis)=0,

moment about z-axis (or an axis parallel to z-axis)=0,

total force in x,y,z directions =0.

That makes six equations, so I expect these will allow me to solve for the six unknowns: F3x,F3y,F3z and F4x,F4y,F4z. But the equations are not independent - as you have demonstrated, by converting the equations to a matrix (A, size 6x6), and finding the rank of A.

I verified that the six equations are not independent, by finding the moments about the x, y, and z axes through the origin, instead of finding the moments about parallel axes thourgh point 2, and by writing out the six independent equations. I still found that the rank of the 6x6 matrix A equals 5.

I added random components to x1, y1, z1, and I added random components to F1x, F1y, F1z, just in case some unlucky combination of constants was the cause of A being rank-deficient. But this did not make a difference: the rank of A was still 5. I confirmed this by finding (with vpasolve) that there is a linear combination of the first 5 rows of A that adds up to the 6th row of A, with an error smaller than 10^-37 in each of the six elements.

I conclude from this that if one balances the 3-dimensional force correctly for this rigid body, and if one balances the moments for two of the three axes, then the moment will be automatically balanced along the third axis. It is not obvious to me why this is the case.

idan 2025-3-3

编辑：William Rose 2025-3-3

在 MATLAB Online 中打开

@idan,

You asked "Thank you for helping, so should I find only 5 unknowns instead of 6? And how much equations should I use?"

I was wondering the same thing. Since the 6x6 matrix A has rank=5, I wondered if I could assume a value for one of the six unknowns, then use the six equations to solve for the remaining five unknowns. I chose to set F3x=1. The five unknowns are

syms F3y F3z F4x F4y F4z

Now size(A)=[6,5]. rank(A)=5 still. vpasolve() still cannot find a solution. (This is evident from the fact that F3y, F3z, etc. are empty.) One could try to solve the matrix equation by

x=A\b;

where size(A)=6,5, size(b)=6,1, and therefore size(x)=5,1. This results in

Warning: Solution does not exist because the system is inconsistent.

So I tried

x1=A(1:5,:)\b(1:5);

and I got

x1=A(1:5,:)\b(1:5)=

584.1462 54.0851 -5.0000 -589.1462 -70.0851

I then tried

x2=A(2:6,:)\b(2:6);

and I got

x2=A(2:6,:)\b(2:6)=

133.8819 3.9545 -15.8578 -138.8819 -19.9545

In other words, I can solve for the five unknowns, if I only use five of the six equations. However, the answer I get is very different, if I use a different subset of the six equations. This indicates that the six equations are inconsistent - which Matlab already told us. We have just confirmed it.

I am not sure why the six equations (with five unknowns) are inconsistent. The inconsistency suggests that I made a bad choice for F3x. But I don;t know what a better choice would be.

One more thing: @Torsten says here that

if rank(C) = rank([C,S]), you can solve the system.

if rank(C) = rank([C,S]) -1, the system is inconsistent.

where C and S correspond to A and b in the present example. Therefore I calculated rank(A)=5 and rank([A,b])=6, which confirms, in another way, that the six equations contain an inconsistency.

I redid the analysis with two equations involving 3-D vectors, instead of six equations involving scalar variables, and I got the same results, including the exact same numeric solutions for x1 and x2 above. This gives me confidence that I did not make a mistake typing in the equations.

I don't know why the equations are inconsistent. There's probably a simple explanation that I'm not seeing.

idan 2025-3-3

Thank you for trying any way😊 I’m sure there is a good reason for the inconsistency.

After having a good solution I want to check if F3 and F4 are inside the friction cones and then I can solve the problem😄 (and check If the random F2 is feasible)

idan 2025-3-10

Hi, I want to add some information that might help us.

the solution we need to solve is the complete solution of the equation:

X=Xparticular + CXhomogeneous

where:

xpx_pxp is the particular solution, representing one possible way the reaction forces distribute to satisfy the equilibrium.
xhx_hxh is the homogeneous solution, representing the null space of the force distribution matrix AAA. It consists of internal force modes that do not affect external equilibrium.

It seems that three are inner forces that applied in the system..

look at the Xh-

Homogeneous Solution xhx_hxh

Represents internal force distributions that do not affect the robot’s net force and moment balance.
These forces are often due to internal constraints like friction, tension, or redundancy in leg support.
Physically, this corresponds to internal stress modes, which allow for multiple valid solutions.

But when trying to solve the equations there is still the Ax=b that I can't solve because of the inconsisnt of the problem, any idea how to solve it?

请先登录，再进行评论。

Answer 3

Walter Roberson 2025-3-2

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2174680-solve-equilibrium-equations-in-matlab-for-a-quadruped-robot#answer_1560947

[eq1; eq2] only has rank 5 even though there are 6 equations. There are either no solutions to [eq1; eq2] or else there are an infinite number of solutions.

3 个评论
显示 1更早的评论隐藏 1更早的评论

idan 2025-3-2

In addition, does this set is linear or nonlinear? Did I have made some linearization by open the cross componenets?

Walter Roberson 2025-3-2

The equations posted are linear equations. I have no information about whether such a system should be linear or not.

请先登录，再进行评论。

Answer 4

idan 2025-3-4

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2174680-solve-equilibrium-equations-in-matlab-for-a-quadruped-robot#answer_1561078

Another option that I was thinking about is to trying to solve with some kind of assessment and check for a minimum force and moments normal (the overall error), then set the value of f3 and f4 as the value of initial starting point in fsolve.

What do you think about it?

6 个评论
显示 4更早的评论隐藏 4更早的评论

Torsten 2025-3-4

编辑：Torsten 2025-3-4

在 MATLAB Online 中打开

This code finds x = [F3x, F3y, F3z, F4x, F4y,F4z] that minimizes the 2-norm error of the linear system A*x=B. I don't know if the solution makes sense physically. But in order to proceed, I think it's necessary to find out why the matrix A is rank-deficient.

% Known forces and weight

F1 = [1; 1; 1];

F2 = [2; 3; 5];

W = [1; 1; 10];

x1=50;

y1=31.65;

z1=16.09;

x2=44.68;

y2=-27.26;

z2=-24.34;

x3=-4.72;

y3=-24.4;

z3=-27.21;

x4=0;

y4=25;

z4=-21.65;

xCOM=2.53;

yCOM=1.1;

zCOM=-4.47;

% Position vectors relative to leg 2

r1 = [x1 - x2; y1 - y2; z1 - z2];

r3 = [x3 - x2; y3 - y2; z3 - z2];

r4 = [x4 - x2; y4 - y2; z4 - z2];

rCOM = [xCOM - x2; yCOM - y2; zCOM - z2];

% Unknown forces

syms F3x F3y F3z F4x F4y F4z

F3 = [F3x; F3y; F3z];

F4 = [F4x; F4y; F4z];

% Force equilibrium

eq1 = F1 + F2 + F3 + F4 + W == 0

eq1 =

% Moment equilibrium

M1 = cross(r1, F1);

M3 = cross(r3, F3);

M4 = cross(r4, F4);

MCOM = cross(rCOM, W);

eq2 = M1 + M3 + M4 + MCOM == 0

eq2 =

%eq2 = cross(r1, F1) + cross(r3, F3) + cross(r4, F4) + cross(rCOM, W) == 0;

[A, B] = equationsToMatrix([eq1; eq2], [F3x, F3y, F3z, F4x, F4y, F4z]);

rank_A = rank(A);

null(A)

ans =

●

rank([A,B])

ans = 6

disp(A);

disp(rank_A);

5

% Solve for F3 and F4

%solutions = vpasolve([eq1; eq2], [F3x, F3y, F3z, F4x, F4y, F4z]);

A = double(A);

B = double(B);

solutions = lsqlin(A,B)

Warning: Rank deficient, rank = 5, tol = 9.161130e-14.

solutions = 6×1

-7.6393 -5.5611 -5.9578 3.3004 0 -4.7689

norm(A*solutions-B)

ans = 5.3150

William Rose 2025-3-5

编辑：William Rose 2025-3-5

在 MATLAB Online 中打开

@Torsten, that is very nice.

@idan, I apologize in advance for the length of what follows.

I now understand why we can't find an analytic solution. First, note that the combination of pre-specified forces at two of the robot's legs (which I will now call points 3 and 4) and the body forces (weight, etc.) combine to produce a net force vector Fe (e for external) and a net moment vector about the origin, Me. We are trying to counteract the external force and moment by applying forces at the other two legs, which I now call points 1 and 2. I originally thought that, since I have six force components to adjust, I would be able to find a solution. But these six components are not enough, because I only have two points at which to apply forces. This limits my ability to apply torques in the plane defined by the origin (or wherever we choose to measure moment about) and points 1 and 2.

The following specific example helps me understand this situation better:

The net external force on the quadruped robot is the combination of weight and pre-specified forces applied at points 3 and 4: Fe=[Fex,Fey,Fez]'. The net external moment about the origin is due to the weight acting at the COM, and the forces at points 3 and 4: Me=[Mex,Mey,Mez]'. I can apply force at points r1=[1,0,0]' and r2=[0,1,0]'. In this case, the only way I can produce torque about the x-axis is by applying force in the z-direction at point r2=[0,1,0]'. (This may not be obvious, but think about it, and write the equation for the moments due to F1 at r1 and F2 at r2.) Therefore I apply force F2z=-Mey/(1 length unit) in the z-direction at point r2, in order to cancel out Mey. The only way I can apply moment about the y-axis is by applying force in the z-direction at point r1. Therefore I apply force F1z=-Mex/(1 length unit) in the z-direction at point r1, in order to cancel out Mex. Now I have successfully cancelled out Mex and Mey, the x- and y-components of the external moment. But I have no extra flexibility to allow me to cancel out the net force Fez. If F1z + F2z, as calculated to cancel out Mex and Mey, does not equal -Fez, then I cannot cancel out Fez.

The system above is described by the following equations:

Fe=[Fex,Fey,Fez]'; Me=[Mex,Mey,Mez]' net external force and net external moment about the origin

The problem is: Find forces F1=[F1x,F1y,F1z]' applied at point r1, and force F2=[F2x,F2y,F2z] applied at point r2, such that

F1+F2+Fe=0 (1)

and

M1+M2+Me=0 (2).

It follows from (1) that

F1x+F2x=-Fex (3a); F1y+F2y=-Fey (3b); F1z+F2z=-Fez (3c)

From the definition of torque (about the origin), we have

M1=[r1yF1z-r1zF1y, r1zF1x-r1xF1z, r1xF1y-r1yF1x]' (4a)

and

M2=[r2yF2z-r2zF2y, r2zF2x-r2xF2z, r2xF2y-r2yF2x]' (4b) .

In the special case where r1=[1,0,0]' and r2=[0,1,0]', (4a) and (4b) become

M1=[0,-F1z,F1y]' (5a) (multiply right side by 1 unit of length to get the units right)

and

M2=[F2z,0,-F2x]' (5b) .

It follows from (2) and (4a) and (4b) that

F2z=-Mex (6a) (multiply left side by 1 unit of length to get units right)

-F1z=-Mey (6b)

F1y-F2x=-Mez (6c)

We can combine (3a,b,c) and (6a,b,c) into a matrix equation:

Ax=b (7)

where

A=[1 0  0  1 0 0
1  0  0 1 0
0  1  0 0 1
0  0  0 0 1
0 -1  0 0 0
1  0 -1 0 0];  % (8a)

and x=vector of unknowns: x=[F1x,F1y,F1z,F2x,F2y,F2z]' (8b)

and b=vector of known quantities: b=[-Fex,-Fey,-Fez,-Mex,-Mey,-Mez]'. (8c)

We see that A is rank-deficient

disp(rank(A))
     5

because row 4 - row 5 = row 3. Rows 4 and 5 correspond to torque about x and y, and row 3 corresponds to force along the z axis. These are exactly the quantities which cannot be all set independently, as discussed above. I.e. the math confirms the verbal description.

There is a way to modify the problem so it has exactly one solution. Apply two force components at each of three points. There will be six unknown force components to determine, as before. The three points canot be co-planar with the origin (the point about which torque is measured), and the force components applied at each point must not be directly toward or away from the origin (the point about which torque is measured).

In the example described above, let the third point be r3=[0,0,1]'.

Apply force F1=[0,F1y,F1z] at point r1=[1,0,0]'. (9a)

Apply force F2=[F2x,0,F2z] at point r2=[0,1,0]'. (9b)

Apply force F3=[F3x,F3y,0] at point r3=[0,0,1]'. (9c)

The vector equations for this system are

F1+F2+F3+Fe=0 (10)

M1+M2+M3+Me=0 (11)

It follows from (9a,b,c) and (10) that

F2x+F3x=-Fex (12a); F1y+F3y=-Fey (12b); F1z+F2z=-Fez (12c)

It follows from the definition of moment, and for the special case of r1, r2, r3 specified in (9a,b,c), that

M1=[0,-F1z,F1y]' (13a); M2=[F2z,0,-F2x]' (13b); M3=[-F3y,F3x,0]' (13c).

It follows from (11) and (13a,b,c) that

F2z-F3y=-Mex (14a)

-F1z+F3x=-Mey (14b)

F1y-F2x=-Mez (14c)

We can combine (12a,b,c) and (14a,b,c) into a matrix equation:

Ax=b (15)

where

A=[0  0  1 0 1  0
0  0 0 0  1
1  0 1 0  0
0  0 1 0 -1
-1  0 0 1  0
0 -1 0 0  0];  % (16a)

and x=vector of unknowns: x=[F1y,F1z,F2x,F2z,F3x,F3y]' (16b)

and b=vector of known quantities: b=[-Fex,-Fey,-Fez,-Mex,-Mey,-Mez]'. (16c)

(Check my work, in case I made a mistake somewhere.)

This matrix A has full rank.

disp(rank(A))
     6

and therefore we can solve for the six unknown forces. I think this approach will work for a more general set of three points, as long as the three points are not co-planar with the point about which torque is measured, and as long as neither of the two force components at points 1,2,3 is directed exactly toward or away from the point about which torque is measured.

William Rose 2025-3-5

quadRobotBalance4.m

@Paul,

That is a good insight. Thanks! You are right that, in general, the net moment created by F1, F2, and W will include a component along the axis connecting P3 and P4, which can't be counteracted.

I tried modifying the locations of points P1 to P4 and W, and I chose F1, F2, and W so that the next external moment, M1+M2+MW, does not include a component parallel to vector P4-P3. The script (attached) fails to find a solution. I try two ways to find a solution: with vpasolve() on the two vector equations, and by doing x=A\b. Neither way fionds a solution. Maybe I made a mistake in my coding or my selection of points and forces. Maybe if I wrote out the equations for this special case by hand, I would find a simplified matrix that gives a unique solution.

I still think that by applying two components of force at each of three locations, the system will have a unique solution. Assming the two components and three locations meet the requirements I mentioned in earlier posts.

Paul 2025-3-5

Thinking about it some more … wouldn’t it be true that the resultant moment of the weight and any two contact forces must be zero around the axis that connects the other two contact points?

William Rose 2025-3-6

@Paul, the weight acts at the c.o.m. location, which is genreally not on the axis between two contact force points, so the weight is likely to create a net moment about that axis. But I agree with you about the forces at the two contact points: those forces won't create a moment about the axis between the contact points.

请先登录，再进行评论。

Answer 5

Paul 2025-3-8

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2174680-solve-equilibrium-equations-in-matlab-for-a-quadruped-robot#answer_1561409

在 MATLAB Online 中打开

Problem parameters

W = [1; 1; 10];
x1=50;y1=31.65;z1=16.09;
x2=sym(44.68);y2=sym(-27.26);z2=sym(-24.34);
x3=-4.72;y3=-24.4;z3=-27.21;
x4=0;y4=25;z4=-21.65;
xCOM=2.53;yCOM=1.1;zCOM=-4.47;
% Position vectors relative to leg 2
r21 = [x1 - x2; y1 - y2; z1 - z2];
r22 = zeros(3,1);
r23 = [x3 - x2; y3 - y2; z3 - z2];
r24 = [x4 - x2; y4 - y2; z4 - z2];
r2COM = [xCOM - x2; yCOM - y2; zCOM - z2];

If the objective is to find the static equilibrium of the rigid body then the contact forces have to satisfy the force and moment balance equations. Combining all four contact forces into a single 12x1 vector we have

% A*F = b,  F = [F1;F2;F3;F4]
A = [repmat(eye(3),1,4); ...
    sym(vcross(r21)),sym(vcross(r22)),sym(vcross(r23)),sym(vcross(r24))];
b = [-W;-sym(vcross(r2COM))*W];

where the first three rows [A,b] are the force balance equations and the second three rows are the moment balance equations around contact point-2.

We have six equations in 12 unknowns, and the solution space is parameterized by a 12-d vector w

w = sym('w',[12,1]);

with all possible solutions given by

F = pinv(A)*b + (eye(12) - pinv(A)*A)*w; % (1)

Verify that this parametric solution satisfies the equation

A*F - b

ans =

In the question, it is assumed that two contact forces are known a priori

F1 = sym([1; 1; 1]);
F2 = sym([2; 3; 5]);

If these forces are valid at equilibrium, we should be able to find the corresponding values of w, but solve fails to do so, which, if correct, indicates that F1 and F2 as given are not a valid pair of contact forces for which the body can be in static equilibrium.

sol = solve([eye(6),zeros(6)]*F == [F1;F2],w)
sol = struct with fields:
     w1: [0x1 sym]
     w2: [0x1 sym]
     w3: [0x1 sym]
     w4: [0x1 sym]
     w5: [0x1 sym]
     w6: [0x1 sym]
     w7: [0x1 sym]
     w8: [0x1 sym]
     w9: [0x1 sym]
    w10: [0x1 sym]
    w11: [0x1 sym]
    w12: [0x1 sym]

If the goal is analyze the static equilirium of the body, then we have to make sure that the contact forces satisfy (1). Of course, (1) yields an infinite number of solutions for equilibrium. Not sure what kind of solution is of interest, but any additional constraint equations added to (1) to further narrow down the scope of the problem need to be linearly independent of the balancing equations.

Or perhaps one could solve for the values in w that yield the minimum norm of F, or any other desirable property of F.

Keep in mind that the mapping from w to F is not 1-1, that is different values in w can lead to the same values in F. For example, solve for F assuming a simple set of values for w

FF = subs(F,w,(1:12).');

Now assume we are given the forces in FF and solve for w. The result is different than what we started from

solw = solve(F==FF)
solw = struct with fields:
     w1: -4634578167/723888464
     w2: -7633332147/180972116
     w3: -40458567/6134648
     w4: -2401984815/55683728
     w5: -13775113539/361944232
     w6: 0
     w7: -2238/59
     w8: 0
     w9: 0
    w10: 0
    w11: 0
    w12: 0

But this solution yields the same force vector.

FF - subs(F,solw)

ans =

function vc = vcross(v)

vc = [0 -v(3) v(2);v(3) 0 -v(1);-v(2) v(1) 0];

end

5 个评论
显示 3更早的评论隐藏 3更早的评论

idan 2025-3-15

Hi. I still don't understand yout solution. can you please give a specific example?

Paul 2025-3-15

What specifically do you not understand? Specific example of what?

请先登录，再进行评论。

Solve equilibrium equations in MATLAB for a quadruped robot

2 个评论
显示无隐藏无

采纳的回答

4 个评论
显示 2更早的评论隐藏 2更早的评论

更多回答（4 个）

7 个评论
显示 5更早的评论隐藏 5更早的评论

3 个评论
显示 1更早的评论隐藏 1更早的评论

6 个评论
显示 4更早的评论隐藏 4更早的评论

5 个评论
显示 3更早的评论隐藏 3更早的评论

另请参阅

类别

标签

Community Treasure Hunt

Solve equilibrium equations in MATLAB for a quadruped robot

2 个评论 显示 无隐藏 无

采纳的回答

4 个评论 显示 2更早的评论隐藏 2更早的评论

更多回答（4 个）

7 个评论 显示 5更早的评论隐藏 5更早的评论

3 个评论 显示 1更早的评论隐藏 1更早的评论

6 个评论 显示 4更早的评论隐藏 4更早的评论

5 个评论 显示 3更早的评论隐藏 3更早的评论

另请参阅

类别

标签

Community Treasure Hunt

2 个评论
显示无隐藏无

4 个评论
显示 2更早的评论隐藏 2更早的评论

7 个评论
显示 5更早的评论隐藏 5更早的评论

3 个评论
显示 1更早的评论隐藏 1更早的评论

6 个评论
显示 4更早的评论隐藏 4更早的评论

5 个评论
显示 3更早的评论隐藏 3更早的评论