Skip to content

Commit

Permalink
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
Browse files Browse the repository at this point in the history
Gabriele committed Feb 2, 2024
1 parent 690723d commit 213f0bb
Showing 17 changed files with 19,620 additions and 26,475 deletions.
Original file line number Diff line number Diff line change
@@ -42,7 +42,7 @@

% Frames list
Frames.BASE = 'root_link';
Frames.IMU = 'imu_frame';
Frames.IMU = 'head_imu_0';
Frames.LEFT_FOOT = 'l_sole';
Frames.RIGHT_FOOT = 'r_sole';
Frames.COM = 'com';
@@ -105,4 +105,4 @@
Ports.NECK_POS_PORT_SIZE = 3;
Ports.IMU_PORT_SIZE = 12;
Ports.IMU_PORT_ORIENTATION_INDEX = [1,2,3];
Ports.WRENCH_PORT_SIZE = 6;
Ports.WRENCH_PORT_SIZE = 6;
Original file line number Diff line number Diff line change
@@ -157,4 +157,4 @@

% Compute contact constraints (friction cone, unilateral constraints)
[ConstraintsMatrix, bVectorConstraints] = wbc.computeRigidContactConstraints ...
(forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin);
(forceFrictionCoefficient, numberOfPoints, torsionalFrictionCoefficient, feet_size, fZmin);
Original file line number Diff line number Diff line change
@@ -344,4 +344,4 @@
Config.noOscillationTime = 0;
Config.directionOfOscillation = [0;1;0];
Config.amplitudeOfOscillation = 0.02; % [m]
Config.frequencyOfOscillation = 0.2; % [Hz]
Config.frequencyOfOscillation = 0.2; % [Hz]
Original file line number Diff line number Diff line change
@@ -28,12 +28,12 @@
%
% If you are simulating the robot with Gazebo, remember that it is required
% to launch Gazebo as follows:
%
%
% gazebo -slibgazebo_yarp_clock.so
%
%
% and properly set the environmental variable YARP_ROBOT_NAME in the .bashrc.

% Simulation time in seconds. For long simulations, put an high number
% Simulation time in seconds. For long simulations, put an high number
% (not inf) for allowing automatic code generation
Config.SIMULATION_TIME = 70;

@@ -46,10 +46,10 @@
%
% 'YOGA': the robot will perform the YOGA++ demo (highly dynamic movements
% while balancing on one foot and two feet)
%
%
% 'COORDINATOR': the robot can either balance on two feet or move from left
% to right follwing a desired center-of-mass trajectory.
%
%
DEMO_TYPE = 'YOGA';

% Config.SCOPES: debugging scopes activation
@@ -68,16 +68,16 @@
Config.PLOT_INTEGRATION_TIME = false;

% Run robot-specific configuration parameters
run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m'));
run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configStateMachine.m'));
run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m'));
run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m'));
run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configStateMachine.m'));
run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m'));

% Deactivate/activate the internal coordinator
if strcmpi(DEMO_TYPE, 'COORDINATOR')

Config.COORDINATOR_DEMO = true;

elseif strcmpi(DEMO_TYPE, 'YOGA')

Config.COORDINATOR_DEMO = false;
end
end
Original file line number Diff line number Diff line change
@@ -17,4 +17,4 @@
rmpath('../../library/matlab-gui');
rmpath('./src-static-gui');

disp('[closeModel]: done.')
disp('[closeModel]: done.')
Original file line number Diff line number Diff line change
@@ -12,7 +12,7 @@
torqueControlBalancingSim([], [], [], 'term')

catch ME

errorMessages = ME;
end

@@ -21,4 +21,4 @@
disp('Compilation done.')

% warning about Simulink timing
warning('The model will anyways start with FEW SECONDS OF DELAY after pressing the ''Start Model'' button.')
warning('The model will anyways start with FEW SECONDS OF DELAY after pressing the ''Start Model'' button.')
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
function pose_base_dot = computeBasePoseDerivative(nu_base_ikin, pose_base)

% COMPUTEBASEPOSEDERIVATIVE computes the base position and orientation
% time derivative in terms of linear velocity
% time derivative in terms of linear velocity
% + quaternion derivative.

%% --- Initialization ---

% angular velocity in body coordinates
@@ -17,4 +17,4 @@

% compute the base pose derivative
pose_base_dot = [nu_base_ikin(1:3); dquat_base];
end
end
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
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
Loading

0 comments on commit 213f0bb

Please sign in to comment.