updated and tested simulink-only simulator

Author: Gabriele

controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configRobot.m

@@ -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;

controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m

@@ -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);

controllers/simulink-balancing-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m

@@ -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]

controllers/simulink-balancing-simulator/initTorqueControlBalancingSim.m

@@ -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

controllers/simulink-balancing-simulator/src-static-gui/closeModel.m

@@ -17,4 +17,4 @@
 rmpath('../../library/matlab-gui');
 rmpath('./src-static-gui');
 
-disp('[closeModel]: done.')
+disp('[closeModel]: done.')

controllers/simulink-balancing-simulator/src-static-gui/compileModel.m

@@ -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.')

controllers/simulink-balancing-simulator/src/computeBasePoseDerivative.m

@@ -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

controllers/simulink-balancing-simulator/src/estimateContactForcesFromDynamics.m

@@ -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

controllers/simulink-balancing-simulator/src/forwardDynamics.m

@@ -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