Merge pull request #4 from Space-Robotics-Laboratory/fix/uno_crawl_pr…

…eset Fix/uno_crawl_preset
Space-Robotics-Laboratory · Sep 16, 2020 · 8168bfc · 8168bfc
2 parents be4419f + 8b29a87
commit 8168bfc
Show file tree

Hide file tree

Showing 29 changed files with 71 additions and 61 deletions.
diff --git a/README.md b/README.md
@@ -36,10 +36,11 @@ We confirmed the code is working with:
 #### Build and Run
 * Download the files from this repository
 * Open MATLAB
-* Select `climbing_simulator/` as the current folder for MATLAB
-  - You should see `dat/`, `lib/` and `src/` directories under it
-* Setup the parameters as you want
-* Run `climb_main.m` file (**Note: MATLAB might ask you to change folder or add path. Choose to "add path"**)
+* Select `ClimbLab/` as the current folder for MATLAB
+  - You should see `config/`, `dat/`, `docs/`, `lib/` and `src/` directories under it
+* Setup the parameters as you want by changing the variable `config`.
+  - Note: If you select USER configuration type, follow instructions described in the `config/USER/config_USER_param_template.m`.
+* Run `src/climb_main.m` file (**Note: MATLAB might ask you to change folder or add path. Choose to "add path"**)
 
 ## Publications
 

diff --git a/config/preset/config_uno_crawl_param.m b/config/preset/config_uno_crawl_param.m
@@ -8,7 +8,7 @@
 %%%%%% Last update: 2020-07-09
 %
 %
-% Load configurations for parameters defined for uno crawl fait simulation 
+% Load configurations for parameters defined for uno crawl gait simulation 
 %
 % Function variables:
 %
@@ -31,19 +31,19 @@
 %%% Type of the robot to simulate ('HubRobo_no_grip', 'HubRobo_grip_to_palm', 'HubRobo_grip_to_spine')
 robot_param.robot_type = 'HubRobo_grip_to_palm';
 %%% x and y position of legs relative to base center [m]
-robot_param.foot_dist  = 0.12;
+robot_param.foot_dist  = 0.14;
 %%% Height of base relative to map [m]
-robot_param.base_height = 0.10;
+robot_param.base_height = 0.09;
 %%% Base position [m] 2x1 vector. or 'default' for default setting
 robot_param.base_pos_xy = [0;0];
 
 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %%% Type of the surface ('flat_HR', 'rough', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') 
 environment_param.surface_type = 'flat_003';
 %%% Maximum simulation time [s]
-environment_param.time_max = 24;
+environment_param.time_max = 30;
 %%% Graspable points detection type
-environment_param.graspable_points_detection_type = 40;
+environment_param.graspable_points_detection_type = 30;
 
 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver')

diff --git a/src/controller/equilibrium/upd_equilibrium_eval.m b/src/controller/equilibrium/upd_equilibrium_eval.m
@@ -16,6 +16,7 @@
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
 %     INPUT
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
+%         surface_param          : Parameters for surface (class)
 %		  LP                     : Link parameters (SpaceDyn class)
 %         SV 	      			 : State variables (SpaceDyn class)
 %         POS_e      		     : Position of the end-effector [m] (3xnum_limb matrix)

diff --git a/src/controller/equilibrium/upd_tsm.m b/src/controller/equilibrium/upd_tsm.m
@@ -1,23 +1,25 @@
 %%%%%% Update
 %%%%%% upd_tsm
 %%%%%% 
-%%%%%% Calculate tumbling moment for axes
+%%%%%% Calculate TSM
 %%%%%% 
 %%%%%% Created 2020-05-08
 %%%%%% Warley Ribeiro
 %%%%%% Last update: 2020-05-08
 %
 %
-% Judgment of equilibrium based on tumbling moment
+% Calculate Tumble Stability Margin
 %
 % Function variables:
 %
 %     OUTPUT
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
-%         equilibrium_eval_param.tumbling_axes         : Number of legs that makes a tumbling axis (tumbling_axes_numberx2 matrix)
-%         equilibrium_eval_param.tumbling_axes_number  : Total number of tumbling axes (scalar)
+%         equilibrium_eval_param.tsm                   : Tumble Stability Margin [m] (scalar)
+%         equilibrium_eval_param.tsm_equilibrium_flag  : Flag for equilibrium based on TSM (scalar)
 %     INPUT
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
+%         LP           			 : Link Parameters (SpaceDyn class)
+
 
 function equilibrium_eval_param = upd_tsm(equilibrium_eval_param, LP)
 

diff --git a/src/controller/equilibrium/upd_tsm_equilibrium_judgment.m b/src/controller/equilibrium/upd_tsm_equilibrium_judgment.m
@@ -1,7 +1,7 @@
 %%%%%% Update
 %%%%%% upd_tsm_equilibrium_judgment
 %%%%%% 
-%%%%%% Calculate tumbling moment for axes
+%%%%%% Equilibrium judgment for each tumbling axis
 %%%%%% 
 %%%%%% Created 2020-05-08
 %%%%%% Warley Ribeiro
@@ -14,10 +14,12 @@
 %
 %     OUTPUT
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
-%         equilibrium_eval_param.tumbling_axes         : Number of legs that makes a tumbling axis (tumbling_axes_numberx2 matrix)
-%         equilibrium_eval_param.tumbling_axes_number  : Total number of tumbling axes (scalar)
+%         equilibrium_eval_param.tsm_axes_judgment     : Parameter for tumbling condition for each tumbling axes (tumbling_axes_number vector)
 %     INPUT
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
+%         LP                     : Link parameters (SpaceDyn class)
+%         SV                     : State values (SpaceDyn class)
+%         POS_e                  : Position of the end-effector [m] (3xnum_limb matrix)
 
 function equilibrium_eval_param = upd_tsm_equilibrium_judgment(equilibrium_eval_param, LP, SV, POS_e)
 

diff --git a/src/controller/equilibrium/upd_tsm_tumbling_moment.m b/src/controller/equilibrium/upd_tsm_tumbling_moment.m
@@ -14,12 +14,13 @@
 %
 %     OUTPUT
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
-%         equilibrium_eval_param.tumbling_axes         : Number of legs that makes a tumbling axis (tumbling_axes_numberx2 matrix)
-%         equilibrium_eval_param.tumbling_axes_number  : Total number of tumbling axes (scalar)
+%         equilibrium_eval_param.Mab         : Tumbling moment for each tumbling axis (1xtumbling_axes_number vector)
 %     INPUT
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (class)
 %         LP           			 : Link Parameters (SpaceDyn class)
 %         SV           			 : State Variables (SpaceDyn class)
+%         POS_e                  : Position of the end-effector [m] (3xnum_limb matrix)
+%         F_grip                 : Maximum gripping force [N] (scalar)
 
 function equilibrium_eval_param = upd_tsm_tumbling_moment(equilibrium_eval_param, LP, SV, POS_e, F_grip)
 

diff --git a/src/controller/motion/upd_base_traject_spline3_one_segment.m b/src/controller/motion/upd_base_traject_spline3_one_segment.m
@@ -20,10 +20,9 @@
 %								1st dim: x-y-z coordinates (1 to 3)
 %								2nd dim: leg number (1 to n)
 %								3rd dim: coefficient index (1 to 4)
-%								4th dim: segment number (1 or 2)
 %     INPUT
-%         gait_param             : Parameters for gait (class)
 %         path_planning_param    : Parameters for path planning (class)
+%         gait_param             : Parameters for gait (class)
 %         motion_planning_param  : Parameters for motion planning (class)
 %         des_SV      			 : Desired State variable (SpaceDyn class)
 %         LP      				 : Link Parameters (SpaceDyn class)

diff --git a/src/controller/motion/upd_inverse_kinematics.m b/src/controller/motion/upd_inverse_kinematics.m
@@ -8,7 +8,7 @@
 %%%%%% Last update: 2020-04-17
 %
 %
-% Update the state of the gripper, if it is open or closed, based on the timing for the crawl gait
+% Update desired joint space configuration state from inverse kinematics
 %
 % Function variables:
 %

diff --git a/src/controller/motion/upd_motion_planning.m b/src/controller/motion/upd_motion_planning.m
@@ -16,9 +16,9 @@
 %         motion_planning_param  : Parameters for motion planning (class)
 %         des_SV      			 : Desired state variables (SpaceDyn class)
 %     INPUT
+%         motion_planning_param  : Parameters for motion planning (class)
 %         gait_param             : Parameters for gait (class)
 %         path_planning_param    : Parameters for path planning (class)
-%         motion_planning_param  : Parameters for motion planning (class)
 %		  LP                     : Link parameters (SpaceDyn class)
 %         SV 	      			 : State variables (SpaceDyn class)
 %         des_SV      			 : Desired state variables (SpaceDyn class)

diff --git a/src/controller/path/upd_graspable_points_in_reachable_area.m b/src/controller/path/upd_graspable_points_in_reachable_area.m
@@ -14,8 +14,6 @@
 %
 %     OUTPUT
 %         path_planning_param                   : Parameters for path planning (class)
-%
-%
 %     INPUT
 %         SV                                    : State values (SpaceDyn class)
 %         LP                                    : Link parameters (SpaceDyn class)

diff --git a/src/controller/path/upd_path_planning.m b/src/controller/path/upd_path_planning.m
@@ -22,6 +22,8 @@
 %         SV                    : State values (SpaceDyn class)
 %         LP                    : Link parameters (SpaceDyn class)
 %         POS_e                 : Position of the end-effector [m] (3xnum_limb matrix)
+%         environment_param     : Parameters for environment (class)
+%         robot_param           : Parameters for robot (class)
 %         time                  : Simulation time [s] (scalar)
 
 function path_planning_param = upd_path_planning(path_planning_param, gait_param, surface_param, des_SV, SV, LP, POS_e, environment_param, robot_param, time)

diff --git a/src/engine/upd_fwd_dynamics.m b/src/engine/upd_fwd_dynamics.m
@@ -15,7 +15,7 @@
 %     OUTPUT
 %         SV                  : State Variables (SpaceDyn class)
 %     INPUT 
-%		  dynamics_flag       : Flag to compute ('on') or not compute ('off') dynamics
+%		  environment_param.dynamics_flag   : Flag to compute ('on') or not compute ('off') dynamics
 %         LP                  : Link parameters (SpaceDyn class)
 %         SV                  : State variables (SpaceDyn class)
 %         des_SV              : Desired state variables (SpaceDyn class)

diff --git a/src/environment/ini_environment.m b/src/environment/ini_environment.m
@@ -15,9 +15,7 @@
 %     OUTPUT
 %         ---
 %     INPUT
-%         time_step           : Time step [s] (scalar) 
-%         grav                : Gravity acceleration [G] (scalar) 
-%         inc                 : Surface inclination [deg] (scalar) 
+%         environment_param   : Parameters for environment (class)
 
 
 function ini_environment(environment_param)

diff --git a/src/environment/ini_graspable_points.m b/src/environment/ini_graspable_points.m
@@ -15,9 +15,10 @@
 %     OUTPUT
 %         surface_param.graspable_points   : 3xN matrix contains position vectors of graspable points [m] (x;y;z)
 %     INPUT
-%         graspable_points_detection_type        : Graspable points detection type (char: 'all','peaks', etc. or int: 0~100 [%]) 
-%     USGAE EXAMPLE
-%         graspable_points_detection_type = 50;
+%         environment_param   : Parameters for environment (class)
+%         surface_param       : Parameters for surface (class)
+%     USAGE EXAMPLE
+%         environment_param.graspable_points_detection_type = 50;
 %         surface_param = ini_graspable_points(graspable_points_detection_type, surface_param);
 
 

diff --git a/src/environment/ini_surface.m b/src/environment/ini_surface.m
@@ -17,7 +17,7 @@
 %         surface_param.K     : Ground reaction force stiffness coefficient (scalar)
 %         surface_param.D     : Ground reaction force damping coefficient (scalar)
 %     INPUT
-%         surface_type        : Surface type (string: flat_HR, rough) 
+%         environment_param   : Parameters for environment (class) 
 
 function surface_param = ini_surface(environment_param)
 

diff --git a/src/misc/ini_id.m b/src/misc/ini_id.m
@@ -1,7 +1,7 @@
 %%%%%% Initialize
 %%%%%% ini_id
 %%%%%% 
-%%%%%% Create video file
+%%%%%% Create id for naming
 %%%%%% 
 %%%%%% Created 2020-04-10
 %%%%%% Warley Ribeiro
@@ -13,14 +13,13 @@
 %     Function variables:
 %
 %     OUTPUT
-%         run_id          : Run identification (string)
-%         run_date        : Run identification date (string)
+%         run_id            : Run identification (string)
+%         run_date          : Run identification date (string)
 %     INPUT
-%         robot_type      : Robot type (string) 
-%         surface_type    : Surface type (string) 
-%         grav            : Gravity [G] (scalar) 
-%         inc             : Surface inclination [deg] (scalar) 
-%         gait_param.type : Gait type (string) 
+%         robot_param       : Parameter for robot (class)
+%         environment_param : Parameter for environment (class)
+%         gait_param        : Parameter for gait (class)
+%         control_param     : Parameter for control (class)
 
 function [run_id, run_date] = ini_id(robot_param, environment_param, gait_param, control_param)
 

diff --git a/src/misc/upd_variables_saved.m b/src/misc/upd_variables_saved.m
@@ -15,7 +15,10 @@
 %     OUTPUT
 %         variables_saved        : Variables to be saved (struct)
 %     INPUT
+%         variables_saved        : Variables to be saved (struct)
+%         save_settings          : Saving settings (struct)
 %         time				     : Simulation time [s] (scalar) 
+%         SV 	      			 : State variables (SpaceDyn class)
 %         equilibrium_eval_param : Parameters for equilibrium evaluation (struct)
 
 function variables_saved = upd_variables_saved(variables_saved, save_settings, time, SV, equilibrium_eval_param)

diff --git a/src/robot/ini_HubRobo_grip_to_palm_LP.m b/src/robot/ini_HubRobo_grip_to_palm_LP.m
@@ -1,5 +1,5 @@
 %%%%%% Initialize
-%%%%%% ini_HubRobo_gripper_to_palm_LP
+%%%%%% ini_HubRobo_grip_to_palm_LP
 %%%%%% 
 %%%%%% Mass/inertia parameters and link connections for HubRobo with grippers
 %%%%%% Last link length is defined as the distance from
@@ -8,7 +8,7 @@
 %%%%%% Created on      2018-02-23 by Warley Ribeiro
 %%%%%% Last updated on 2020-06-21 by Kentaro Uno
 
-function LP = ini_HubRobo_gripper_to_palm_LP()
+function LP = ini_HubRobo_grip_to_palm_LP()
 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%%

diff --git a/src/robot/ini_robot.m b/src/robot/ini_robot.m
@@ -15,16 +15,14 @@
 %
 %     OUTPUT
 %         LP           : Link Parameters (SpaceDyn class)
-%         des_SV       : Desired State Variables (SpaceDyn class)
 %         SV           : State Variables (SpaceDyn class)
+%         des_SV       : Desired State Variables (SpaceDyn class)
 %         F_grip       : Maximum gripping force [N] (scalar)
 %         POS_e        : Position of the end-effector [m] (3xnum_limb matrix)
 %         ORI_e        : Orientation of the end-effector [DC] (3x3*num_limb matrix)
 %         cont_POS     : Initial point of contact which is used as equilibrium position [m] (3xnum_limb matrix)
 %     INPUT
-%         robot        : Robot type (string) 
-%         foot_dist    : Distance from projection of robot center to end-point in both x and y directions [m] (scalar)
-%         base_height  : Height of base from mean surface of the ground [m] (scalar)
+%         robot_param  : Parameter for robot (class)
 %         gait_param   : Gait parameters
 %         surface_param: Surface parameters
 %

diff --git a/src/visualization/vis_animation_range.m b/src/visualization/vis_animation_range.m
@@ -14,11 +14,11 @@
 %     OUTPUT
 %         ani_settings.i_lim                       : Limits for the i-axis(1x2vector), i : x, y, z
 %     INPUT
-%         motion_planning_param                    : Motion planning param
+%         motion_planning_param                    : Parameters for motion plan (struct)
+%         surface_param                            : Parameters for surface (struct)
 %         ani_settings.i_lim                       : Limits for the i-axis(1x2vector), i : x, y, z
 %         ani_settings.i_vis_range                 : Range for the i-axis (1x2 vector), i : x, y, z
-
-
+%         inc                                      : Surface inclination [deg] (scalar)
 
 
 function [ ani_settings ] = vis_animation_range(motion_planning_param, surface_param, ani_settings, inc)

diff --git a/src/visualization/vis_animation_settings.m b/src/visualization/vis_animation_settings.m
@@ -23,6 +23,8 @@
 %         ani_settings.z_lim_low_is_surface  : Use or not surface as lower limit for z-axis (string)
 %         ani_settings.camera_az             : Azimuth angle for view [deg] (scalar)
 %         ani_settings.camera_el             : Elevation angle for view [deg] (scalar)
+%         surface_param                      : Parameters for surface (class)
+%         time                          	 : Simulation time [s] (scalar)
 
 
 function vis_animation_settings(ani_settings, surface_param, time)

diff --git a/src/visualization/vis_com_projection.m b/src/visualization/vis_com_projection.m
@@ -1,14 +1,14 @@
 %%%%%% Visualize
 %%%%%% vis_com_projection
 %%%%%%
-%%%%%% Disp CoM projection point
+%%%%%% Display CoM projection point
 %%%%%%
 %%%%%% Created 2020-05-28
 %%%%%% Yusuke Koizumi
 %%%%%% Last update: 2020-05-28
 %
 %
-% Disp gracpable points
+% Display Center of Mass projection
 %
 % Function variables:
 %

diff --git a/src/visualization/vis_goal.m b/src/visualization/vis_goal.m
@@ -1,14 +1,14 @@
 %%%%%% Visualize
 %%%%%% vis_goal
 %%%%%%
-%%%%%% Disp goal
+%%%%%% Display goal
 %%%%%%
 %%%%%% Created 2020-05-28
 %%%%%% Yusuke Koizumi
 %%%%%% Last update: 2020-05-28
 %
 %
-% Disp goal
+% Display goal position
 %
 % Function variables:
 %

diff --git a/src/visualization/vis_graspable_points.m b/src/visualization/vis_graspable_points.m
@@ -1,14 +1,14 @@
 %%%%%% Visualize
 %%%%%% vis_graspable_points
 %%%%%%
-%%%%%% Disp graspable points
+%%%%%% Display graspable points
 %%%%%%
 %%%%%% Created 2020-05-14
 %%%%%% Yusuke Koizumi
 %%%%%% Last update: 2020-05-14
 %
 %
-% Disp graspable points
+% Display graspable points
 %
 % Function variables:
 %

diff --git a/src/visualization/vis_graspable_points_in_reachable_area.m b/src/visualization/vis_graspable_points_in_reachable_area.m
@@ -1,20 +1,21 @@
 %%%%%% Visualize
 %%%%%% vis_graspable_points_in_reachable_area
 %%%%%%
-%%%%%% Disp graspable points in reachable area
+%%%%%% Display graspable points in reachable area
 %%%%%%
 %%%%%% Created 2020-05-14
 %%%%%% Yusuke Koizumi
 %%%%%% Last update: 2020-05-14
 %
 %
-% Disp graspable points in reachable area
+% Display graspable points in reachable area
 %
 % Function variables:
 %
 %     OUTPUT
 %         -
 %     INPUT
+%         path_planning_param                                   : Parameters for Path Plannning (class)
 %         inc                                                   : Surface inclination [deg] (scalar)
 %         ani_settings.graspable_points_in_reachable_area_color : Color for points [RGB] (1x3 vector)
 %         ani_settings.graspable_points_in_reachable_area_marker: Type of marker (String)