-
Notifications
You must be signed in to change notification settings - Fork 43
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
updated and tested simulink-only simulator
Gabriele
committed
Feb 2, 2024
1 parent
690723d
commit 213f0bb
Showing
17 changed files
with
19,620 additions
and
26,475 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
20 changes: 10 additions & 10 deletions
20
controllers/simulink-balancing-simulator/src/estimateContactForcesFromDynamics.m
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,36 +1,36 @@ | ||
function f_dynamics = estimateContactForcesFromDynamics(tau, JL, JR, JDotL_nu, JDotR_nu, M, h, LEFT_RIGHT_FOOT_IN_CONTACT) | ||
|
||
% try to estimate the contact forces using the joint torques, the | ||
% dynamics model, the robot state and the contact constraints. | ||
ndof = size(M,1)-6; | ||
B = [zeros(6,ndof); eye(ndof)]; | ||
f_dynamics = zeros(12,1); | ||
|
||
if sum(LEFT_RIGHT_FOOT_IN_CONTACT) > 1.5 | ||
|
||
% two feet balancing | ||
Jc = [JL; JR]; | ||
JcDot_nu = [JDotL_nu; JDotR_nu]; | ||
|
||
pinvDampJMJ = eye(12)/(Jc/M*Jc'); | ||
f_dynamics = pinvDampJMJ*(Jc/M*(h-B*tau)-JcDot_nu); | ||
|
||
elseif LEFT_RIGHT_FOOT_IN_CONTACT(1) > 0.5 && LEFT_RIGHT_FOOT_IN_CONTACT(2) < 0.5 | ||
|
||
% left foot balancing | ||
Jc_left = JL; | ||
Jc_leftDot_nu = JDotL_nu; | ||
|
||
pinvDampJMJ_left = eye(6)/(Jc_left/M*Jc_left'); | ||
f_dynamics = [pinvDampJMJ_left*(Jc_left/M*(h-B*tau)-Jc_leftDot_nu); zeros(6,1)]; | ||
|
||
elseif LEFT_RIGHT_FOOT_IN_CONTACT(2) > 0.5 && LEFT_RIGHT_FOOT_IN_CONTACT(1) < 0.5 | ||
|
||
% right foot balancing | ||
Jc_right = JR; | ||
Jc_rightDot_nu = JDotR_nu; | ||
|
||
pinvDampJMJ_right = eye(6)/(Jc_right/M*Jc_right'); | ||
f_dynamics = [zeros(6,1); pinvDampJMJ_right*(Jc_right/M*(h-B*tau)-Jc_rightDot_nu)]; | ||
end | ||
end | ||
end |
8 changes: 4 additions & 4 deletions
8
controllers/simulink-balancing-simulator/src/forwardDynamics.m
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,10 +1,10 @@ | ||
function nuDot = forwardDynamics(tau, JL, JR, M, h, f, LEFT_RIGHT_FOOT_IN_CONTACT) | ||
|
||
% get the robot accelerations using the joint torques, the | ||
% dynamics model, the robot state and the contact constraints. | ||
ndof = size(M,1)-6; | ||
B = [zeros(6,ndof); eye(ndof)]; | ||
|
||
nuDot = M\(B*tau+LEFT_RIGHT_FOOT_IN_CONTACT(1)*JL'*f(1:6)+LEFT_RIGHT_FOOT_IN_CONTACT(2)*JR'*f(7:end)-h); | ||
end | ||
|
||
end |
Oops, something went wrong.