diff --git a/README.md b/README.md index 248d628..defdcab 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # ClimbLab - Climbing robot Matlab simulator -![ex3_uneven_dynamic_Uno-gait-planning_stability_polyhedron.gif](./docs/media/ex3_uneven_dynamic_Uno-gait-planning_stability_polyhedron.gif) +![climblab.png](./docs/media/climblab.png) Author(s) and maintainer(s): [Space Robotics Lab.](http://www.astro.mech.tohoku.ac.jp/e/index.html) Climbing Robotics Team @@ -27,9 +27,20 @@ This simulator wraps up functions for: |--------|--------| | 3) Stability region visualization [2] | 4) Topographically salient region detection and non-periodic foothold selection [3] | -## Usage +## Publication -If you use this simulator in academic context, please put citations of the following publications. +If you use this simulator in an academic context, please cite the following publication. +> K. Uno, W. F. R. Ribeiro, Y. Koizumi, K. Haji, K. Kurihara, W. Jones, and K. Yoshida +> **"ClimbLab: MATLAB Simulation Platform for Legged Climbing Robotics"**, +> Proceedings of the 24th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines (CLAWAR), 2021. + + @inproceedings{uno2021climblab, + title={ClimbLab: MATLAB Simulation Platform for Legged Climbing Robotics}, + author={Kentaro Uno and Warley F. R. Ribeiro and Yusuke Koizumi and Keigo Haji and Koki Kurihara and William Jones and Kazuya Yoshida}, + booktitle={Proceedings of the 24th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines (CLAWAR)}, + pages={TBD}, + year={2021} + } #### Requirements We confirmed the code is working with: @@ -46,7 +57,9 @@ We confirmed the code is working with: - 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 +## References + +Our recent works using ClimbLab simulator are as follows: [1] Kentaro Uno *et al*., "[Gait Planning for a Free-Climbing Robot Based on Tumble Stability](https://ieeexplore.ieee.org/document/8700455)", Proceedings of the 2019 IEEE/SICE International Symposium on System Integration (SII), Paris, France, 2019, pp. 289-294, [doi: 10.1109/SII.2019.8700455](https://doi.org/10.1109/SII.2019.8700455). @@ -58,12 +71,16 @@ We confirmed the code is working with: Please contact us by sending an email to the following address if there is any question or suggestion. - limb[at]astro.mech.tohoku.ac.jp + srl-limb[at]grp.tohoku.ac.jp ## Release note * Version 1.0: released on 18th Sept. 2020. * Version 2.0: released on 26th Oct. 2020. This version includes: - - topographically salient regioon detection algorithm [3] - - non-periodic gait plannner [3] - - gripper detachment evaluation + - topographically salient region detection algorithm [3] + - non-periodic gait planner [3] + - gripper detachment evaluation +* Version 3.0: released on 27th Aug.2021. This version includes + - Camera sensing + - New robot models + - More analysis scripts diff --git a/config/USER/config_USER_param_template.m b/config/USER/config_USER_param_template.m index 48677da..c599ea8 100644 --- a/config/USER/config_USER_param_template.m +++ b/config/USER/config_USER_param_template.m @@ -7,7 +7,7 @@ %%%%%% %%%%%% Created 2020-07-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-10-01 +%%%%%% Last update: 2021-07-26 %%%%%% Keigo Haji % % @@ -18,7 +18,7 @@ % OUTPUT % robot_param % environment_param -% gait_param +% gait_planning_param % control_param % equilibrium_eval_param % ani_settings @@ -27,12 +27,13 @@ % gripper_param % map_param % matching_settings +% sensing_camera_param % INPUT % - -function [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings, gripper_param, map_param, matching_settings] = config_USER_param(robot_param, environment_param, gait_param, control_param, ... - equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings) +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_USER_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param) %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -44,7 +45,12 @@ %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - +if contains(robot_param.robot_type, 'HubRobo_v3') + %%% Controller Proportional (P) gain + control_param.kp = 10.0; + %%% Controller Derivative (D) gain + control_param.kd = 0.3; +end %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -67,4 +73,7 @@ %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Matching Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + end \ No newline at end of file diff --git a/config/config_all_default_param.m b/config/config_all_default_param.m new file mode 100644 index 0000000..97bc269 --- /dev/null +++ b/config/config_all_default_param.m @@ -0,0 +1,51 @@ +%%%%%% Configuration +%%%%%% config_all_default_param +%%%%%% +%%%%%% Define all default parameters +%%%%%% +%%%%%% Created 2021-03-02 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-04-24 +%%%%%% Kentaro Uno +% +% +% Define and load configurations for all default parameters. +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% gripper_param +% map_param +% matching_settings +% sensing_camera_param +% INPUT +% - +% +% +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_all_default_param() + + +% Load all default parameters +robot_param = config_robot_param(); +environment_param = config_environment_param(); +gait_planning_param = config_gait_planning_param(); +control_param = config_control_param(); +equilibrium_eval_param = config_equilibrium_param(); +ani_settings = config_animation_settings(); +save_settings = config_save_settings(environment_param); +plot_settings = config_plot_settings(); +[gripper_param, map_param, matching_settings] = config_target_detection_param(); +sensing_camera_param = config_sensing_camera_param(); + + +end + diff --git a/config/config_gait_param.m b/config/config_gait_param.m deleted file mode 100644 index 899e82f..0000000 --- a/config/config_gait_param.m +++ /dev/null @@ -1,44 +0,0 @@ -%%%%%% Configuration -%%%%%% config_gait_param -%%%%%% -%%%%%% Configure default gait parameters -%%%%%% -%%%%%% Created 2020-07-08 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-08 -% -% -% Load default configurations for gait methods -% -% Function variables: -% -% OUTPUT -% gait_param -% INPUT -% - - -function gait_param = config_gait_param() -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver') -gait_param.type = 'crawl_fixed_stride'; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Crawl gait settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Period of one cycle [s] -gait_param.T = 4; -%%% Duty cycle -gait_param.beta = 0.75; -%%% Horizontal distance per step (stride) [m] -gait_param.step_length = 0.05; -%%% Step height [m] -gait_param.step_height = 0.03; -%%% Walking sequence: 1 - 3 - 4 - 2 -gait_param.sequence = [1; 3; 4; 2]; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Uno gait settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Goal position [m] -gait_param.goal = [0.4;0;-0.08]; - -end \ No newline at end of file diff --git a/config/config_save_settings.m b/config/config_save_settings.m deleted file mode 100644 index d6acd95..0000000 --- a/config/config_save_settings.m +++ /dev/null @@ -1,28 +0,0 @@ -%%%%%% Configuration -%%%%%% config_save_settings -%%%%%% -%%%%%% Configure default saving settings -%%%%%% -%%%%%% Created 2020-07-08 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-08 -% -% -% Load default configurations for saving -% -% Function variables: -% -% OUTPUT -% save_settings -% INPUT -% - - -function save_settings = config_save_settings() - -%%% Save basic variables to csv file on/off -save_settings.csv_file = 'on'; - -%%% Save TSM variables to csv file on/off -save_settings.tsm = 'off'; -%%% Save variables to csv file on/off -save_settings.gia = 'off'; diff --git a/config/config_simulation.m b/config/config_simulation.m index 4821b6f..d97b030 100644 --- a/config/config_simulation.m +++ b/config/config_simulation.m @@ -5,8 +5,8 @@ %%%%%% %%%%%% Created 2020-07-09 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-10-15 -%%%%%% Keentaro Uno +%%%%%% Last update: 2021-07-08 +%%%%%% Keigo Haji % % % Load configurations for simulation @@ -16,7 +16,7 @@ % OUTPUT % robot_param % environment_param -% gait_param +% gait_planning_param % control_param % equilibrium_eval_param % ani_settings @@ -25,48 +25,83 @@ % gripper_param % map_param % matching_settings +% sensing_camera_param % INPUT % config -function [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings, gripper_param, map_param, matching_settings] = config_simulation(config) +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_simulation(config) %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -robot_param = config_robot_param(); -environment_param = config_environment_param(); -gait_param = config_gait_param(); -control_param = config_control_param(); -equilibrium_eval_param = config_equilibrium_param(); +% Load all default parameters +[robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_all_default_param(); -ani_settings = config_animation_settings(); -save_settings = config_save_settings(); -plot_settings = config_plot_settings(); -[gripper_param, map_param, matching_settings] = config_target_detection_param(); +% Reads the config file with the specified config name and overwrites the variables if strcmp(config,'USER') - [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings, gripper_param, map_param, matching_settings] = config_USER_param(robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ... - ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings); + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_USER_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... + ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param); +end +if strcmp(config,'example_demo_1') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_example_demo_1_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param); +end +if strcmp(config,'example_demo_2') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_example_demo_2_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param); +end +if strcmp(config,'example_demo_3') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_example_demo_3_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... + ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param); end if strcmp(config,'gia_static') - [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings] = config_gia_static(robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ... + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_gia_static_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... ani_settings, save_settings, plot_settings); end -if strcmp(config,'uno_crawl_param') - [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings] = config_uno_crawl_param(robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ... +if strcmp(config,'clawar_2021_dynamic_climbing_demo') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_clawar_2021_dynamic_climbing_demo_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... ani_settings, save_settings, plot_settings); end -if strcmp(config,'nonperiodic_demo_param') - [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings] = config_nonperiodic_demo_param(robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ... +if strcmp(config,'crawl_gait_for_discrete_footholds') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_crawl_gait_for_discrete_footholds_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... ani_settings, save_settings, plot_settings); end -if strcmp(config,'iSAIRAS_2020_demo_param') - [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings, gripper_param, map_param, matching_settings] = config_iSAIRAS_2020_demo_param(robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ... +if strcmp(config,'nonperiodic_gait_for_discrete_footholds') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_nonperiodic_gait_for_discrete_footholds_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... + ani_settings, save_settings, plot_settings); +end +if strcmp(config,'iSAIRAS_2020_demo') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_iSAIRAS_2020_demo_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... + ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param); +end +if strcmp(config,'base_pos_opt_quasistatic') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings] = config_base_pos_opt_quasistatic_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings); end - +if strcmp(config,'clawar_2021_dynamic_climbing_demo') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_clawar_2021_dynamic_climbing_demo_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... + ani_settings, save_settings, plot_settings); +end +if strcmp(config,'clawar_2021_statistical_analysis') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_clawar_2021_statistical_analysis_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... + ani_settings, save_settings, plot_settings); +end +if strcmp(config,'hubrobo_testfield_exp') + [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_hubrobo_testfield_exp_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param); +end end \ No newline at end of file diff --git a/config/config_animation_settings.m b/config/default/config_animation_settings.m similarity index 51% rename from config/config_animation_settings.m rename to config/default/config_animation_settings.m index 3e89616..62559b6 100644 --- a/config/config_animation_settings.m +++ b/config/default/config_animation_settings.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-07-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-08 +%%%%%% Last update: 2021-07-06 +%%%%%% Keigo Haji % % % Load default configurations for saving @@ -24,12 +25,15 @@ ani_settings.display = 'on'; %%% Save animation ani_settings.save = 'off'; +%%% elapsed time show on/off +ani_settings.animation_elapsed_time_show = 'on'; %%% Frame rate [frames/s] [20,25,50] ani_settings.frame_rate = 20; %%% Animation video resolution [px] [[1280 720], [640 480]] ani_settings.animation_resolution = [640 480]; + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] ani_settings.x_lim = [-0.25, 0.4]; @@ -63,12 +67,22 @@ %%% Link radius [m] ani_settings.link_radius = 0.012; %%% Base thickness [m] -ani_settings.base_thickness = 0.03; - +ani_settings.base_upper_thickness = 0.03; +ani_settings.base_lower_thickness = 0.03; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.0; %%% Robot color -ani_settings.robot_color = [0.2, 0.2, 0]; +ani_settings.robot_base_upper_color = [0.2, 0.2, 0]; +ani_settings.robot_base_lower_color = [0.2, 0.2, 0]; +ani_settings.robot_limb_color = [0.2, 0.2, 0]; %%% Robot transparency -ani_settings.robot_alpha = 0.8; +ani_settings.robot_base_alpha = 0.8; +ani_settings.robot_limb_alpha = 0.8; + +%%% Frames at CoM and each joint on/off +ani_settings.frames_show = 'off'; + ani_settings.frames_line_width = 2.0; + ani_settings.frames_size = 0.05; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Surface viz. on/off @@ -82,10 +96,12 @@ ani_settings.graspable_points_show = 'off'; %%% Graspable points color ani_settings.graspable_points_color = [0, 0, 1.0]; + %%% Graspable points alpha + ani_settings.graspable_points_alpha = 1.0; %%% Graspable points marker - ani_settings.graspable_points_marker = '.'; + ani_settings.graspable_points_marker = 'o'; %%% Graspable points size - ani_settings.graspable_points_size = 5; + ani_settings.graspable_points_size = 10; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -115,7 +131,7 @@ ani_settings.goal_size = 25; %%% Goal vis. height from the ground ani_settings.goal_vis_height = 0.0; - + %%% Next desired position vis. on/off ani_settings.next_desired_position_show = 'off'; %%% Next desired position line color m : magenta @@ -123,7 +139,7 @@ %%% Next desired position line width ani_settings.next_desired_position_line_width = 1; -%%% Plot trajectory +%%% Plot Limb trajectory ani_settings.trajectory_show = 'on'; %%% Trajectory line color ani_settings.trajectory_color = 'c'; @@ -131,43 +147,84 @@ ani_settings.trajectory_width = 3; %%% Trajectory line type ani_settings.trajectory_line_type = ':'; - + +%%% Plot CoM trajectory +ani_settings.com_proj_trajectory_show = 'off'; + %%% Trajectory line color + ani_settings.com_proj_trajectory_color = 'k'; + %%% Trajectory line widith + ani_settings.com_proj_trajectory_width = 2; + %%% Trajectory line type + ani_settings.com_proj_trajectory_line_type = '-'; + + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Support triangle visualization ani_settings.support_triangle_show = 'off'; +%%% Support triangle above the robot visualization + ani_settings.robot_top_support_triangle_show = 'off'; %%% Support triangle color - ani_settings.support_triangle_color = [0 1.0 1.0]; + ani_settings.support_triangle_color = [0 136/255 170/255]; %%% Support triangle edge line color ani_settings.support_triangle_edge_color = [0.0 0.0 0.0]; %%% Support triangle transparency ani_settings.support_triangle_alpha = 0.5; -%%% Stability polyhedron face color -ani_settings.polyh_Face_color = [0 0 1]; -%%% Stability polyhedron face alpha -ani_settings.polyh_Face_alpha = 0.25; -%%% Stability polyhedron edge color -ani_settings.polyh_Edge_color = [0 0 1]; -%%% Stability polyhedron edge width -ani_settings.polyh_Edge_width = 2; - -%%% Gravity vector visualize on/off -ani_settings.gravity_vec_show = 'off'; - %%% Gravity vector color - ani_settings.gravity_vec_color = [0 0.7 0]; - %%% Gravity vector width - ani_settings.gravity_vec_width = 3; - -%%% GIA vector visualize on/off -ani_settings.gia_vec_show = 'off'; - %%% GIA vector color - ani_settings.gia_vec_color = [1 0 0]; - %%% GIA vector width - ani_settings.gia_vec_width = 3; +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + + %%% GIA Stable Region visualize on/off (requires equilibrium evaluation method as 'gia') + ani_settings.gia_stable_region_show = 'off'; + %%% GIA Stable Region face color + ani_settings.gia_stable_region_Face_color = [0 0 1]; + %%% GIA Stable Region face alpha + ani_settings.gia_stable_region_Face_alpha = 0.25; + %%% GIA Stable Region edge color + ani_settings.gia_stable_region_Edge_color = [0 0 1]; + %%% GIA Stable Region edge width + ani_settings.gia_stable_region_Edge_width = 2; + + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'off'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [0 0.7 0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 3; + + %%% GIA vector visualize on/off + ani_settings.gia_vec_show = 'off'; + %%% GIA vector color + ani_settings.gia_vec_color = [1 0 0]; + %%% GIA vector width + ani_settings.gia_vec_width = 3; + +%%% Transformation from force to visualize Fg (gravitational force) and +%%% Fe (reaction force) Stable Region in the position coordinate +%%% -> this scale is used for all force-dimensional variables visualization +ani_settings.force_expansion_factor = 0.04; + + %%% Gravitational Force vector visualize on/off + ani_settings.gravitational_force_vec_show = 'off'; + %%% Gravity vector color + ani_settings.gravitational_force_vec_color = [0 0.7 0]; + %%% Gravity vector width + ani_settings.gravitational_force_vec_width = 3; + + %%% Reaction force vectors vis. on/off + ani_settings.reaction_force_show = 'off'; + %%% Reaction force vectors color + ani_settings.reaction_force_vec_color = [1 0 0]; + %%% Reaction force vectors color width + ani_settings.reaction_force_vec_width = 3; %%% CoM projection point vis. on/off ani_settings.com_projection_show = 'off'; +%%% CoM projection point above the robot vis. on/off +ani_settings.robot_top_com_projection_show = 'off'; %%% CoM projection point color ani_settings.com_projection_color = [1 0 0]; %%% CoM projection point marker @@ -176,5 +233,35 @@ ani_settings.com_projection_size = 25; %%% CoM projection point vis. height from the ground ani_settings.com_projection_vis_height = 0.0; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% RealSense Camera fov vis. on/off +ani_settings.sensing_fov_show = 'off'; + %%% Sensing camera position marker size + ani_settings.sensing_camera_MarkerSize = 1; + %%% Sensing camera marker color + ani_settings.sensing_camera_MarkerColor = '.k'; + %%% Sensing camera fov line width + ani_settings.sensing_camera_fov_LineWidth = 1; + %%% Sensing camera fov line color + ani_settings.sensing_camera_fov_LineColor = 'k'; +%%% RealSense Camera fov filling vis. on/off +ani_settings.sensing_fov_face_filling = 'off'; + %%% Filling color + ani_settings.sensing_fov_face_color = 'c'; + %%% Filling color alpha + ani_settings.sensing_fov_face_alpha = 0.5; +%%% Sensed Graspable points viz. on/off +ani_settings.sensed_graspable_points_show = 'off'; + %%% Sensed Graspable points color + ani_settings.sensed_graspable_points_color = [1.0, 0, 1.0]; + %%% Sensed Graspable points alpha + ani_settings.sensed_graspable_points_alpha = 1.0; + %%% Sensed Graspable points marker + ani_settings.sensed_graspable_points_marker = 'o'; + %%% Sensed Graspable points size + ani_settings.sensed_graspable_points_size = 10; + end \ No newline at end of file diff --git a/config/config_control_param.m b/config/default/config_control_param.m similarity index 100% rename from config/config_control_param.m rename to config/default/config_control_param.m diff --git a/config/config_environment_param.m b/config/default/config_environment_param.m similarity index 60% rename from config/config_environment_param.m rename to config/default/config_environment_param.m index dde4a2b..69bc0c1 100644 --- a/config/config_environment_param.m +++ b/config/default/config_environment_param.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-07-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-08 +%%%%%% Last update: 2021-05-04 +%%%%%% Keigo Haji % % % Load default configurations for environments @@ -28,9 +29,28 @@ %%% Dynamics on/off environment_param.dynamics_flag = 'on'; -%%% Gripper detachment method ('none', 'max_holding_force') +%%% Gripper detachment method ('none', 'tsm', 'max_holding_force') environment_param.detachment_detection_method = 'none'; +%%% Simulation stop +%%% Simulation stop reaching goal when all limbs are grasping +environment_param.sim_stop_goal_grasping = 'on'; +%%% Simulation stop reaching goal under any condition +environment_param.sim_stop_goal_any = 'off'; +%%% Simulation stop reaching goal threshold +environment_param.sim_stop_goal_threshold = 0.03; +%%% Simulation stop if robot is stuck (selecting same grasping point) +environment_param.sim_stop_stuck = 'off'; +%%% Simulation stop stuck threshold +environment_param.sim_stop_stuck_threshold = 0.005; +%%% Simulation stop stuck search range threshold +environment_param.sim_stop_stuck_serach_range_threshold = 4; +%%% Simulation stop for singular configuration +environment_param.sim_stop_singular = 'on'; +%%% Simulation stop for joint outside limit +environment_param.sim_stop_joint_limit = 'on'; +%%% Simulation stop for maximum time +environment_param.sim_stop_time = 'on'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Time settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Time-step [s] diff --git a/config/config_equilibrium_param.m b/config/default/config_equilibrium_param.m similarity index 54% rename from config/config_equilibrium_param.m rename to config/default/config_equilibrium_param.m index d0d8b55..e78a611 100644 --- a/config/config_equilibrium_param.m +++ b/config/default/config_equilibrium_param.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-07-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-08 +%%%%%% Last update: 2020-02-22 +%%%%%% Kentaro Uno % % % Load default configurations for equilibrium evaluation methods @@ -20,14 +21,7 @@ function equilibrium_eval_param = config_equilibrium_param() %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Equilibrium evaluation method ('none', 'tsm', 'gia') +%%% Equilibrium evaluation method ('none', 'tsm', 'gia', 'tsm_and_gia') equilibrium_eval_param.type = 'tsm'; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GIA polyhedron settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Plot polyhderon on animation on/off (requires equilibrium evaluation method as 'gia') -equilibrium_eval_param.plot_polyhedron = 'off'; -% Transformation from acceleration to plot in the position coordinate -equilibrium_eval_param.expansion_factor = 0.02; end \ No newline at end of file diff --git a/config/default/config_gait_planning_param.m b/config/default/config_gait_planning_param.m new file mode 100644 index 0000000..319d903 --- /dev/null +++ b/config/default/config_gait_planning_param.m @@ -0,0 +1,60 @@ +%%%%%% Configuration +%%%%%% config_gait_planning_param +%%%%%% +%%%%%% Configure default gait parameters +%%%%%% +%%%%%% Created 2020-07-08 +%%%%%% Warley Ribeiro +%%%%%% Last update: 2021-07-07 +%%%%%% Keigo Haji +% +% +% Load default configurations for gait methods +% +% Function variables: +% +% OUTPUT +% gait_planning_param +% INPUT +% - + +function gait_planning_param = config_gait_planning_param() +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Gait type ('do_nothing', 'periodic_crawl_gait,' 'crawl_fixed_stride','crawl_gait_for_discrete_footholds','nonperiodic_gait_for_discrete_footholds','GIA_opt_based_pose_planner') +gait_planning_param.type = 'crawl_fixed_stride'; + + +%%% Goal position [m] (3*N matrix) +gait_planning_param.goal(:,1) = [0.4;0;0]; + +%%% Kinematics feasiblity check switch +gait_planning_param.feasibility_check_excluding_same_graspable_point_switch = 'on'; +gait_planning_param.feasibility_check_number_of_legs = 5; % 4 or 5 + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Crawl gait settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Period of one cycle [s] +gait_planning_param.T = 4; +%%% Duty cycle +gait_planning_param.beta = 0.75; +%%% Horizontal distance per step (stride) [m] +gait_planning_param.step_length = 0.05; +%%% Step height [m] +gait_planning_param.step_height = 0.03; +%%% Walking sequence: +% If the gait_type is set as 'crawl' and the walking sequense is not +% defined in your config file such as preset or USER, the walking sequence +% is set [2; 1; 3; 4] as default in ini_gait.m. +gait_planning_param.sequence = []; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% gait for discrete footholds settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Allowable maximum stride [m] -- If you set this too large, it might +%%% not able to pass the kinematic feasibility. If you already limit the +%%% allowable stride, it helps with saving the computational time. +gait_planning_param.allowable_max_stride = 0.1; + + +end \ No newline at end of file diff --git a/config/config_plot_settings.m b/config/default/config_plot_settings.m similarity index 67% rename from config/config_plot_settings.m rename to config/default/config_plot_settings.m index dcad703..b9be78c 100644 --- a/config/config_plot_settings.m +++ b/config/default/config_plot_settings.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-07-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-09-08 +%%%%%% Last update: 2021-07-06 +%%%%%% Keigo Haji % % % Load default configurations for plots @@ -76,18 +77,37 @@ plot_settings.footholds = 'off'; plot_settings.footholds_fig_number = 101; + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% 201 ~ 300 -%%% Plot TSM +%%% Plot TSM (To turn ON, requires equilibrium evaluation method as 'tsm') plot_settings.tsm = 'off'; plot_settings.tsm_fig_number = 201; -%%% Plot Acceleration margin -plot_settings.acc_margin = 'off'; -plot_settings.acc_margin_fig_number = 202; +%%% Plot GIA (Acceleration) margin (To turn ON, requires equilibrium evaluation method as 'gia') +plot_settings.gia_margin = 'off'; +plot_settings.gia_margin_fig_number = 202; + +%%% Plot GIA Inclination margin (To turn ON, requires equilibrium evaluation method as 'gia') +plot_settings.gia_inclination_margin = 'off'; +plot_settings.gia_inclination_margin_fig_number = 203; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Evaluation related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% 301 ~ 400 +%%% Plot manipulability +plot_settings.manipulability = 'off'; +plot_settings.manipulability_fig_number = 301; +plot_settings.mean_manipulability = 'off'; +plot_settings.mean_manipulability_fig_number = 302; +plot_settings.min_manipulability = 'off'; +plot_settings.min_manipulability_fig_number = 303; + +%%% Plot Max absolute torque of all joint +plot_settings.joint_max_torque = 'off'; +plot_settings.joint_max_torque_fig_number = 310; + +%%% Plot RMS torque of all joint +plot_settings.joint_rms_torque = 'off'; +plot_settings.joint_rms_torque_fig_number = 311; -%%% Plot Inclination margin -plot_settings.inclination_margin = 'off'; -plot_settings.inclination_margin_fig_number = 203; - end \ No newline at end of file diff --git a/config/config_robot_param.m b/config/default/config_robot_param.m similarity index 51% rename from config/config_robot_param.m rename to config/default/config_robot_param.m index c41d292..fb9bbd4 100644 --- a/config/config_robot_param.m +++ b/config/default/config_robot_param.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-07-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-08 +%%%%%% Last update: 2021-06-02 +%%%%%% Keigo Haji % % % Load default configurations for robot @@ -20,16 +21,24 @@ function robot_param = config_robot_param() %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') -robot_param.robot_type = 'HubRobo_v2_2_grip_to_palm'; +%%% Type of the robot to simulate ('HubRobo_grip_to_spine_old', +%%% 'HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_spine', +%%% 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', +%%% 'HubRobo_v3_2_grip_to_palm', 'ANYmal_B', 'Climbing_ANYmal','ALPHRED' +robot_param.robot_type = 'HubRobo_v3_2_grip_to_palm'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Position settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% x and y position of legs relative to base center [m] -robot_param.foot_dist = 0.12; +robot_param.x_foot_dist = 0.14; +robot_param.y_foot_dist = 0.14; +%%% Initial x and y "offset" position of the base from the neutral position of the base center [m] +robot_param.x_base_pos_offset_from_the_neutral_pos = 0.0; +robot_param.y_base_pos_offset_from_the_neutral_pos = 0.0; %%% Height of base relative to map [m] -robot_param.base_height = 0.12; +robot_param.base_height = 0.11; %%% Base position [m] 2x1 vector. or 'default' for default setting robot_param.base_pos_xy = [0;0]; - +%%% Base yaw orientation [deg] +robot_param.initial_yaw = 0; end \ No newline at end of file diff --git a/config/default/config_save_settings.m b/config/default/config_save_settings.m new file mode 100644 index 0000000..1003332 --- /dev/null +++ b/config/default/config_save_settings.m @@ -0,0 +1,45 @@ +%%%%%% Configuration +%%%%%% config_save_settings +%%%%%% +%%%%%% Configure default saving settings +%%%%%% +%%%%%% Created 2020-07-08 +%%%%%% Warley Ribeiro +%%%%%% Last update: 2021-07-06 +%%%%%% Keigo Haji +% +% +% Load default configurations for saving +% +% Function variables: +% +% OUTPUT +% save_settings +% INPUT +% - + +function save_settings = config_save_settings(environment_param) + +%%% Time interval for saving variables (should be larger than time-step) +save_settings.variable_saving_time_interval = environment_param.time_step; + +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; + +%%% Save the loaded config file in the dat folder +save_settings.config_file = 'on'; + +%%% Save TSM variables to csv file on/off +save_settings.tsm = 'off'; +%%% Save variables to csv file on/off +save_settings.gia = 'off'; +%%% Save manipulability to csv file on/off +save_settings.manipulability = 'off'; + %%% Save mean manipulability of all limbs to csv file on/off + save_settings.mean_manipulability = 'off'; + %%% Save minimum manipulability of all limbs to csv file on/off + save_settings.min_manipulability = 'off'; +%%% Save Maximum of absolute torque of all joint +save_settings.joint_max_torque = 'off'; +%%% Save Root Mean Square (RMS) of torque of all joint (*need Signal Processing Toolbox) +save_settings.joint_rms_torque = 'off'; diff --git a/config/default/config_sensing_camera_param.m b/config/default/config_sensing_camera_param.m new file mode 100644 index 0000000..3c3b735 --- /dev/null +++ b/config/default/config_sensing_camera_param.m @@ -0,0 +1,55 @@ +%%%%%% Configuration +%%%%%% config_sensing_camera_param +%%%%%% +%%%%%% Configure default RealSense Camera parameters +%%%%%% +%%%%%% Created 2021-02-10 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-02-25 +%%%%%% Keigo Haji +% +% +% Load default configurations for a sensing camera +% +% Function variables: +% +% OUTPUT +% sensing_camera_param +% INPUT +% - + +function sensing_camera_param = config_sensing_camera_param() +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Sensing Camera on/off +sensing_camera_param.sensing_flag = 'off'; +%%% Type of Sensing +sensing_camera_param.sensing_type = 'RealSense_d435i'; %'RealSense_d435i' or 'circle' + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Position settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Following Values are based on the RealSense D435i on HubRobo base. +sensing_camera_param.mounting_angle = [0; -45; 0]*pi/180; % rpy +sensing_camera_param.mounting_position = [0.079; -0.011 ; 0.07]; %[m] Mesured by CAD of Hubrobo v3.2 + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% fov settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Following Values are written in RealSense D435i datasheet. +sensing_camera_param.fov_horizontal = 86*pi/180; %[radian] +sensing_camera_param.fov_vertical = 57*pi/180; %[radian] +sensing_camera_param.fov_max_distance = 2; %[m] +sensing_camera_param.fov_min_distance = 0.28; %[m] + +%%% Following Values are set for circular sensing. +sensing_camera_param.circular_radius_from_base_pos = 0.4; %Radius to be sensed from the base center of the robot + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initial Known area settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Known area shape +sensing_camera_param.known_area_shape = 'rectangle'; %'rectangle' or 'circle' + +%%% Rectangle Param + % Set the range of acquisition in the +x and +-y directions from the initial robot position. + sensing_camera_param.ini_margin_from_base_pos_to_plus_x = 0.4; %[m] + sensing_camera_param.ini_margin_from_base_pos_to_y = 0.4; %[m] + +%%% Circle Param +sensing_camera_param.ini_circular_radius_from_base_pos = 0.4; %[m] + +end \ No newline at end of file diff --git a/config/preset/config_anymal_climb_test_param.m b/config/preset/config_anymal_climb_test_param.m new file mode 100644 index 0000000..bdb6004 --- /dev/null +++ b/config/preset/config_anymal_climb_test_param.m @@ -0,0 +1,230 @@ +%%%%%% Configuration +%%%%%% config_anymal_climb_test +%%%%%% +%%%%%% Define preset parameters +%%%%%% +%%%%%% Created 2021-01-18 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-02-22 +%%%%%% Kentaro Uno +% +% +% Load configurations for example of ANYmal robot climbing +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_anymal_climb_test_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') +robot_param.robot_type = 'Climbing_ANYmal'; + +% Nomally, the following four items should be defined in the LP file, +% but it is also possible to specify the joint allocation type and offset +% angles (theta1 and 2) for the hip joints here for more convenience. + +% if you set "insect" for the following type, theta1 and 2 are forced to be pi/4 and -pi/2 [rad], +% respectively. +robot_param.joint_allocation_type = 'insect'; % this can also be set in ini_LP file +% for the mammalian robot, you can specify the leg configuration from "oo", "xx", and "M". +robot_param.leg_config_type = 'xx'; % this can also be set in LP file +robot_param.theta_1 = 0.1440; % [rad] +% note that if you put the negative num for theta_2, the IK solver take automatically +% 'oo' configuration solution +robot_param.theta_2 = -0.1955; % [rad] + +%%% "Initial" x and y position of legs relative to base center [m] +robot_param.x_foot_dist = 0.6; +robot_param.y_foot_dist = 0.3; +%%% Height of base relative to map [m] +robot_param.base_height = 0.2; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Type of the surface ('flat_HR', 'flat_HR_5m_x_5m', 'rough', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') +environment_param.surface_type = 'flat_HR_5m_x_5m'; +%%% Gravity [G] +environment_param.grav = 1; + +%%% Dynamics on/off +environment_param.dynamics_flag = 'on'; +%%% Maximum simulation time [s] +environment_param.time_max = 2; + +%%% Surface Inclination [deg] +environment_param.inc = 0; + +% Floor reaction force stiffness coefficient +environment_param.surface_K = 100000; +% Floor reaction force damping coefficient +environment_param.surface_D = 100.0; + +%%% Gripper detachment method ('none', 'max_holding_force') +environment_param.detachment_detection_method = 'max_holding_force'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds','nonperiodic_gait_for_discrete_footholds' ) +gait_planning_param.type = 'crawl_fixed_stride'; + +%%% Horizontal distance per step (stride) [m] +gait_planning_param.step_length = 0.1; +%%% Step height [m] +gait_planning_param.step_height = 0.1; +%%% Period of one cycle [s] +gait_planning_param.T = 8; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.2; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller Proportional (P) gain +control_param.kp = 2000.0; +%%% Controller Derivative (D) gain +control_param.kd = 4.5; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia', 'tsm_and_gia') +equilibrium_eval_param.type = 'tsm_and_gia'; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] +ani_settings.x_lim = [-1.0, 1.0]; +%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] +ani_settings.y_lim = [-1.0, 1.0]; +%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] +ani_settings.z_lim = [-0.3, 1.0]; +%%% Lower z-axis limit same as lower point on surface on/off +ani_settings.z_lim_low_is_surface = 'off'; +%%% 'on' camera follows robot, 'off' camera fixed +ani_settings.move_camera = 'off'; +%%% Camera angle azimuth and elevation [deg] +ani_settings.camera_az =-20; +ani_settings.camera_el = 15; + +%%% Font name +ani_settings.font_name = 'Times New Roman'; +%%% Font size +ani_settings.font_size = 25; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Link radius [m] +ani_settings.link_radius = 0.030; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.20; +ani_settings.base_lower_thickness = 0.05; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.0; +%%% Robot color +ani_settings.robot_base_upper_color = [0.3, 0.3, 0.3]; +ani_settings.robot_base_lower_color = [0.0, 0.0, 0.0]; +ani_settings.robot_limb_color = [0.3, 0.3, 0.3]; +%%% Robot transparency +ani_settings.robot_base_alpha = 0.5; +ani_settings.robot_limb_alpha = 0.5; + +%%% Frames at each joint on/off +ani_settings.frames_show = 'on'; + ani_settings.frames_line_width = 2.0; + ani_settings.frames_size = 0.1; + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + + %%% GIA Stable Region visualize on/off (requires equilibrium evaluation method as 'gia') + ani_settings.gia_stable_region_show = 'on'; + %%% Stability polyhedron face color + ani_settings.polyh_Face_color = [0 0 1]; + %%% Stability polyhedron face alpha + ani_settings.polyh_Face_alpha = 0.25; + %%% Stability polyhedron edge color + ani_settings.polyh_Edge_color = [0 0 1]; + %%% Stability polyhedron edge width + ani_settings.polyh_Edge_width = 2; + + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'off'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [0 0.7 0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 3; + + %%% GIA vector visualize on/off + ani_settings.gia_vec_show = 'off'; + %%% GIA vector color + ani_settings.gia_vec_color = [1 0 0]; + %%% GIA vector width + ani_settings.gia_vec_width = 3; + +%%% Transformation from force to visualize Fg (gravitational force) and +%%% Fe (reaction force) Stable Region in the position coordinate +%%% -> this scale is used for all force-dimensional variables visualization +ani_settings.force_expansion_factor = 0.005; + + %%% Gravitational Force vector visualize on/off + ani_settings.gravitational_force_vec_show = 'on'; + %%% Gravity vector color + ani_settings.gravitational_force_vec_color = [0 0.7 0]; + %%% Gravity vector width + ani_settings.gravitational_force_vec_width = 3; + + %%% Reaction force vectors vis. on/off + ani_settings.reaction_force_show = 'on'; + %%% Reaction force vectors color + ani_settings.reaction_force_vec_color = [1 0 0]; + %%% Reaction force vectors color width + ani_settings.reaction_force_vec_width = 2; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Plot trajectory +ani_settings.trajectory_show = 'off'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; +%%% Save TSM variables to csv file on/off +save_settings.tsm = 'on'; +% %%% Save variables to csv file on/off +save_settings.gia = 'on'; +%%% Save manipulability to csv file on/off +save_settings.manipulability = 'on'; +%%% Save Maximum of absolute torque of all joint +save_settings.joint_max_torque = 'on'; +%%% Save Root Mean Square (RMS of torque of all joint (*need Signal Processing Toolbox) +save_settings.joint_rms_torque = 'on'; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Plot Base position +plot_settings.base_pos = 'off'; +%%% Plot footholds history on/off +plot_settings.footholds = 'on'; +%%% Plot TSM +plot_settings.tsm = 'on'; +%%% Plot Gravito-Inertial Acceleration margin +plot_settings.gia_margin = 'on'; +%%% Plot manipulability +plot_settings.manipulability = 'on'; +%%% Plot max torque of all joint +plot_settings.joint_max_torque = 'on'; +%%% Plot RMS torque of all joint +plot_settings.joint_rms_torque = 'on'; + +end \ No newline at end of file diff --git a/config/preset/config_base_pos_opt_quasistatic_param.m b/config/preset/config_base_pos_opt_quasistatic_param.m new file mode 100644 index 0000000..a7c0def --- /dev/null +++ b/config/preset/config_base_pos_opt_quasistatic_param.m @@ -0,0 +1,177 @@ +%%%%%% Configuration +%%%%%% config_base_pos_opt_quasistatic_param +%%%%%% +%%%%%% Define config_base_pos_opt_quasistatic_param parameters +%%%%%% +%%%%%% Created 2020-07-09 +%%%%%% Yusuke Koizumi +%%%%%% Last update: 2021-03-24 +%%%%%% Keigo Haji +% +% +% Load the parameters of the quasistatic simulation to optimize the base +% pose based on the GIA of the robot and the manipulability of the legs. +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings] = config_base_pos_opt_quasistatic_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Type of the robot to simulate ('HubRobo_no_grip', 'HubRobo_grip_to_palm', 'HubRobo_grip_to_spine', 'HubRobo_grip_to_spine_old') +robot_param.robot_type = 'HubRobo_v3_2_grip_to_palm'; +%%% x and y position of legs relative to base center [m] +robot_param.x_foot_dist = 0.15; +robot_param.y_foot_dist = 0.15; + +%%% Height of base relative to map [m] +if strcmp(robot_param.robot_type,'HubRobo_no_grip') + robot_param.base_height = 0.10; +end +if strcmp(robot_param.robot_type,'HubRobo_grip_to_spine_old') + robot_param.base_height = 0.14; +end +if strcmp(robot_param.robot_type,'HubRobo_v3_2_grip_to_palm') + robot_param.base_height = 0.1; +end +%%% 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 = 4;%500*N; +%%% Graspable points detection type +environment_param.graspable_points_detection_type = 0; +environment_param.inc = 0; +environment_param.dynamics_flag = 'no_acc'; +% environment_param.grav = 1; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds','nonperiodic_gait_for_discrete_footholds','GIA_opt_based_pose_planner') +gait_planning_param.type = 'GIA_opt_based_pose_planner'; +gait_planning_param.step_height = 0.04; + +gait_planning_param.T = 4; +% gait_planning_param.goal = [0.7;0;0.175]; +% gait_planning_param.goal = [0.36;0;0.5436]; +gait_planning_param.goal = [0.6;0;0.2]; + +% [a,b,c] = get_map_pos(gait_planning_param.goal(1),gait_planning_param.goal(2)); +% gait_planning_param.goal = [a;b;c]; + +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.1; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +if strcmp(robot_param.robot_type, 'HubRobo_v3_1_grip_to_palm') + control_param.kp = 10.0; + control_param.kd = 0.3; +end +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia') +equilibrium_eval_param.type = 'gia'; +if strcmp(equilibrium_eval_param.type,'gia') + ani_settings.gia_stable_region_show = 'on'; + plot_settings.gia = 'on'; +else + ani_settings.gia_stable_region_show = 'off'; + plot_settings.gia = 'off'; +end +ani_settings.gravitational_acceleration_vec_show ='on'; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Link radius [m] +ani_settings.link_radius = 0.0185; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.07; +ani_settings.base_lower_thickness = 0.03; +%%% Robot color +ani_settings.robot_base_upper_color = [0.1, 0.1, 0.1]; +ani_settings.robot_base_lower_color = [0.1, 0.1, 0.1]; +ani_settings.robot_limb_color = [0.1, 0.1, 0.1]; +%%% Robot transparency +ani_settings.robot_base_alpha = 0.5; +ani_settings.robot_limb_alpha = 0.5; + +%%% Graspable points viz. on/off +ani_settings.graspable_points_show = 'on'; +%%% Graspable points size +ani_settings.graspable_points_size = 40; +%%% Graspable points in reachable area viz. on/off +ani_settings.graspable_points_in_reachable_area_show = 'on'; +%%% Reachable area viz. on/off +ani_settings.reachable_area_show = 'on'; +%%% Goal vis. on/off +ani_settings.goal_show = 'on'; +%%% Next desired position vis. on/off +ani_settings.next_desired_position_show = 'on'; +%%% Support triangle visualization +ani_settings.support_triangle_show = 'on'; +%%% CoM projection point vis. on/off +ani_settings.com_projection_show = 'on'; + +% ani_settings.camera_az=-90; +% ani_settings.camera_el = 90; +if ~strcmp(gait_planning_param.type,'crawl_uno_ver') && ~strcmp(gait_planning_param.type,'koizumi') + ani_settings.graspable_points_show = 'off'; + ani_settings.graspable_points_in_reachable_area_show = 'off' ; + ani_settings.reachable_area_show = 'off'; + ani_settings.goal_show = 'off'; + ani_settings.next_desired_position_show = 'off'; +end +if strcmp(gait_planning_param.type,'koizumi') + ani_settings.graspable_points_show = 'off'; + ani_settings.graspable_points_in_reachable_area_show = 'off' ; + ani_settings.reachable_area_show = 'off'; + ani_settings.goal_show = 'off'; + ani_settings.next_desired_position_show = 'on'; + + equilibrium_eval_param.expansion_factor = 0.01; + ani_settings.z_vis_range = [-0.35 0.35]; +end + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Save basic variables to csv file on/off +% save_settings.csv_file = 'on'; +% +% %%% Save TSM variables to csv file on/off +% save_settings.tsm = 'on'; +% ani_settings.save = 'on'; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Plot footholds history on/off +plot_settings.footholds = 'on'; +%%% Plot TSM +plot_settings.tsm = 'on'; +if strcmp(equilibrium_eval_param.type,'tsm') + plot_settings.gia_margin = 'off'; + plot_settings.gia_inclination_margin = 'off'; +elseif strcmp(equilibrium_eval_param.type,'gia') + plot_settings.tsm = 'off'; + plot_settings.gia_margin = 'on'; +end + +if strcmp(plot_settings.tsm,'on') + save_settings.tsm = 'on'; +end +if strcmp(plot_settings.gia,'on') + save_settings.gia = 'on'; +end +if strcmp(plot_settings.gia_margin,'on') +equilibrium_eval_param.type = 'gia'; +save_settings.gia = 'on'; +end + +end \ No newline at end of file diff --git a/config/preset/config_clawar_2021_dynamic_climbing_demo_param.m b/config/preset/config_clawar_2021_dynamic_climbing_demo_param.m new file mode 100644 index 0000000..7cce825 --- /dev/null +++ b/config/preset/config_clawar_2021_dynamic_climbing_demo_param.m @@ -0,0 +1,259 @@ +%%%%%% Configuration +%%%%%% config_clawar_2021_dynamic_climbing_demo_param +%%%%%% +%%%%%% Define preset parameters +%%%%%% +%%%%%% -------------------------------------------------------------------- +%%%%%% This configration file can reproduce the simulation result used in +%%%%%% CLAWAR 2021 proceedings paper by K. Uno and G. Valsecchi et al. +%%%%%% Proceedings Paper URL: +%%%%%% https://******** (to be added) +%%%%%% -------------------------------------------------------------------- +%%%%%% +%%%%%% Created 2021-01-18 +%%%%%% by Kentaro Uno +%%%%%% Last update: 2021-08-23 +%%%%%% by Kentaro Uno +% +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_clawar_2021_dynamic_climbing_demo_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') +robot_param.robot_type = 'Climbing_ANYmal'; + +% Nomally, the following four items should be defined in the LP file, +% but it is also possible to specify the joint allocation type and offset +% angles (theta1 and 2) for the hip joints here for more convenience. + +% if you set "insect" for the following type, theta1 and 2 are forced to be pi/4 and -pi/2 [rad], +% respectively. +robot_param.joint_allocation_type = 'insect'; % this can also be set in ini_LP file +% for the mammalian robot, you can specify the leg configuration from "oo", "xx", and "M". +robot_param.leg_config_type = 'oo'; % this can also be set in LP file +robot_param.theta_1 = 0.1440; % [rad] +% note that if you put the negative num for theta_2, the IK solver take automatically +% 'oo' configuration solution +robot_param.theta_2 = -0.1955; % [rad] + +%%% "Initial" x and y position of legs relative to base center [m] +horizontal_length_from_base_corner = 0.45; % [m] +yaw_angle = 45; % [deg] +base_width = 0.3; % [m] +base_length = 0.3; % [m] +robot_param.x_foot_dist = base_length/2 + horizontal_length_from_base_corner * cos(yaw_angle*pi/180); +robot_param.y_foot_dist = base_width/2 + horizontal_length_from_base_corner * sin(yaw_angle*pi/180); +%%% Height of base relative to map [m] +robot_param.base_height = 0.15; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Type of the surface ('flat_HR', 'flat_HR_5m_x_5m', 'rough', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') +environment_param.surface_type = 'flat_HR_5m_x_5m'; +%%% Gravity [G] +environment_param.grav = 1; + +%%% Dynamics on/off +environment_param.dynamics_flag = 'on'; +%%% Maximum simulation time [s] +environment_param.time_max = 64; + +%%% Surface Inclination [deg] +environment_param.inc = 45; + +% Floor reaction force stiffness coefficient +environment_param.surface_K = 100000; +% Floor reaction force damping coefficient +environment_param.surface_D = 100.0; + +%%% Gripper detachment method ('none', 'max_holding_force') +environment_param.detachment_detection_method = 'max_holding_force'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gait type ('do_nothing', 'crawl_fixed_stride','periodic_gait_for_discrete_footholds','nonperiodic_gait_for_discrete_footholds' ) +gait_planning_param.type = 'diagonal_gait_fixed_stride'; + +%%% Horizontal distance per step (stride) [m] +gait_planning_param.step_length = 0.1; +%%% Step height [m] +gait_planning_param.step_height = 0.1; +%%% Period of one cycle [s] +gait_planning_param.T = 32; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.2; +% %%% Walking sequence: +% gait_planning_param.sequence = [1; 2; 4; 3]; +%%% Goal position [m] +gait_planning_param.goal = [1.0; 0.0; 0.0]; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller Proportional (P) gain +control_param.kp = 2000.0; +%%% Controller Derivative (D) gain +control_param.kd = 4.5; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia', 'tsm_and_gia') +equilibrium_eval_param.type = 'tsm_and_gia'; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] +ani_settings.x_lim = [-0.6, 0.6]; +%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] +ani_settings.y_lim = [-0.7, 0.7]; +%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] +ani_settings.z_lim = [-0.6, 0.8]; +%%% Lower z-axis limit same as lower point on surface on/off +ani_settings.z_lim_low_is_surface = 'off'; +%%% 'on' camera follows robot, 'off' camera fixed +ani_settings.move_camera = 'off'; +%%% Camera angle azimuth and elevation [deg] +ani_settings.camera_az =-15; +ani_settings.camera_el = 5; + +%%% Font name +ani_settings.font_name = 'Times New Roman'; +%%% Font size +ani_settings.font_size = 18; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Link radius [m] +ani_settings.link_radius = 0.030; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.20; +ani_settings.base_lower_thickness = 0.05; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.0; +%%% Robot color +ani_settings.robot_base_upper_color = [0.1, 0.1, 0.1]; +ani_settings.robot_base_lower_color = [0.0, 0.0, 0.0]; +ani_settings.robot_limb_color = [0.3, 0.3, 0.3]; +%%% Robot transparency +ani_settings.robot_base_alpha = 1.0; +ani_settings.robot_limb_alpha = 1.0; + +%%% Frames at each joint on/off +ani_settings.frames_show = 'off'; + ani_settings.frames_line_width = 2.0; + ani_settings.frames_size = 0.1; + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + + %%% GIA Stable Region visualize on/off (requires equilibrium evaluation method as 'gia') + ani_settings.gia_stable_region_show = 'off'; + %%% Stability polyhedron face color + ani_settings.polyh_Face_color = [0 0 1]; + %%% Stability polyhedron face alpha + ani_settings.polyh_Face_alpha = 0.25; + %%% Stability polyhedron edge color + ani_settings.polyh_Edge_color = [0 0 1]; + %%% Stability polyhedron edge width + ani_settings.polyh_Edge_width = 2; + + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'off'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [0 0.7 0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 3; + + %%% GIA vector visualize on/off + ani_settings.gia_vec_show = 'off'; + %%% GIA vector color + ani_settings.gia_vec_color = [1 0 0]; + %%% GIA vector width + ani_settings.gia_vec_width = 3; + +%%% Transformation from force to visualize Fg (gravitational force) and +%%% Fe (reaction force) Stable Region in the position coordinate +%%% -> this scale is used for all force-dimensional variables visualization +ani_settings.force_expansion_factor = 0.002; + + %%% Gravitational Force vector visualize on/off + ani_settings.gravitational_force_vec_show = 'on'; + %%% Gravity vector color + ani_settings.gravitational_force_vec_color = [0.7 0 0]; + %%% Gravity vector width + ani_settings.gravitational_force_vec_width = 3; + + %%% Reaction force vectors vis. on/off + ani_settings.reaction_force_show = 'on'; + %%% Reaction force vectors color + ani_settings.reaction_force_vec_color = [1 0 1]; + %%% Reaction force vectors color width + ani_settings.reaction_force_vec_width = 2; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Plot trajectory +ani_settings.trajectory_show = 'off'; + +%%% Support triangle visualization +ani_settings.support_triangle_show = 'on'; + %%% Support triangle transparency + ani_settings.support_triangle_edge_color = 'none'; + +%%% Goal vis. on/off +ani_settings.goal_show = 'on'; + %%% Goal vis. marker size + ani_settings.goal_size = 40; + %%% Goal vis. height from the ground + ani_settings.goal_vis_height = 0.04; % [m] + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Time interval for saving variables (should be larger than time-step) +save_settings.variable_saving_time_interval = 0.05; + + +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; +%%% Save TSM variables to csv file on/off +save_settings.tsm = 'on'; +% %%% Save variables to csv file on/off +save_settings.gia = 'on'; +%%% Save manipulability to csv file on/off +save_settings.manipulability = 'on'; +%%% Save Maximum of absolute torque of all joint +save_settings.joint_max_torque = 'on'; +%%% Save Root Mean Square (RMS) of torque of all joint (*need Signal Processing Toolbox) +save_settings.joint_rms_torque = 'on'; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Plot Base position +plot_settings.base_pos = 'off'; +%%% Plot footholds history on/off +plot_settings.footholds = 'on'; +%%% Plot TSM +plot_settings.tsm = 'on'; +%%% Plot Gravito-Inertial Acceleration margin +plot_settings.gia_margin = 'on'; +%%% Plot manipulability +plot_settings.manipulability = 'on'; +%%% Plot max torque of all joint +plot_settings.joint_max_torque = 'on'; +%%% Plot RMS torque of all joint +plot_settings.joint_rms_torque = 'on'; + +end \ No newline at end of file diff --git a/config/preset/config_clawar_2021_statistical_analysis_param.m b/config/preset/config_clawar_2021_statistical_analysis_param.m new file mode 100644 index 0000000..59e179c --- /dev/null +++ b/config/preset/config_clawar_2021_statistical_analysis_param.m @@ -0,0 +1,196 @@ +%%%%%% Configuration +%%%%%% config_clawar_2021_statistical_analysis_param +%%%%%% +%%%%%% Define preset parameters +%%%%%% +%%%%%% -------------------------------------------------------------------- +%%%%%% This configration file can reproduce the simulation result used in +%%%%%% CLAWAR 2021 proceedings paper by K. Uno and G. Valsecchi et al. +%%%%%% Proceedings Paper URL: +%%%%%% https://******** (to be added) +%%%%%% -------------------------------------------------------------------- +%%%%%% +%%%%%% Created 2021-02-16 +%%%%%% by Kentaro Uno +%%%%%% Last update: 2021-08-23 +%%%%%% by Kentaro Uno +% +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_clawar_2021_statistical_analysis_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') +robot_param.robot_type = 'Climbing_ANYmal'; + +% Nomally, the following four items should be defined in the LP file, +% but it is also possible to specify the joint allocation type and offset +% angles (theta1 and 2) for the hip joints here for more convenience. + +% if you set "insect" for the following type, theta1 and 2 are forced to be pi/4 and -pi/2 [rad], +% respectively. +robot_param.joint_allocation_type = 'insect'; % this can also be set in ini_LP file +% for the mammalian robot, you can specify the leg configuration from "oo", "xx", and "M". +robot_param.leg_config_type = 'oo'; % this can also be set in LP file +robot_param.theta_1 = 0.1440; % [rad] +% note that if you put the negative num for theta_2, the IK solver take automatically +% 'oo' configuration solution +robot_param.theta_2 = -0.1955; % [rad] + +%%% "Initial" x and y position of legs relative to base center [m] +horizontal_length_from_base_corner = 0.45; % [m] +yaw_angle = 45; % [deg] +base_width = 0.3; % [m] +base_length = 0.3; % [m] +robot_param.x_foot_dist = base_length/2 + horizontal_length_from_base_corner * cos(yaw_angle*pi/180); +robot_param.y_foot_dist = base_width/2 + horizontal_length_from_base_corner * sin(yaw_angle*pi/180); +% robot_param.x_foot_dist = 0.4; +% robot_param.y_foot_dist = 0.4; + +%%% Height of base relative to map [m] +robot_param.base_height = 0.2; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Type of the surface ('flat_HR', 'flat_HR_5m_x_5m', 'rough', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') +environment_param.surface_type = 'flat_HR_5m_x_5m'; +%%% Gravity [G] (1/6: moon, 3/8: mars, 10^-5: asteroid) +environment_param.grav = 1; + +%%% Dynamics on/off +environment_param.dynamics_flag = 'on'; +%%% Maximum simulation time [s] +environment_param.time_max = 2; + +%%% Surface Inclination [deg] +environment_param.inc = 45; + +% Floor reaction force stiffness coefficient +environment_param.surface_K = 100000; +% Floor reaction force damping coefficient +environment_param.surface_D = 100.0; + +%%% Gripper detachment method ('none', 'max_holding_force') +environment_param.detachment_detection_method = 'none'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver') +gait_planning_param.type = 'do_nothing'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller type ('do_nothing', 'torque_PD') +control_param.type = 'torque_PD'; +%%% Controller Proportional (P) gain +control_param.kp = 500.0; +%%% Controller Derivative (D) gain +control_param.kd = 10.0; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia', 'tsm_and_gia') +equilibrium_eval_param.type = 'tsm_and_gia'; + +% Plot polyhderon on animation on/off (requires equilibrium evaluation method as 'gia') +equilibrium_eval_param.plot_polyhedron = 'off'; +% Transformation from acceleration to plot in the position coordinate +equilibrium_eval_param.expansion_factor = 0.02; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] +ani_settings.x_lim = [-0.8, 0.8]; +%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] +ani_settings.y_lim = [-0.9, 0.9]; +%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] +ani_settings.z_lim = [-0.6, 0.8]; +%%% Lower z-axis limit same as lower point on surface on/off +ani_settings.z_lim_low_is_surface = 'off'; +%%% 'on' camera follows robot, 'off' camera fixed +ani_settings.move_camera = 'off'; +%%% Camera angle azimuth and elevation [deg] +ani_settings.camera_az =-0; +ani_settings.camera_el = 0; + +%%% Font name +ani_settings.font_name = 'Times New Roman'; +%%% Font size +ani_settings.font_size = 25; + +%%% Link radius [m] +ani_settings.link_radius = 0.010; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.03; +ani_settings.base_lower_thickness = 0.03; +%%% Robot color +ani_settings.robot_base_upper_color = [0.2, 0.2, 0.2]; +ani_settings.robot_base_lower_color = [0.2, 0.2, 0.2]; +ani_settings.robot_limb_color = [0.2, 0.2, 0.2]; +%%% Robot transparency +ani_settings.robot_base_alpha = 1.0; +ani_settings.robot_limb_alpha = 1.0; + +%%% Stability polyhedron face color +ani_settings.polyh_Face_color = [0 0 1]; +%%% Stability polyhedron face alpha +ani_settings.polyh_Face_alpha = 0.25; +%%% Stability polyhedron edge color +ani_settings.polyh_Edge_color = [0 0 1]; +%%% Stability polyhedron edge width +ani_settings.polyh_Edge_width = 2; + +%%% Gravity vector visualize on/off +ani_settings.gravity_vec_show = 'on'; + %%% Gravity vector color + ani_settings.gravity_vec_color = [0 0.7 0]; + %%% Gravity vector width + ani_settings.gravity_vec_width = 3; + +%%% GIA vector visualize on/off +ani_settings.gia_vec_show = 'on'; + %%% GIA vector color + ani_settings.gia_vec_color = [1 0 0]; + %%% GIA vector width + ani_settings.gia_vec_width = 3; + +ani_settings.trajectory_show = 'off'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; +%%% Save TSM variables to csv file on/off (requires equilibrium evaluation method as 'tsm') +save_settings.tsm = 'on'; +%%% Save GIA variables to csv file on/off (requires equilibrium evaluation method as 'gia') +save_settings.gia = 'on'; +%%% Save manipularbility measure to csv file on/off +save_settings.manipulability = 'on'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Plot Base position +plot_settings.base_pos = 'off'; + +%%% Plot Joint torque +plot_settings.joint_torque = 'on'; + +%%% Plot TSM (requires equilibrium evaluation method as 'tsm') +plot_settings.tsm = 'on'; + +%%% Plot GIA (Acceleration) margin (requires equilibrium evaluation method as 'gia') +plot_settings.gia_margin = 'on'; + +%%% Plot GIA Inclination margin (requires equilibrium evaluation method as 'gia') +plot_settings.gia_inclination_margin = 'on'; + +%%% Plot manipulability +plot_settings.manipulability = 'on'; +end \ No newline at end of file diff --git a/config/preset/config_uno_crawl_param.m b/config/preset/config_crawl_gait_for_discrete_footholds_param.m similarity index 67% rename from config/preset/config_uno_crawl_param.m rename to config/preset/config_crawl_gait_for_discrete_footholds_param.m index 2ffa75b..0023e1e 100644 --- a/config/preset/config_uno_crawl_param.m +++ b/config/preset/config_crawl_gait_for_discrete_footholds_param.m @@ -1,21 +1,23 @@ %%%%%% Configuration -%%%%%% config_uno_crawl_param +%%%%%% config_crawl_gait_for_discrete_footholds_param %%%%%% -%%%%%% Define uno_crawl_gait parameters +%%%%%% Define crawl_gait_for_discrete_footholds parameters %%%%%% %%%%%% Created 2020-07-09 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-09 +%%%%%% Last updated: 2021-07-09 +%%%%%% Keigo Haji % % -% Load configurations for parameters defined for uno crawl gait simulation +% Load configurations for parameters defined for periodic gait for discrete +% footholds simulation % % Function variables: % % OUTPUT % robot_param % environment_param -% gait_param +% gait_planning_param % control_param % equilibrium_eval_param % ani_settings @@ -24,16 +26,17 @@ % INPUT % - -function [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings] = config_uno_crawl_param(robot_param, environment_param, gait_param, control_param, ... +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_crawl_gait_for_discrete_footholds_param(robot_param, environment_param, gait_planning_param, control_param, ... equilibrium_eval_param, ani_settings, save_settings, plot_settings) %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') -robot_param.robot_type = 'HubRobo_v2_2_grip_to_palm'; +robot_param.robot_type = 'HubRobo_v3_2_grip_to_palm'; %%% x and y position of legs relative to base center [m] -robot_param.foot_dist = 0.13; +robot_param.x_foot_dist = 0.18; +robot_param.y_foot_dist = 0.18; %%% Height of base relative to map [m] -robot_param.base_height = 0.08; +robot_param.base_height = 0.10; %%% Base position [m] 2x1 vector. or 'default' for default setting robot_param.base_pos_xy = [0;0]; @@ -41,25 +44,27 @@ %%% 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 = 40; +environment_param.time_max = 8; %%% Graspable points detection type -environment_param.graspable_points_detection_type = 40; +environment_param.graspable_points_detection_type = 0; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver') -gait_param.type = 'crawl_uno_ver'; +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds','nonperiodic_gait_for_discrete_footholds' ) +gait_planning_param.type = 'crawl_gait_for_discrete_footholds'; %%% Step height [m] -gait_param.step_height = 0.03; +gait_planning_param.step_height = 0.03; %%% Period of one cycle [s] -gait_param.T = 4; +gait_planning_param.T = 4; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.1; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Controller Proportional (P) gain -control_param.kp = 3.0; +control_param.kp = 10.0; %%% Controller Derivative (D) gain -control_param.kd = 0.02; +control_param.kd = 0.30; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Equilibrium evaluation method ('none', 'tsm', 'gia') @@ -67,11 +72,15 @@ %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Link radius [m] -ani_settings.link_radius = 0.012; +ani_settings.link_radius = 0.015; %%% Base thickness [m] -ani_settings.base_thickness = 0.05; +ani_settings.base_upper_thickness = 0.04; +ani_settings.base_lower_thickness = 0.02; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.2; %%% Robot transparency -ani_settings.robot_alpha = 1; +ani_settings.robot_base_alpha = 1; +ani_settings.robot_limb_alpha = 1; %%% Graspable points viz. on/off ani_settings.graspable_points_show = 'on'; @@ -94,7 +103,7 @@ %%% Save basic variables to csv file on/off save_settings.csv_file = 'on'; -%%% Save TSM variables to csv file on/off +%%% Save TSM variables to csv file on/off (requires equilibrium evaluation method as 'tsm') save_settings.tsm = 'on'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/config/preset/config_default_param.m b/config/preset/config_default_param.m deleted file mode 100644 index 757fbbc..0000000 --- a/config/preset/config_default_param.m +++ /dev/null @@ -1,392 +0,0 @@ -%%%%%% Configuration -%%%%%% config_default_param -%%%%%% -%%%%%% Define default parameters -%%%%%% -%%%%%% Created 2020-07-09 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-09-08 -% -% -% Define configurations for default parameters. This file is not actually used, but it contains a list of ALL parameters. use -% it as a reference for the variables in your USER configuration. -% -% Function variables: -% -% OUTPUT -% robot_param -% environment_param -% gait_param -% control_param -% equilibrium_eval_param -% ani_settings -% save_settings -% plot_settings -% INPUT -% - - -function [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings] = config_default_param(robot_param, environment_param, gait_param, control_param, ... - equilibrium_eval_param, ani_settings, save_settings, plot_settings) -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') -robot_param.robot_type = 'HubRobo_v2_2_grip_to_palm'; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Position settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% x and y position of legs relative to base center [m] -robot_param.foot_dist = 0.12; -%%% Height of base relative to map [m] -robot_param.base_height = 0.12; -%%% Base position [m] 2x1 vector. or 'default' for default setting -robot_param.base_pos_xy = [0;0]; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Type of the surface ('flat_HR', 'rough', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') -environment_param.surface_type = 'rough'; -%%% Gravity [G] -environment_param.grav = 1/6; - -%%% Dynamics on/off -environment_param.dynamics_flag = 'on'; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Time settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Time-step [s] -environment_param.time_step = 0.001; -%%% Maximum simulation time [s] -environment_param.time_max = 8; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Surface Inclination [deg] -environment_param.inc = 0; -%%% Ground reaction force stiffness coefficient -environment_param.surface_K = 1000; -%%% Ground reaction force damping coefficient (scalar) -environment_param.surface_D = 1; -%%% Graspable points detection type -environment_param.graspable_points_detection_type = 'all'; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver') -gait_param.type = 'crawl_fixed_stride'; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Crawl gait settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Period of one cycle [s] -gait_param.T = 4; -%%% Duty cycle -gait_param.beta = 0.75; -%%% Horizontal distance per step (stride) [m] -gait_param.step_length = 0.05; -%%% Step height [m] -gait_param.step_height = 0.03; -%%% Walking sequence: 1 - 3 - 4 - 2 -gait_param.sequence = [1; 3; 4; 2]; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Uno gait settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Goal position [m] -gait_param.goal = [0.4;0;-0.08]; - - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Controller type ('do_nothing', 'torque_PD') -control_param.type = 'torque_PD'; - - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Controller Proportional (P) gain -control_param.kp = 3.00; -%%% Controller Derivative (D) gain -control_param.kd = 0.02; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Equilibrium evaluation method ('none', 'tsm', 'gia') -equilibrium_eval_param.type = 'tsm'; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GIA polyhedron settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Plot polyhderon on animation on/off (requires equilibrium evaluation method as 'gia') -equilibrium_eval_param.plot_polyhedron = 'off'; -% Transformation from acceleration to plot in the position coordinate -equilibrium_eval_param.expansion_factor = 0.02; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Display animation on/off -ani_settings.display = 'on'; -%%% Save animation -ani_settings.save = 'off'; - -%%% Frame rate [frames/s] [20,25,50] -ani_settings.frame_rate = 20; -%%% Animation video resolution [px] [[1280 720], [640 480]] -ani_settings.animation_resolution = [640 480]; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] -ani_settings.x_lim = [-0.25, 0.4]; -%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] -ani_settings.y_lim = [-0.25, 0.25]; -%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] -ani_settings.z_lim = [-0.18, 0.15]; -%%% Lower z-axis limit same as lower point on surface on/off -ani_settings.z_lim_low_is_surface = 'on'; - -%%% 'on' camera follows robot, 'off' camera fixed -ani_settings.move_camera = 'on'; - %%% x-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] - ani_settings.x_vis_range = ani_settings.x_lim - mean(ani_settings.x_lim); - %%% y-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25]] - ani_settings.y_vis_range = ani_settings.y_lim - mean(ani_settings.y_lim); - %%% z-axis visualization range [m] [[-0.08, 0.1], [-0.1, .14]] - ani_settings.z_vis_range = ani_settings.z_lim - mean(ani_settings.z_lim); - - -%%% Camera angle azimuth and elevation [deg] -ani_settings.camera_az =-20; -ani_settings.camera_el = 12; - -%%% Font name -ani_settings.font_name = 'Times New Roman'; -%%% Font size -ani_settings.font_size = 25; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Link radius [m] -ani_settings.link_radius = 0.012; -%%% Base thickness [m] -ani_settings.base_thickness = 0.03; - -%%% Robot color -ani_settings.robot_color = [0.2, 0.2, 0]; -%%% Robot transparency -ani_settings.robot_alpha = 0.8; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Surface viz. on/off -ani_settings.surface_show = 'on'; - %%% Surface grid color - ani_settings.grid_color = [0.9 0.9 0.9]; - %%% Surface color - ani_settings.surface_color = gray; - -%%% Graspable points viz. on/off -ani_settings.graspable_points_show = 'off'; - %%% Graspable points color - ani_settings.graspable_points_color = [0, 0, 1.0]; - %%% Graspable points marker - ani_settings.graspable_points_marker = '.'; - %%% Graspable points size - ani_settings.graspable_points_size = 5; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Graspable points in reachable area viz. on/off -ani_settings.graspable_points_in_reachable_area_show = 'off'; - %%% Graspable points in reachable area color - ani_settings.graspable_points_in_reachable_area_color = [0, 1.0, 0]; - %%% Graspable points in reachable area marker - ani_settings.graspable_points_in_reachable_area_marker = '.'; - %%% Graspable points in reachable area size - ani_settings.graspable_points_in_reachable_area_size = 20; - -%%% Reachable area viz. on/off -ani_settings.reachable_area_show = 'off'; - %%% Reachable area line color - ani_settings.reachable_area_line_color = 'magenta'; - %%% Reachable area line width - ani_settings.reachable_area_line_width = 1; - -%%% Goal vis. on/off -ani_settings.goal_show = 'off'; - %%% Goal color - ani_settings.goal_color = [0 0 1.0]; - %%% Goal marker - ani_settings.goal_marker = '.'; - %%% Goal size - ani_settings.goal_size = 25; - %%% Goal vis. height from the ground - ani_settings.goal_vis_height = 0.0; - -%%% Next desired position vis. on/off -ani_settings.next_desired_position_show = 'off'; - %%% Next desired position line color m : magenta - ani_settings.next_desired_position_color = 'm'; - %%% Next desired position line width - ani_settings.next_desired_position_line_width = 1; - -%%% Plot trajectory -ani_settings.trajectory_show = 'on'; - %%% Trajectory line color - ani_settings.trajectory_color = 'c'; - %%% Trajectory line widith - ani_settings.trajectory_width = 3; - %%% Trajectory line type - ani_settings.trajectory_line_type = ':'; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Support triangle visualization -ani_settings.support_triangle_show = 'off'; - %%% Support triangle color - ani_settings.support_triangle_color = [0 1.0 1.0]; - %%% Support triangle edge line color - ani_settings.support_triangle_edge_color = [0.0 0.0 0.0]; - %%% Support triangle transparency - ani_settings.support_triangle_alpha = 0.5; - -%%% Stability polyhedron face color -ani_settings.polyh_Face_color = [0 0 1]; -%%% Stability polyhedron face alpha -ani_settings.polyh_Face_alpha = 0.25; -%%% Stability polyhedron edge color -ani_settings.polyh_Edge_color = [0 0 1]; -%%% Stability polyhedron edge width -ani_settings.polyh_Edge_width = 2; - -%%% Gravity vector visualize on/off -ani_settings.gravity_vec_show = 'off'; - %%% Gravity vector color - ani_settings.gravity_vec_color = [0 0.7 0]; - %%% Gravity vector width - ani_settings.gravity_vec_width = 3; - -%%% GIA vector visualize on/off -ani_settings.gia_vec_show = 'off'; - %%% GIA vector color - ani_settings.gia_vec_color = [1 0 0]; - %%% GIA vector width - ani_settings.gia_vec_width = 3; - -%%% CoM projection point vis. on/off -ani_settings.com_projection_show = 'off'; - %%% CoM projection point color - ani_settings.com_projection_color = [1 0 0]; - %%% CoM projection point marker - ani_settings.com_projection_marker = '.'; - %%% CoM projection point size - ani_settings.com_projection_size = 25; - %%% CoM projection point vis. height from the ground - ani_settings.com_projection_vis_height = 0.0; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Save basic variables to csv file on/off -save_settings.csv_file = 'on'; - -%%% Save TSM variables to csv file on/off -save_settings.tsm = 'off'; -%%% Save variables to csv file on/off -save_settings.gia = 'off'; - -%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%%% Font name -plot_settings.font_name = 'Times New Roman'; -%%% Font size -plot_settings.font_size = 25; -%%% Line width -plot_settings.width = 3; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Basic graphs %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% 2 ~ 100 -%%% Plot Base position -plot_settings.base_pos = 'on'; -plot_settings.base_pos_fig_number = 2; -%%% Plot Base orientation -plot_settings.base_ori = 'off'; -plot_settings.base_ori_fig_number = 3; -%%% Plot Base velocity -plot_settings.base_vel = 'off'; -plot_settings.base_vel_fig_number = 4; -%%% Plot Base angular velocity -plot_settings.base_angvel = 'off'; -plot_settings.base_angvel_fig_number = 5; -%%% Plot Base velocity -plot_settings.base_acc = 'off'; -plot_settings.base_acc_fig_number = 6; -%%% Plot Base angular velocity -plot_settings.base_angacc = 'off'; -plot_settings.base_angacc_fig_number = 7; - -%%% Plot Joint angular position -plot_settings.joint_pos = 'off'; -plot_settings.joint_pos_fig_number = 8; -%%% Plot Joint angular velocity -plot_settings.joint_vel = 'off'; -plot_settings.joint_vel_fig_number = 9; -%%% Plot Joint angular acceleration -plot_settings.joint_acc = 'off'; -plot_settings.joint_acc_fig_number = 10; -%%% Plot Joint torque -plot_settings.joint_torque = 'off'; -plot_settings.joint_torque_fig_number = 11; - -%%% Plot Leg Position -plot_settings.leg_pos = 'off'; -plot_settings.leg_pos_fig_number = 12; % and 13, 14, 15 are also used - -%%% Plot Reaction Force -plot_settings.reaction_force = 'off'; -plot_settings.reaction_force_fig_number = 16; % and 17~23 are also used - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% 101 ~ 200 -%%% Plot footholds history on/off -plot_settings.footholds = 'off'; -plot_settings.footholds_fig_number = 101; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% 201 ~ 300 -%%% Plot TSM -plot_settings.tsm = 'off'; -plot_settings.tsm_fig_number = 201; - -%%% Plot Acceleration margin -plot_settings.acc_margin = 'off'; -plot_settings.acc_margin_fig_number = 202; - -%%% Plot Inclination margin -plot_settings.inclination_margin = 'off'; -plot_settings.inclination_margin_fig_number = 203; - -end \ No newline at end of file diff --git a/config/preset/config_example_demo_1_param.m b/config/preset/config_example_demo_1_param.m new file mode 100644 index 0000000..00dabc4 --- /dev/null +++ b/config/preset/config_example_demo_1_param.m @@ -0,0 +1,356 @@ +%%%%%% +%%%%%% Configuration +%%%%%% config_example_demo_1_param +%%%%%% +%%%%%% Define simulation parameters for the representative simulation cases +%%%%%% of the ClimbLab +%%%%%% [x] Demo (1) Static simulation to demonstrate the detachment of the +%%%%%% gripper from an over-hanging wall +%%%%%% [ ] Demo (2) Steep slope climbing of the mammalian typed robot +%%%%%% [ ] Demo (3) Perceptive walking of the half human sized quadrupedal robot +%%%%%% +%%%%%% +%%%%%% -------------------------------------------------------------------- +%%%%%% This configration file can reproduce the similar result of CLAWAR +%%%%%% 2021 proceedings paper by K. Uno, W. Ribeiro et al. +%%%%%% Proceedings Paper URL: +%%%%%% https://******** (to be added) +%%%%%% -------------------------------------------------------------------- +%%%%%% +%%%%%% Creation: 2021-03-02 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-06-30 +%%%%%% Kentaro Uno +% +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_example_demo_1_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') +robot_param.robot_type = 'ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N'; +%%% x and y position of legs relative to base center [m] +robot_param.x_foot_dist = 0.150; +robot_param.y_foot_dist = 0.150; +%%% Height of base relative to map [m] +robot_param.base_height = 0.080; +%%% Base position [m] 2x1 vector. or 'default' for default setting +robot_param.base_pos_xy = [0.0;0.0]; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Following Values are written in RealSense D435i datasheet. +sensing_camera_param.fov_horizontal = 86*pi/180; %[radian] +sensing_camera_param.fov_vertical = 57*pi/180; %[radian] +sensing_camera_param.fov_max_distance = 2; %[m] +sensing_camera_param.fov_min_distance = 0.05; %[m] + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gravity [G] +environment_param.grav = 1; +%%% Type of the surface ('flat_HR', 'rough', 'climbing_holds_1m_x_1m', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') +environment_param.surface_type = 'rough'; +%%% Maximum simulation time [s] +environment_param.time_max = 1; +%%% Dynamics on/off +environment_param.dynamics_flag = 'on'; +%%% Surface Inclination [deg] +% environment_param.inc = 100; %The robot keeps holding on to the wall. +environment_param.inc = 110; % The robot falls over. + +%%% Gripper detachment method ('none', 'max_holding_force', 'tsm') +environment_param.detachment_detection_method = 'max_holding_force'; + +environment_param.sim_stop_stuck = 'off'; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds', nonperiodic_gait_for_discrete_footholds) +gait_planning_param.type = 'do_nothing'; +%%% Step height [m] +gait_planning_param.step_height = 0.04; +%%% Period of one cycle [s] +gait_planning_param.T = 16; +%%% Duty cycle +gait_planning_param.beta = 0.75; +%%% Goal position [m] +gait_planning_param.goal = [0.4;0.5;0.0]; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.1; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller type ('do_nothing', 'torque_PD') +control_param.type = 'torque_PD'; +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller Proportional (P) gain +control_param.kp = 10.0; +%%% Controller Derivative (D) gain +control_param.kd = 0.30; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia', 'tsm_and_gia') +equilibrium_eval_param.type = 'tsm_and_gia'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% elapsed time show on/off +ani_settings.animation_elapsed_time_show = 'off'; + +%%% Frame rate [frames/s] [20,25,50] +ani_settings.frame_rate = 20; +%%% Animation video resolution [px] [[1280 720], [640 480]] +ani_settings.animation_resolution = [320 400]; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] +ani_settings.x_lim = [-0.35, 0.2]; +%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] +ani_settings.y_lim = [-0.5, 0.5]; +%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] +ani_settings.z_lim = [-0.5, 0.5]; +%%% Lower z-axis limit same as lower point on surface on/off +ani_settings.z_lim_low_is_surface = 'off'; + +%%% 'on' camera follows robot, 'off' camera fixed +ani_settings.move_camera = 'off'; + %%% x-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] + ani_settings.x_vis_range = ani_settings.x_lim - mean(ani_settings.x_lim); + %%% y-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25]] + ani_settings.y_vis_range = ani_settings.y_lim - mean(ani_settings.y_lim); + %%% z-axis visualization range [m] [[-0.08, 0.1], [-0.1, .14]] + ani_settings.z_vis_range = ani_settings.z_lim - mean(ani_settings.z_lim); +%%% Camera angle azimuth and elevation [deg] +ani_settings.camera_az = -25; +ani_settings.camera_el = 10; + +%%% Font name +ani_settings.font_name = 'Calibri'; +%%% Font size +ani_settings.font_size = 15; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Surface viz. on/off +ani_settings.surface_show = 'on'; + %%% Surface grid color + ani_settings.grid_color = 'white'; + %%% Surface color + ani_settings.surface_color = [0.5, 0.5, 0.5]; + +%%% Graspable points viz. on/off +ani_settings.graspable_points_show = 'off'; + %%% Graspable points color + ani_settings.graspable_points_color = [0.0, 0.8, 0.0]; + %%% Graspable points marker + ani_settings.graspable_points_marker = '.'; + %%% Graspable points size + ani_settings.graspable_points_size = 10; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Link radius [m] +ani_settings.link_radius = 0.0185; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.07; +ani_settings.base_lower_thickness = 0.03; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.2; +%%% Robot color +ani_settings.robot_base_upper_color = [0.1, 0.1, 0.1]; +ani_settings.robot_base_lower_color = [0.1, 0.1, 0.1]; +ani_settings.robot_limb_color = [0.1, 0.1, 0.1]; +%%% Robot transparency +ani_settings.robot_base_alpha = 0.72; +ani_settings.robot_limb_alpha = 0.72; + +%%% Frames at each joint on/off +ani_settings.frames_show = 'off'; + ani_settings.frames_line_width = 2.0; + ani_settings.frames_size = 0.1; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Graspable points in reachable area viz. on/off +ani_settings.graspable_points_in_reachable_area_show = 'off'; + %%% Graspable points size + ani_settings.graspable_points_in_reachable_area_size = 40; + +%%% Reachable area viz. on/off +ani_settings.reachable_area_show = 'off'; + %%% Reachable area line width + ani_settings.reachable_area_line_width = 3; + +%%% Next desired position vis. on/off +ani_settings.next_desired_position_show = 'off'; + ani_settings.next_desired_position_color = [0.0, 0.7, 0.7]; + %%% Next desired position line width + ani_settings.next_desired_position_line_width = 3; + +%%% Plot trajectory +ani_settings.trajectory_show = 'off'; + %%% Trajectory line color + ani_settings.trajectory_color = [0.5, 0.5, 0.5]; + %%% Trajectory line widith + ani_settings.trajectory_width = 3; + %%% Trajectory line type + ani_settings.trajectory_line_type = ':'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Support triangle visualization +ani_settings.support_triangle_show = 'on'; + %%% Support triangle color + ani_settings.support_triangle_color = [0 136/255 170/255]; + %%% Support triangle transparency + ani_settings.support_triangle_edge_color = 'none'; + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + + %%% GIA Stable Region visualize on/off (requires equilibrium evaluation method as 'gia') + ani_settings.gia_stable_region_show = 'off'; + %%% Stability polyhedron face color + ani_settings.gia_stable_region_Face_color = [0 0 1]; + %%% Stability polyhedron face alpha + ani_settings.gia_stable_region_Face_alpha = 0.25; + %%% Stability polyhedron edge color + ani_settings.gia_stable_region_Edge_color = [0 0 1]; + %%% Stability polyhedron edge width + ani_settings.gia_stable_region_Edge_width = 2; + + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'off'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [1.0, 0.0, 0.0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 6; + + %%% GIA vector visualize on/off + ani_settings.gia_vec_show = 'off'; + %%% GIA vector color + ani_settings.gia_vec_color = [1 0 0]; + %%% GIA vector width + ani_settings.gia_vec_width = 3; + +%%% Transformation from force to visualize Fg (gravitational force) and +%%% Fe (reaction force) Stable Region in the position coordinate +%%% -> this scale is used for all force-dimensional variables visualization +ani_settings.force_expansion_factor = 0.015; + + %%% Gravitational Force vector visualize on/off + ani_settings.gravitational_force_vec_show = 'on'; + %%% Gravity vector color + ani_settings.gravitational_force_vec_color = [0.7 0 0]; + %%% Gravity vector width + ani_settings.gravitational_force_vec_width = 3; + + %%% Reaction force vectors vis. on/off + ani_settings.reaction_force_show = 'on'; + %%% Reaction force vectors color + ani_settings.reaction_force_vec_color = [0.7 0 0.7]; + %%% Reaction force vectors color width + ani_settings.reaction_force_vec_width = 2; + +%%% CoM projection point vis. on/off +ani_settings.com_projection_show = 'off'; + %%% CoM projection point vis. marker size + ani_settings.com_projection_size = 40; + %%% CoM projection point vis. height from the ground + % visualization position can be offseted to locate it vertically upper to the ground to show it well + ani_settings.com_projection_vis_height = 0.04; % [m] + +%%% Goal vis. on/off +ani_settings.goal_show = 'off'; + %%% Goal vis. marker size + ani_settings.goal_size = 40; + %%% Goal vis. height from the ground + ani_settings.goal_vis_height = 0.04; % [m] + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% RealSense Camera fov vis. on/off +ani_settings.sensing_fov_show = 'off'; + %%% Sensing camera position marker size + ani_settings.sensing_camera_MarkerSize = 10; + %%% Sensing camera marker color + ani_settings.sensing_camera_MarkerColor = '.k'; + %%% Sensing camera fov line width + ani_settings.sensing_camera_fov_LineWidth = 1; + %%% Sensing camera fov line color + ani_settings.sensing_camera_fov_LineColor = 'w'; +%%% RealSense Camera fov filling vis. on/off +ani_settings.sensing_fov_face_filling = 'off'; + %%% Filling color + ani_settings.sensing_fov_face_color = 'w'; + %%% Filling color alpha + ani_settings.sensing_fov_face_alpha = 0.5; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; +%%% Save TSM variables to csv file on/off (requires equilibrium evaluation method as 'tsm') +save_settings.tsm = 'on'; +%%% Save GIA variables to csv file on/off (requires equilibrium evaluation method as 'gia') +save_settings.gia = 'on'; +%%% Save manipularbility measure to csv file on/off +save_settings.manipulability = 'off'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Plot Base position +plot_settings.base_pos = 'off'; + +%%% Plot Joint torque +plot_settings.joint_torque = 'on'; + +%%% Plot TSM (requires equilibrium evaluation method as 'tsm') +plot_settings.tsm = 'on'; + +%%% Plot GIA (Acceleration) margin (requires equilibrium evaluation method as 'gia') +plot_settings.gia_margin = 'on'; + +%%% Plot GIA Inclination margin (requires equilibrium evaluation method as 'gia') +plot_settings.gia_inclination_margin = 'on'; + +%%% Plot manipulability +plot_settings.manipulability = 'off'; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gripper Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Map Parameters and settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Matching Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +matching_settings.submatching = 'on'; +matching_settings.colony = 'on'; +matching_settings.threshold = 130; + +end \ No newline at end of file diff --git a/config/preset/config_example_demo_2_param.m b/config/preset/config_example_demo_2_param.m new file mode 100644 index 0000000..da0cf19 --- /dev/null +++ b/config/preset/config_example_demo_2_param.m @@ -0,0 +1,384 @@ +%%%%%% +%%%%%% Configuration +%%%%%% config_example_demo_2_param +%%%%%% +%%%%%% Define simulation parameters for the representative simulation cases +%%%%%% of the ClimbLab +%%%%%% [ ] Demo (1) Static simulation to demonstrate the detachment of the +%%%%%% gripper from an over-hanging wall +%%%%%% [x] Demo (2) Steep slope climbing of the mammalian typed robot +%%%%%% [ ] Demo (3) Perceptive walking of the half human sized quadrupedal robot +%%%%%% +%%%%%% +%%%%%% -------------------------------------------------------------------- +%%%%%% This configration file can reproduce the similar result of CLAWAR +%%%%%% 2021 proceedings paper by K. Uno, W. Ribeiro et al. +%%%%%% Proceedings Paper URL: +%%%%%% https://******** (to be added) +%%%%%% -------------------------------------------------------------------- +%%%%%% +%%%%%% Creation: 2021-03-02 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-06-30 +%%%%%% Kentaro Uno +% +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_example_demo_2_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') +robot_param.robot_type = 'ANYmal_B'; + +% Nomally, the following four items should be defined in the LP file, +% but it is also possible to specify the joint allocation type and offset +% angles (theta1 and 2) for the hip joints here for more convenience. + +% if you set "insect" for the following type, theta1 and 2 are forced to be pi/4 and -pi/2 [rad], +% respectively. +robot_param.joint_allocation_type = 'mammal'; % this can also be set in ini_LP file +% for the mammalian robot, you can specify the leg configuration from "oo", "xx", and "M". +robot_param.leg_config_type = 'xx'; % this can also be set in LP file +robot_param.theta_1 = 0.0; % [rad] +% note that if you put the negative num for theta_2, the IK solver take automatically +% 'oo' configuration solution +robot_param.theta_2 = 0.0; % [rad] + +%%% "Initial" x and y position of legs relative to base center [m] +robot_param.x_foot_dist = 0.35; +robot_param.y_foot_dist = 0.35; +%%% Height of base relative to map [m] +robot_param.base_height = 0.25; + +%%% Base position [m] 2x1 vector. or 'default' for default setting +robot_param.base_pos_xy = [0.0;0.0]; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Following Values are written in RealSense D435i datasheet. +sensing_camera_param.fov_horizontal = 86*pi/180; %[radian] +sensing_camera_param.fov_vertical = 57*pi/180; %[radian] +sensing_camera_param.fov_max_distance = 2; %[m] +sensing_camera_param.fov_min_distance = 0.05; %[m] + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gravity [G] +environment_param.grav = 1; +%%% Type of the surface ('flat_HR', 'flat_HR_5m_x_5m', 'rough', 'rough_3m_x_3m', 'climbing_holds_1m_x_1m', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') +environment_param.surface_type = 'flat_HR_5m_x_5m'; +%%% Maximum simulation time [s] +environment_param.time_max = 16; +%%% Dynamics on/off +environment_param.dynamics_flag = 'on'; +%%% Surface Inclination [deg] +environment_param.inc = 45; + +%%% Floor reaction force stiffness coefficient +environment_param.surface_K = 100000; +%%% Floor reaction force damping coefficient +environment_param.surface_D = 100.0; + +%%% Gripper detachment method ('none', 'max_holding_force', 'tsm') +environment_param.detachment_detection_method = 'max_holding_force'; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gait type (e.g. 'do_nothing', 'diagonal_gait_fixed_stride') +gait_planning_param.type = 'diagonal_gait_fixed_stride'; +%%% Step height [m] +gait_planning_param.step_height = 0.1; +gait_planning_param.step_length = 0.1; +%%% Period of one cycle [s] +gait_planning_param.T = 8; +%%% Duty cycle +gait_planning_param.beta = 0.75; +%%% Goal position [m] +gait_planning_param.goal = [0.4;0.5;0.0]; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.2; +%%% Walking sequence: +gait_planning_param.sequence = [1; 3; 4; 2]; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller type ('do_nothing', 'torque_PD') +control_param.type = 'torque_PD'; +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller Proportional (P) gain +control_param.kp = 850.0; +%%% Controller Derivative (D) gain +control_param.kd = 3.0; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia', 'tsm_and_gia') +equilibrium_eval_param.type = 'tsm_and_gia'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% elapsed time show on/off +ani_settings.animation_elapsed_time_show = 'off'; + +%%% Frame rate [frames/s] [20,25,50] +ani_settings.frame_rate = 20; +%%% Animation video resolution [px] [[1280 720], [640 480]] +ani_settings.animation_resolution = [640 480]; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] +ani_settings.x_lim = [-0.5, 1.0]; +%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] +ani_settings.y_lim = [-1.0, 1.0]; +%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] +ani_settings.z_lim = [-0.5, 1.0]; +%%% Lower z-axis limit same as lower point on surface on/off +ani_settings.z_lim_low_is_surface = 'off'; + +%%% 'on' camera follows robot, 'off' camera fixed +ani_settings.move_camera = 'off'; + %%% x-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] + ani_settings.x_vis_range = ani_settings.x_lim - mean(ani_settings.x_lim); + %%% y-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25]] + ani_settings.y_vis_range = ani_settings.y_lim - mean(ani_settings.y_lim); + %%% z-axis visualization range [m] [[-0.08, 0.1], [-0.1, .14]] + ani_settings.z_vis_range = ani_settings.z_lim - mean(ani_settings.z_lim); +%%% Camera angle azimuth and elevation [deg] +ani_settings.camera_az = -25; +ani_settings.camera_el = 10; + +%%% Font name +ani_settings.font_name = 'Calibri'; +%%% Font size +ani_settings.font_size = 20; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Surface viz. on/off +ani_settings.surface_show = 'on'; + %%% Surface grid color + ani_settings.grid_color = 'white'; + %%% Surface color + ani_settings.surface_color = [0.5, 0.5, 0.5]; + +%%% Graspable points viz. on/off +ani_settings.graspable_points_show = 'off'; + %%% Graspable points color + ani_settings.graspable_points_color = [0.0, 0.8, 0.0]; + %%% Graspable points marker + ani_settings.graspable_points_marker = '.'; + %%% Graspable points size + ani_settings.graspable_points_size = 10; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Link radius [m] +ani_settings.link_radius = 0.030; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.20; +ani_settings.base_lower_thickness = 0.05; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.0; +%%% Robot color +ani_settings.robot_base_upper_color = [0.0, 0.3, 0.6]; +ani_settings.robot_base_lower_color = [0.3, 0.3, 0.3]; +ani_settings.robot_limb_color = [0.3, 0.3, 0.3]; +%%% Robot transparency +ani_settings.robot_base_alpha = 1.0; +ani_settings.robot_limb_alpha = 1.0; + +%%% Frames at each joint on/off +ani_settings.frames_show = 'off'; + ani_settings.frames_line_width = 2.0; + ani_settings.frames_size = 0.1; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Graspable points in reachable area viz. on/off +ani_settings.graspable_points_in_reachable_area_show = 'off'; + %%% Graspable points size + ani_settings.graspable_points_in_reachable_area_size = 40; + +%%% Reachable area viz. on/off +ani_settings.reachable_area_show = 'off'; + %%% Reachable area line width + ani_settings.reachable_area_line_width = 3; + +%%% Next desired position vis. on/off +ani_settings.next_desired_position_show = 'off'; + ani_settings.next_desired_position_color = [0.0, 0.7, 0.7]; + %%% Next desired position line width + ani_settings.next_desired_position_line_width = 3; + +%%% Plot trajectory +ani_settings.trajectory_show = 'off'; + %%% Trajectory line color + ani_settings.trajectory_color = [0.5, 0.5, 0.5]; + %%% Trajectory line widith + ani_settings.trajectory_width = 3; + %%% Trajectory line type + ani_settings.trajectory_line_type = ':'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Support triangle visualization +ani_settings.support_triangle_show = 'on'; + %%% Support triangle color + ani_settings.support_triangle_color = [0 136/255 170/255]; + %%% Support triangle transparency + ani_settings.support_triangle_edge_color = 'none'; + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + + %%% GIA Stable Region visualize on/off (requires equilibrium evaluation method as 'gia') + ani_settings.gia_stable_region_show = 'on'; + %%% Stability polyhedron face color + ani_settings.gia_stable_region_Face_color = [0 136/255 170/255]; + %%% Stability polyhedron face alpha + ani_settings.gia_stable_region_Face_alpha = 0.2; + %%% Stability polyhedron edge color + ani_settings.gia_stable_region_Edge_color = 'none'; + %%% Stability polyhedron edge width + ani_settings.gia_stable_region_Edge_width = 1; + + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'off'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [1.0, 0.0, 0.0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 6; + + %%% GIA vector visualize on/off + ani_settings.gia_vec_show = 'on'; + %%% GIA vector color + ani_settings.gia_vec_color = [1 0 0]; + %%% GIA vector width + ani_settings.gia_vec_width = 3; + +%%% Transformation from force to visualize Fg (gravitational force) and +%%% Fe (reaction force) Stable Region in the position coordinate +%%% -> this scale is used for all force-dimensional variables visualization +ani_settings.force_expansion_factor = 0.005; + + %%% Gravitational Force vector visualize on/off + ani_settings.gravitational_force_vec_show = 'off'; + %%% Gravity vector color + ani_settings.gravitational_force_vec_color = [0.7 0 0]; + %%% Gravity vector width + ani_settings.gravitational_force_vec_width = 3; + + %%% Reaction force vectors vis. on/off + ani_settings.reaction_force_show = 'off'; + %%% Reaction force vectors color + ani_settings.reaction_force_vec_color = [0.7 0 0.7]; + %%% Reaction force vectors color width + ani_settings.reaction_force_vec_width = 2; + +%%% CoM projection point vis. on/off +ani_settings.com_projection_show = 'off'; + %%% CoM projection point vis. marker size + ani_settings.com_projection_size = 40; + %%% CoM projection point vis. height from the ground + % visualization position can be offseted to locate it vertically upper to the ground to show it well + ani_settings.com_projection_vis_height = 0.04; % [m] + +%%% Goal vis. on/off +ani_settings.goal_show = 'off'; + %%% Goal vis. marker size + ani_settings.goal_size = 40; + %%% Goal vis. height from the ground + ani_settings.goal_vis_height = 0.04; % [m] + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% RealSense Camera fov vis. on/off +ani_settings.sensing_fov_show = 'off'; + %%% Sensing camera position marker size + ani_settings.sensing_camera_MarkerSize = 10; + %%% Sensing camera marker color + ani_settings.sensing_camera_MarkerColor = '.k'; + %%% Sensing camera fov line width + ani_settings.sensing_camera_fov_LineWidth = 1; + %%% Sensing camera fov line color + ani_settings.sensing_camera_fov_LineColor = 'w'; +%%% RealSense Camera fov filling vis. on/off +ani_settings.sensing_fov_face_filling = 'off'; + %%% Filling color + ani_settings.sensing_fov_face_color = 'w'; + %%% Filling color alpha + ani_settings.sensing_fov_face_alpha = 0.5; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Time interval for saving variables (should be larger than time-step) +save_settings.variable_saving_time_interval = 0.05; + +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; +%%% Save TSM variables to csv file on/off (requires equilibrium evaluation method as 'tsm') +save_settings.tsm = 'on'; +%%% Save GIA variables to csv file on/off (requires equilibrium evaluation method as 'gia') +save_settings.gia = 'on'; +%%% Save manipularbility measure to csv file on/off +save_settings.manipulability = 'off'; +%%% Save Maximum of absolute torque of all joint +save_settings.joint_max_torque = 'on'; +%%% Save Root Mean Square (RMS of torque of all joint (*need Signal Processing Toolbox) +save_settings.joint_rms_torque = 'on'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Plot Base position +plot_settings.base_pos = 'off'; +%%% Plot Joint torque +plot_settings.joint_torque = 'on'; +%%% Plot TSM (requires equilibrium evaluation method as 'tsm') +plot_settings.tsm = 'on'; +%%% Plot GIA (Acceleration) margin (requires equilibrium evaluation method as 'gia') +plot_settings.gia_margin = 'on'; +%%% Plot GIA Inclination margin (requires equilibrium evaluation method as 'gia') +plot_settings.gia_inclination_margin = 'on'; +%%% Plot manipulability +plot_settings.manipulability = 'off'; +%%% Plot max torque of all joint +plot_settings.joint_max_torque = 'on'; +%%% Plot RMS torque of all joint +plot_settings.joint_rms_torque = 'on'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gripper Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Map Parameters and settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Matching Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +matching_settings.submatching = 'on'; +matching_settings.colony = 'on'; +matching_settings.threshold = 130; + +end \ No newline at end of file diff --git a/config/preset/config_example_demo_3_param.m b/config/preset/config_example_demo_3_param.m new file mode 100644 index 0000000..e572603 --- /dev/null +++ b/config/preset/config_example_demo_3_param.m @@ -0,0 +1,336 @@ +%%%%%% +%%%%%% Configuration +%%%%%% config_example_demo_3_param +%%%%%% +%%%%%% Define simulation parameters for the representative simulation cases +%%%%%% of the ClimbLab +%%%%%% [ ] Demo (1) Static simulation to demonstrate the detachment of the +%%%%%% gripper from an over-hanging wall +%%%%%% [ ] Demo (2) Steep slope climbing of the mammalian typed robot +%%%%%% [x] Demo (3) Perceptive walking of the half human sized quadrupedal robot +%%%%%% +%%%%%% +%%%%%% -------------------------------------------------------------------- +%%%%%% This configration file can reproduce the similar result of CLAWAR +%%%%%% 2021 proceedings paper by K. Uno, W. Ribeiro et al. +%%%%%% Proceedings Paper URL: +%%%%%% https://******** (to be added) +%%%%%% -------------------------------------------------------------------- +%%%%%% +%%%%%% Created 2021-03-09 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-06-30 +%%%%%% Kentaro Uno +% +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% gripper_param +% map_param +% matching_settings +% sensing_camera_param +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_example_demo_3_param(robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ... + ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Type of the robot to simulate ('HubRobo_v3_2_grip_to_palm', 'ANYmal_B', 'ALPHRED') +robot_param.robot_type = 'ALPHRED'; +%%% x and y position of legs relative to base center [m] +robot_param.x_foot_dist = 0.20; +robot_param.y_foot_dist = 0.20; +%%% Height of base relative to map [m] +robot_param.base_height = 0.50; +%%% Base position [m] 2x1 vector. or 'default' for default setting +robot_param.base_pos_xy = [-0.8;0.0]; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gravity [G] +environment_param.grav = 1; +%%% Type of the surface ('flat_HR', 'rough', 'grid_map_35mm_1m_center_thinned_50', etc. Refer to ini_surface.m) +environment_param.surface_type = 'grid_3mx3m_dx100mm_thined_40'; +%%% Maximum simulation time [s] +environment_param.time_max = 100; +%%% Graspable points detection type 'gripper' 'all' 'peaks' 0 - 100 +environment_param.graspable_points_detection_type = 'all'; +%%% Dynamics on/off +environment_param.dynamics_flag = 'off'; +%%% Surface Inclination [deg] +environment_param.inc = 0; + +%%% Gripper detachment method ('none', 'max_holding_force', 'tsm') +environment_param.detachment_detection_method = 'max_holding_force'; + +%%% Ground reaction force stiffness coefficient +environment_param.surface_K = 100000; +%%% Ground reaction force damping coefficient (scalar) +environment_param.surface_D = 100; + +%%% Simulation stop reaching goal threshold +environment_param.sim_stop_goal_threshold = 0.10; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds', 'nonperiodic_gait_for_discrete_footholds') +gait_planning_param.type = 'nonperiodic_gait_for_discrete_footholds'; +%%% Step height [m] +gait_planning_param.step_height = 0.05; +gait_planning_param.step_length = 0.10; +%%% Period of one cycle [s] +gait_planning_param.T = 4; +%%% Duty cycle +gait_planning_param.beta = 0.75; +%%% Goal position [m] +gait_planning_param.goal = [1.0;0.0;0.0]; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.3; +%%% Update Moving Direction on/off +gait_planning_param.local_path_plan_flag = 'off'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller Proportional (P) gain +control_param.kp = 1500; +%%% Controller Derivative (D) gain +control_param.kd = 4.5; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia') +equilibrium_eval_param.type = 'tsm'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Link radius [m] +ani_settings.link_radius = 0.0185; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.07; +ani_settings.base_lower_thickness = 0.03; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.2; +%%% Robot color +ani_settings.robot_base_upper_color = [0.1, 0.1, 0.1]; +ani_settings.robot_base_lower_color = [0.1, 0.1, 0.1]; +ani_settings.robot_limb_color = [0.1, 0.1, 0.1]; +%%% Robot transparency +ani_settings.robot_base_alpha = 0.8; +ani_settings.robot_limb_alpha = 0.8; + +%%% Frames at each joint on/off +ani_settings.frames_show = 'off'; + ani_settings.frames_line_width = 2.0; + ani_settings.frames_size = 0.05; + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] +ani_settings.x_lim = [-1.5, 1.5]; +%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] +ani_settings.y_lim = [-1.5, 1.5]; +%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] +ani_settings.z_lim = [-0.0, 0.7]; +%%% Lower z-axis limit same as lower point on surface on/off +ani_settings.z_lim_low_is_surface = 'off'; + +%%% 'on' camera follows robot, 'off' camera fixed +ani_settings.move_camera = 'off'; + %%% x-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] + ani_settings.x_vis_range = ani_settings.x_lim - mean(ani_settings.x_lim); + %%% y-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25]] + ani_settings.y_vis_range = ani_settings.y_lim - mean(ani_settings.y_lim); + %%% z-axis visualization range [m] [[-0.08, 0.1], [-0.1, .14]] + ani_settings.z_vis_range = ani_settings.z_lim - mean(ani_settings.z_lim); +%%% Camera angle azimuth and elevation [deg] +ani_settings.camera_az = -10; %Default -40 +ani_settings.camera_el = 20; %default 10 + +%%% Font name +ani_settings.font_name = 'Calibri'; +%%% Font size +ani_settings.font_size = 25; + +%%% Frame rate [frames/s] [20,25,50] +ani_settings.frame_rate = 10; +%%% Animation video resolution [px] [[1280 720], [640 480]] +ani_settings.animation_resolution = [1280 720]; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Surface viz. on/off +ani_settings.surface_show = 'on'; + %%% Surface grid color + ani_settings.grid_color = 'none'; + %%% Surface color + ani_settings.surface_color = gray; + +%%% Graspable points viz. on/off +ani_settings.graspable_points_show = 'on'; + %%% Graspable points color + ani_settings.graspable_points_color = [0.8, 0.8, 0.8]; + %%% Graspable points alpha + ani_settings.graspable_points_alpha = 1.0; + %%% Graspable points marker + ani_settings.graspable_points_marker = 'o'; + %%% Graspable points size + ani_settings.graspable_points_size = 20; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Graspable points in reachable area viz. on/off +ani_settings.graspable_points_in_reachable_area_show = 'off'; + +%%% Reachable area viz. on/off +ani_settings.reachable_area_show = 'on'; + %%% Reachable area line color + ani_settings.reachable_area_line_color = 'm'; + %%% Reachable area line width + ani_settings.reachable_area_line_width = 1; + + +%%% Moving Direction viz. on/off +ani_settings.moving_direction_show = 'off'; + ani_settings.moving_direction_vec_color = [1 0 0]; + ani_settings.moving_direction_vec_width = 3; + ani_settings.moving_direction_expansion_factor = 0.08; + + +%%% Next desired position vis. on/off +ani_settings.next_desired_position_show = 'on'; + +%%% Plot trajectory +ani_settings.trajectory_show = 'off'; + %%% Trajectory line color + ani_settings.trajectory_color = [0.5, 0.5, 0.5]; + %%% Trajectory line widith + ani_settings.trajectory_width = 3; + %%% Trajectory line type + ani_settings.trajectory_line_type = ':'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Support triangle visualization +ani_settings.support_triangle_show = 'on'; + %%% Support triangle color + ani_settings.support_triangle_color = [0 136/255 170/255]; + %%% Support triangle transparency + ani_settings.support_triangle_edge_color = 'none'; + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'on'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [1 0 0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 3; + +%%% Transformation from force to visualize Fg (gravitational force) and +%%% Fe (reaction force) Stable Region in the position coordinate +%%% -> this scale is used for all force-dimensional variables visualization +ani_settings.force_expansion_factor = 0.005; + ani_settings.reaction_force_show = 'off'; + +%%% CoM projection point vis. on/off +ani_settings.com_projection_show = 'off'; + +%%% Goal vis. on/off +ani_settings.goal_show = 'on'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% RealSense Camera fov vis. on/off +ani_settings.sensing_fov_show = 'on'; + %%% Sensing camera position marker size + ani_settings.sensing_camera_MarkerSize = 1; + %%% Sensing camera marker color + ani_settings.sensing_camera_MarkerColor = '.k'; + %%% Sensing camera fov line width + ani_settings.sensing_camera_fov_LineWidth = 1; + %%% Sensing camera fov line color + ani_settings.sensing_camera_fov_LineColor = 'w'; +%%% RealSense Camera fov filling vis. on/off +ani_settings.sensing_fov_face_filling = 'on'; + %%% Filling color + ani_settings.sensing_fov_face_color = 'w'; + %%% Filling color alpha + ani_settings.sensing_fov_face_alpha = 0.2; + +%%% Sensed Graspable points viz. on/off +ani_settings.sensed_graspable_points_show = 'on'; + %%% Graspable points color + ani_settings.sensed_graspable_points_color = [0 136/255 170/255]; + %%% Graspable points alpha + ani_settings.sensed_graspable_points_alpha = 1.0; + %%% Graspable points marker + ani_settings.sensed_graspable_points_marker = 'o'; + %%% Graspable points size + ani_settings.sensed_graspable_points_size = 20; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Time interval for saving variables (should be larger than time-step) +save_settings.variable_saving_time_interval = 0.05; + +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; + +%%% Save TSM variables to csv file on/off +save_settings.tsm = 'on'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Plot footholds history on/off +plot_settings.footholds = 'on'; +%%% Plot TSM +plot_settings.tsm = 'on'; + +plot_settings.joint_torque = 'on'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Sensing Camera on/off +sensing_camera_param.sensing_flag = 'on'; +%%% Sensing type +sensing_camera_param.sensing_type = 'RealSense_d435i'; % 'circle' or 'RealSense_d435i' +%%% Known area shape +sensing_camera_param.known_area_shape = 'circle'; %'rectangle' or 'circle' + +sensing_camera_param.mounting_angle = [0; -60; 0]*pi/180; % rpy +sensing_camera_param.mounting_position = [0.08599/sqrt(2); 0 ; 0.07]; %[m] + +sensing_camera_param.fov_min_distance = 0.0; %[m] + +%%% Circle Param +sensing_camera_param.ini_circular_radius_from_base_pos = 0.6; %[m] + + +end \ No newline at end of file diff --git a/config/preset/config_gia_static.m b/config/preset/config_gia_static_param.m similarity index 50% rename from config/preset/config_gia_static.m rename to config/preset/config_gia_static_param.m index 405af98..4c2af2a 100644 --- a/config/preset/config_gia_static.m +++ b/config/preset/config_gia_static_param.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-07-09 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-09 +%%%%%% Last update: 2021-02-22 +%%%%%% Kentaro Uno % % % Load configurations for a template of GIA polyhedron in static case @@ -15,7 +16,7 @@ % OUTPUT % robot_param % environment_param -% gait_param +% gait_planning_param % control_param % equilibrium_eval_param % ani_settings @@ -24,14 +25,15 @@ % INPUT % - -function [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings] = config_gia_static(robot_param, environment_param, gait_param, control_param, ... +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_gia_static_param(robot_param, environment_param, gait_planning_param, control_param, ... equilibrium_eval_param, ani_settings, save_settings, plot_settings) %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') robot_param.robot_type = 'HubRobo_grip_to_spine_old'; %%% x and y position of legs relative to base center [m] -robot_param.foot_dist = 0.12; +robot_param.x_foot_dist = 0.12; +robot_param.y_foot_dist = 0.12; %%% Height of base relative to map [m] robot_param.base_height = 0.14; @@ -52,22 +54,20 @@ %%% Gripper detachment method ('none', 'max_holding_force') environment_param.detachment_detection_method = 'none'; +%%% Simulation stop if robot is stuck (selecting same grasping point) +environment_param.sim_stop_stuck = 'off'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver') -gait_param.type = 'do_nothing'; +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds') +gait_planning_param.type = 'do_nothing'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Controller type ('do_nothing', 'torque_PD') control_param.type = 'do_nothing'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Equilibrium evaluation method ('none', 'tsm', 'gia') +%%% Equilibrium evaluation method ('none', 'tsm', 'gia', 'tsm_and_gia') equilibrium_eval_param.type = 'gia'; -% Plot polyhderon on animation on/off (requires equilibrium evaluation method as 'gia') -equilibrium_eval_param.plot_polyhedron = 'on'; -% Transformation from acceleration to plot in the position coordinate -equilibrium_eval_param.expansion_factor = 0.02; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] @@ -87,36 +87,66 @@ %%% Link radius [m] ani_settings.link_radius = 0.010; %%% Base thickness [m] -ani_settings.base_thickness = 0.03; - +ani_settings.base_upper_thickness = 0.03; +ani_settings.base_lower_thickness = 0.03; %%% Robot color -ani_settings.robot_color = [0.2, 0.2, 0]; +ani_settings.robot_base_upper_color = [0.2, 0.2, 0]; +ani_settings.robot_base_lower_color = [0.2, 0.2, 0]; +ani_settings.robot_limb_color = [0.2, 0.2, 0]; %%% Robot transparency -ani_settings.robot_alpha = 0.5; - -%%% Stability polyhedron face color -ani_settings.polyh_Face_color = [0 0 1]; -%%% Stability polyhedron face alpha -ani_settings.polyh_Face_alpha = 0.25; -%%% Stability polyhedron edge color -ani_settings.polyh_Edge_color = [0 0 1]; -%%% Stability polyhedron edge width -ani_settings.polyh_Edge_width = 2; - -%%% Gravity vector visualize on/off -ani_settings.gravity_vec_show = 'on'; - %%% Gravity vector color - ani_settings.gravity_vec_color = [0 0.7 0]; - %%% Gravity vector width - ani_settings.gravity_vec_width = 3; - -%%% GIA vector visualize on/off -ani_settings.gia_vec_show = 'on'; - %%% GIA vector color - ani_settings.gia_vec_color = [1 0 0]; - %%% GIA vector width - ani_settings.gia_vec_width = 3; +ani_settings.robot_base_alpha = 0.5; +ani_settings.robot_limb_alpha = 0.5; + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + + %%% GIA Stable Region visualize on/off (requires equilibrium evaluation method as 'gia') + ani_settings.gia_stable_region_show = 'on'; + %%% Stability polyhedron face color + ani_settings.polyh_Face_color = [0 0 1]; + %%% Stability polyhedron face alpha + ani_settings.polyh_Face_alpha = 0.25; + %%% Stability polyhedron edge color + ani_settings.polyh_Edge_color = [0 0 1]; + %%% Stability polyhedron edge width + ani_settings.polyh_Edge_width = 2; + + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'on'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [0 0.7 0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 3; + + %%% GIA vector visualize on/off + ani_settings.gia_vec_show = 'on'; + %%% GIA vector color + ani_settings.gia_vec_color = [1 0 0]; + %%% GIA vector width + ani_settings.gia_vec_width = 3; + +%%% Transformation from force to visualize Fg (gravitational force) and +%%% Fe (reaction force) Stable Region in the position coordinate +%%% -> this scale is used for all force-dimensional variables visualization +ani_settings.force_expansion_factor = 0.04; + + %%% Gravitational Force vector visualize on/off + ani_settings.gravitational_force_vec_show = 'off'; + %%% Gravity vector color + ani_settings.gravitational_force_vec_color = [0 0.7 0]; + %%% Gravity vector width + ani_settings.gravitational_force_vec_width = 3; + %%% Reaction force vectors vis. on/off + ani_settings.reaction_force_show = 'off'; + %%% Reaction force vectors color + ani_settings.reaction_force_vec_color = [1 0 0]; + %%% Reaction force vectors color width + ani_settings.reaction_force_vec_width = 3; + +%%% Trajectory visualization on/off ani_settings.trajectory_show = 'off'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/config/preset/config_hubrobo_testfield_exp_param.m b/config/preset/config_hubrobo_testfield_exp_param.m new file mode 100644 index 0000000..f9fa3df --- /dev/null +++ b/config/preset/config_hubrobo_testfield_exp_param.m @@ -0,0 +1,335 @@ +%%%%%% +%%%%%% Configuration +%%%%%% config_hubrobo_testfield_exp_param +%%%%%% +%%%%%% Define simulation parameters for the target detection and nonperiodic +%%%%%% swing limb selection on the hubrobo testfield +%%%%%% +%%%%%% Creation: 2021-04-12 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-04-16 +%%%%%% Keigo Haji +% +% +% Load configurations for parameters to simulate HubRobo experiment on the +% bouldering holds testfield +% +% Function variables: +% +% OUTPUT +% robot_param +% environment_param +% gait_planning_param +% control_param +% equilibrium_eval_param +% ani_settings +% save_settings +% plot_settings +% gripper_param +% map_param +% matching_settings +% sensing_camera_param +% INPUT +% - + +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_hubrobo_testfield_exp_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param) +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') +robot_param.robot_type = 'HubRobo_v3_2_grip_to_palm'; +%%% x and y position of legs relative to base center [m] +robot_param.x_foot_dist = 0.150; +robot_param.y_foot_dist = 0.150; +%%% Height of base relative to map [m] + % robot_param.base_height = 0.060; + robot_param.base_height = 0.080; + +%%% Base position [m] 2x1 vector + robot_param.base_pos_xy = [-0.4;0.1]; +% robot_param.base_pos_xy = [-0.6;-0.3]; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Following Values are written in RealSense D435i datasheet. +sensing_camera_param.fov_horizontal = 86*pi/180; %[radian] +sensing_camera_param.fov_vertical = 57*pi/180; %[radian] +sensing_camera_param.fov_max_distance = 0.3; %[m] +sensing_camera_param.fov_min_distance = 0.05; %[m] + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Gravity [G] +environment_param.grav = 1/6; + +%%% Type of the surface ('flat_HR', 'rough', 'climbing_holds_1m_x_1m', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') +environment_param.surface_type = 'climbing_holds_map_on_full_testfield'; + if strcmp(environment_param.surface_type, 'climbing_holds_map_on_full_testfield') + robot_param.base_pos_xy = [-0.6;-0.3]; + end + + +%%% Maximum simulation time [s] +environment_param.time_max = 96; + +%%% Graspable points detection type 'gripper' or 'climbing_holds_map_on_testfield' + % If 'climbing_holds_map_on_testfield' is selected, you can skip the + % target_detection, and get the result directly. + environment_param.graspable_points_detection_type = 'climbing_holds_map_on_full_testfield'; + +%%% Dynamics on/off +environment_param.dynamics_flag = 'off'; + +%%% Surface Inclination [deg] + environment_param.inc = 0; + % environment_param.inc = 30; + % environment_param.inc = 45; + % environment_param.inc = 60; + % environment_param.inc = 75; + % environment_param.inc = 90; + +%%% Gripper detachment method ('none', 'max_holding_force', 'tsm') +environment_param.detachment_detection_method = 'max_holding_force'; + +%%% Simulation stop reaching goal threshold + % environment_param.sim_stop_goal_threshold = 0.03; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds', nonperiodic_gait_for_discrete_footholds) +gait_planning_param.type = 'nonperiodic_gait_for_discrete_footholds'; + +%%% Step height [m] +gait_planning_param.step_height = 0.04; + +%%% Period of one cycle [s] + if strcmp(environment_param.dynamics_flag, 'on') + gait_planning_param.T = 16; + end + if strcmp(environment_param.dynamics_flag, 'off') + gait_planning_param.T = 4; + end + +%%% Duty cycle +gait_planning_param.beta = 0.75; + +%%% Goal position [m] + gait_planning_param.goal = [0.4;-0.1;0.0]; + if strcmp(environment_param.surface_type, 'climbing_holds_map_on_full_testfield') + gait_planning_param.goal = [0.8;0.3;0.0]; + end + + +%%% Allowable maximum stride [m] + % gait_planning_param.allowable_max_stride = 0.1; + gait_planning_param.allowable_max_stride = 0.5; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% PD controller settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Controller Proportional (P) gain +control_param.kp = 10.0; +%%% Controller Derivative (D) gain +control_param.kd = 0.30; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Equilibrium evaluation method ('none', 'tsm', 'gia') +equilibrium_eval_param.type = 'tsm'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Frame rate [frames/s] [20,25,50] +ani_settings.frame_rate = 20; +%%% Animation video resolution [px] [[1280 720], [640 480]] +ani_settings.animation_resolution = [1280 720]; + +%%% Link radius [m] +ani_settings.link_radius = 0.0185; +%%% Base thickness [m] +ani_settings.base_upper_thickness = 0.07; +ani_settings.base_lower_thickness = 0.03; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.05; +%%% Robot color +ani_settings.robot_base_upper_color = [0.1, 0.1, 0.1]; +ani_settings.robot_base_lower_color = [0.1, 0.1, 0.1]; +ani_settings.robot_limb_color = [0.1, 0.1, 0.1]; +%%% Robot transparency +ani_settings.robot_base_alpha = 0.72; +ani_settings.robot_limb_alpha = 0.72; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] +ani_settings.x_lim = [-0.6, 0.6]; +%%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] +ani_settings.y_lim = [-0.5, 0.5]; + + if strcmp(environment_param.surface_type, 'climbing_holds_map_on_full_testfield') + ani_settings.x_lim = [-0.85, 1]; + ani_settings.y_lim = [-0.6, 0.6]; + end + +%%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] +ani_settings.z_lim = [-0.05, 0.30]; +%%% Lower z-axis limit same as lower point on surface on/off +ani_settings.z_lim_low_is_surface = 'off'; + +%%% 'on' camera follows robot, 'off' camera fixed +ani_settings.move_camera = 'off'; + %%% x-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] + ani_settings.x_vis_range = ani_settings.x_lim - mean(ani_settings.x_lim); + %%% y-axis visualization range [m] [[-0.25, 0.4], [-0.25, .25]] + ani_settings.y_vis_range = ani_settings.y_lim - mean(ani_settings.y_lim); + %%% z-axis visualization range [m] [[-0.08, 0.1], [-0.1, .14]] + ani_settings.z_vis_range = ani_settings.z_lim - mean(ani_settings.z_lim); +%%% Camera angle azimuth and elevation [deg] +ani_settings.camera_az = -40; +ani_settings.camera_el = 20; + +%%% Font name +ani_settings.font_name = 'Times New Roman'; +%%% Font size +ani_settings.font_size = 25; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Surface viz. on/off +ani_settings.surface_show = 'on'; + %%% Surface grid color + ani_settings.grid_color = [0.9 0.9 0.9]; + %%% Surface color + ani_settings.surface_color = gray; + +%%% Graspable points viz. on/off +ani_settings.graspable_points_show = 'on'; + %%% Graspable points color + ani_settings.graspable_points_color = [0.0, 1.0, 0.0]; + %%% Graspable points marker + ani_settings.graspable_points_marker = 'o'; + %%% Graspable points size + ani_settings.graspable_points_size = 20; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Graspable points in reachable area viz. on/off +ani_settings.graspable_points_in_reachable_area_show = 'on'; + %%% Graspable points size + ani_settings.graspable_points_in_reachable_area_size = 40; + +%%% Reachable area viz. on/off +ani_settings.reachable_area_show = 'on'; + %%% Reachable area line width + ani_settings.reachable_area_line_width = 3; + +%%% Next desired position vis. on/off +ani_settings.next_desired_position_show = 'on'; + ani_settings.next_desired_position_color = [0.0, 0.7, 0.7]; + %%% Next desired position line width + ani_settings.next_desired_position_line_width = 3; + +%%% Plot trajectory +ani_settings.trajectory_show = 'off'; + %%% Trajectory line color + ani_settings.trajectory_color = [0.5, 0.5, 0.5]; + %%% Trajectory line widith + ani_settings.trajectory_width = 3; + %%% Trajectory line type + ani_settings.trajectory_line_type = ':'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Support triangle visualization +ani_settings.support_triangle_show = 'on'; + %%% Support triangle transparency + ani_settings.support_triangle_edge_color = 'none'; + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'on'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [1.0, 0.0, 0.0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 6; + +%%% CoM projection point vis. on/off +ani_settings.com_projection_show = 'on'; + %%% CoM projection point vis. marker size + ani_settings.com_projection_size = 40; + %%% CoM projection point vis. height from the ground + % visualization position can be offseted to locate it vertically upper to the ground to show it well + ani_settings.com_projection_vis_height = 0.04; % [m] + +%%% Goal vis. on/off +ani_settings.goal_show = 'on'; + %%% Goal vis. marker size + ani_settings.goal_size = 40; + %%% Goal vis. height from the ground + ani_settings.goal_vis_height = 0.04; % [m] + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% RealSense Camera fov vis. on/off +ani_settings.sensing_fov_show = 'off'; + %%% Sensing camera position marker size + ani_settings.sensing_camera_MarkerSize = 1; + %%% Sensing camera marker color + ani_settings.sensing_camera_MarkerColor = '.k'; + %%% Sensing camera fov line width + ani_settings.sensing_camera_fov_LineWidth = 1; + %%% Sensing camera fov line color + ani_settings.sensing_camera_fov_LineColor = 'w'; +%%% RealSense Camera fov filling vis. on/off +ani_settings.sensing_fov_face_filling = 'on'; + %%% Filling color + ani_settings.sensing_fov_face_color = 'w'; + %%% Filling color alpha + ani_settings.sensing_fov_face_alpha = 0.2; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Save basic variables to csv file on/off +save_settings.csv_file = 'on'; + +%%% Save TSM variables to csv file on/off +save_settings.tsm = 'on'; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% Plot footholds history on/off +plot_settings.footholds = 'on'; + +%%% Plot TSM +plot_settings.tsm = 'on'; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gripper Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Map Parameters and settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Matching Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +matching_settings.submatching = 'on'; +matching_settings.colony = 'on'; +matching_settings.threshold = 120; + +% By this switch, the targets detected on the board will be deleted +matching_settings.delete_lower_targets = 'on'; + + +end \ No newline at end of file diff --git a/config/preset/config_iSAIRAS_2020_demo_param.m b/config/preset/config_iSAIRAS_2020_demo_param.m index 1f4c936..c81a4c5 100644 --- a/config/preset/config_iSAIRAS_2020_demo_param.m +++ b/config/preset/config_iSAIRAS_2020_demo_param.m @@ -15,7 +15,7 @@ %%%%%% %%%%%% Creation: 2020-09-10 %%%%%% Kentaro Uno -%%%%%% Last update: 2020-10-26 +%%%%%% Last update: 2021-02-22 %%%%%% Kentaro Uno % % @@ -27,7 +27,7 @@ % OUTPUT % robot_param % environment_param -% gait_param +% gait_planning_param % control_param % equilibrium_eval_param % ani_settings @@ -36,22 +36,32 @@ % INPUT % - -function [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings, gripper_param, map_param, matching_settings] = config_USER_param(robot_param, environment_param, gait_param, control_param, ... - equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings) +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_iSAIRAS_2020_demo_param(robot_param, environment_param, gait_planning_param, control_param, ... + equilibrium_eval_param, ani_settings, save_settings, plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param) %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') -robot_param.robot_type = 'HubRobo_v3_1_grip_to_palm'; +robot_param.robot_type = 'HubRobo_v3_2_grip_to_palm'; %%% x and y position of legs relative to base center [m] -robot_param.foot_dist = 0.150; +robot_param.x_foot_dist = 0.150; +robot_param.y_foot_dist = 0.150; %%% Height of base relative to map [m] robot_param.base_height = 0.060; %%% Base position [m] 2x1 vector. or 'default' for default setting robot_param.base_pos_xy = [0.2;0.5]; +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Following Values are written in RealSense D435i datasheet. +sensing_camera_param.fov_horizontal = 86*pi/180; %[radian] +sensing_camera_param.fov_vertical = 57*pi/180; %[radian] +sensing_camera_param.fov_max_distance = 0.3; %[m] +sensing_camera_param.fov_min_distance = 0.05; %[m] + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -76,16 +86,18 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver', nonperiodic_uno_ver) -gait_param.type = 'nonperiodic_uno_ver'; +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds', nonperiodic_gait_for_discrete_footholds) +gait_planning_param.type = 'nonperiodic_gait_for_discrete_footholds'; %%% Step height [m] -gait_param.step_height = 0.05; +gait_planning_param.step_height = 0.04; %%% Period of one cycle [s] -gait_param.T = 16; +gait_planning_param.T = 16; %%% Duty cycle -gait_param.beta = 0.75; +gait_planning_param.beta = 0.75; %%% Goal position [m] -gait_param.goal = [0.4;0.5;0.0]; +gait_planning_param.goal = [0.4;0.5;0.0]; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.1; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -114,9 +126,17 @@ %%% Link radius [m] ani_settings.link_radius = 0.0185; %%% Base thickness [m] -ani_settings.base_thickness = 0.07; +ani_settings.base_upper_thickness = 0.07; +ani_settings.base_lower_thickness = 0.03; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.05; +%%% Robot color +ani_settings.robot_base_upper_color = [0.1, 0.1, 0.1]; +ani_settings.robot_base_lower_color = [0.1, 0.1, 0.1]; +ani_settings.robot_limb_color = [0.1, 0.1, 0.1]; %%% Robot transparency -ani_settings.robot_alpha = 0.72; +ani_settings.robot_base_alpha = 0.72; +ani_settings.robot_limb_alpha = 0.72; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] @@ -195,12 +215,18 @@ ani_settings.support_triangle_show = 'on'; %%% Support triangle transparency ani_settings.support_triangle_edge_color = 'none'; - -%%% Gravity vector visualize on/off -ani_settings.gravity_vec_show = 'on'; - %%% Gravity vector color - ani_settings.gravity_vec_color = [1.0, 0.0, 0.0]; - + +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.02; + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'on'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [1.0, 0.0, 0.0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 6; + %%% CoM projection point vis. on/off ani_settings.com_projection_show = 'on'; %%% CoM projection point vis. marker size @@ -208,6 +234,7 @@ %%% CoM projection point vis. height from the ground % visualization position can be offseted to locate it vertically upper to the ground to show it well ani_settings.com_projection_vis_height = 0.04; % [m] + %%% Goal vis. on/off ani_settings.goal_show = 'on'; %%% Goal vis. marker size @@ -215,6 +242,23 @@ %%% Goal vis. height from the ground ani_settings.goal_vis_height = 0.04; % [m] +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% RealSense Camera fov vis. on/off +ani_settings.sensing_fov_show = 'on'; + %%% Sensing camera position marker size + ani_settings.sensing_camera_MarkerSize = 1; + %%% Sensing camera marker color + ani_settings.sensing_camera_MarkerColor = '.k'; + %%% Sensing camera fov line width + ani_settings.sensing_camera_fov_LineWidth = 1; + %%% Sensing camera fov line color + ani_settings.sensing_camera_fov_LineColor = 'w'; +%%% RealSense Camera fov filling vis. on/off +ani_settings.sensing_fov_face_filling = 'on'; + %%% Filling color + ani_settings.sensing_fov_face_color = 'w'; + %%% Filling color alpha + ani_settings.sensing_fov_face_alpha = 0.2; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -233,8 +277,6 @@ plot_settings.footholds = 'on'; %%% Plot TSM plot_settings.tsm = 'on'; -%%% Plot TSM -plot_settings.tsm = 'on'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gripper Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/config/preset/config_nonperiodic_demo_param.m b/config/preset/config_nonperiodic_gait_for_discrete_footholds_param.m similarity index 74% rename from config/preset/config_nonperiodic_demo_param.m rename to config/preset/config_nonperiodic_gait_for_discrete_footholds_param.m index 193b001..d28d8d3 100644 --- a/config/preset/config_nonperiodic_demo_param.m +++ b/config/preset/config_nonperiodic_gait_for_discrete_footholds_param.m @@ -1,23 +1,23 @@ -%%%%%% %%%%%% Configuration -%%%%%% config_nonperiodic_demo_param +%%%%%% config_nonperiodic_gait_for_discrete_footholds_param %%%%%% -%%%%%% Define simulation parameters for the nonperiodic swing limb -%%%%%% selection demonstration +%%%%%% Define parameters for the nonperiodic swing limb selection demonstration %%%%%% %%%%%% Created 2020-09-10 %%%%%% Kentaro Uno -%%%%%% Last update: 2020-09-15 +%%%%%% Last updated: 2021-05-17 +%%%%%% Kentaro Uno % % -% Load configurations for parameters defined for the nonperiodic gait +% Load configurations for parameters defined for the nonperiodic gait for discrete +% footholds simulation % % Function variables: % % OUTPUT % robot_param % environment_param -% gait_param +% gait_planning_param % control_param % equilibrium_eval_param % ani_settings @@ -26,21 +26,24 @@ % INPUT % - -function [robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings] = config_nonperiodic_demo_param(robot_param, environment_param, gait_param, control_param, ... +function [robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings] = config_nonperiodic_gait_for_discrete_footholds_param(robot_param, environment_param, gait_planning_param, control_param, ... equilibrium_eval_param, ani_settings, save_settings, plot_settings) %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Type of the robot to simulate ('HubRobo_v2_2_no_grip', 'HubRobo_v2_2_grip_to_palm', 'HubRobo_v3_1_grip_to_palm', 'HubRobo_v2_2_grip_to_spine') -robot_param.robot_type = 'HubRobo_v3_1_grip_to_palm'; +robot_param.robot_type = 'HubRobo_v3_2_grip_to_palm'; %%% x and y position of legs relative to base center [m] -robot_param.foot_dist = 0.150; +robot_param.x_foot_dist = 0.16; +robot_param.y_foot_dist = 0.16; +%%% initial yaw angle [deg] +robot_param.initial_yaw = 30; %%% Height of base relative to map [m] robot_param.base_height = 0.060; %%% Base position [m] 2x1 vector. or 'default' for default setting -robot_param.base_pos_xy = [0.0;0.0]; +robot_param.base_pos_xy = [-0.3;-0.2;0.0]; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -48,7 +51,7 @@ %%% Gravity [G] environment_param.grav = 1/6; %%% Type of the surface ('flat_HR', 'rough', 'flat_003', 'flat_006', 'flat_008', 'flat_009', 'flat_012') -environment_param.surface_type = 'flat_009'; +environment_param.surface_type = 'flat_006'; %%% Maximum simulation time [s] environment_param.time_max = 20; %%% Graspable points detection type @@ -56,7 +59,7 @@ %%% Dynamics on/off environment_param.dynamics_flag = 'on'; %%% Surface Inclination [deg] -environment_param.inc = 20; +environment_param.inc = 10; %%% Gripper detachment method ('none', 'max_holding_force', 'tsm') environment_param.detachment_detection_method = 'max_holding_force'; @@ -66,16 +69,18 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_uno_ver', nonperiodic_uno_ver) -gait_param.type = 'nonperiodic_uno_ver'; +%%% Gait type ('do_nothing', 'crawl_fixed_stride','crawl_gait_for_discrete_footholds', nonperiodic_gait_for_discrete_footholds) +gait_planning_param.type = 'nonperiodic_gait_for_discrete_footholds'; %%% Step height [m] -gait_param.step_height = 0.04; +gait_planning_param.step_height = 0.03; %%% Period of one cycle [s] -gait_param.T = 4; +gait_planning_param.T = 8; %%% Duty cycle -gait_param.beta = 0.75; +gait_planning_param.beta = 0.75; %%% Goal position [m] -gait_param.goal = [0.4;0.4;-0.08]; +gait_planning_param.goal = [0.3;0.2;0.0]; +%%% Allowable maximum stride [m] +gait_planning_param.allowable_max_stride = 0.15; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Control Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -97,19 +102,33 @@ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% Link radius [m] -ani_settings.link_radius = 0.017; +ani_settings.link_radius = 0.0185; %%% Base thickness [m] -ani_settings.base_thickness = 0.07; +ani_settings.base_upper_thickness = 0.07; +ani_settings.base_lower_thickness = 0.03; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.2; +%%% Robot color +ani_settings.robot_base_upper_color = [0.1, 0.1, 0.1]; +ani_settings.robot_base_lower_color = [0.1, 0.1, 0.1]; +ani_settings.robot_limb_color = [0.1, 0.1, 0.1]; %%% Robot transparency -ani_settings.robot_alpha = 0.8; +ani_settings.robot_base_alpha = 0.8; +ani_settings.robot_limb_alpha = 0.8; + +%%% Frames at each joint on/off +ani_settings.frames_show = 'on'; + ani_settings.frames_line_width = 2.0; + ani_settings.frames_size = 0.05; + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Camera related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%% x-axis limits [m] [[-0.25, 0.4], [-0.25, .25], [0, .5]] -ani_settings.x_lim = [-0.25, 0.8]; +ani_settings.x_lim = [-0.5, 0.5]; %%% y-axis limits [m] [[-0.25, 0.4], [-0.25, .25]] -ani_settings.y_lim = [-0.25, 0.5]; +ani_settings.y_lim = [-0.5, 0.5]; %%% z-axis limits [m] [[-0.08, 0.1], [-0.1, .14]] [-0.25 0.25] -ani_settings.z_lim = [-0.25, 0.25]; +ani_settings.z_lim = [-0.10, 0.25]; %%% Lower z-axis limit same as lower point on surface on/off ani_settings.z_lim_low_is_surface = 'off'; @@ -170,9 +189,17 @@ %%% Support triangle visualization ani_settings.support_triangle_show = 'on'; -%%% Gravity vector visualize on/off -ani_settings.gravity_vec_show = 'on'; - +%%% Transformation from acceleration to visualize GIA Stable Region, GIA +%%% vector in the position coordinate -> this scale is used for all +%%% accelerational variables visualization +ani_settings.acceleration_expansion_factor = 0.04; + %%% Gravitational Acceleration vector visualize on/off + ani_settings.gravitational_acceleration_vec_show = 'on'; + %%% Gravity Acceleration vector color + ani_settings.gravitational_acceleration_vec_color = [0.7 0 0]; + %%% Gravity Acceleration vector width + ani_settings.gravitational_acceleration_vec_width = 6; + %%% CoM projection point vis. on/off ani_settings.com_projection_show = 'on'; @@ -186,7 +213,7 @@ %%% Save basic variables to csv file on/off save_settings.csv_file = 'on'; -%%% Save TSM variables to csv file on/off +%%% Save TSM variables to csv file on/off (requires equilibrium evaluation method as 'tsm') save_settings.tsm = 'on'; %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Plot Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff --git a/docs/media/climblab.png b/docs/media/climblab.png new file mode 100644 index 0000000..bc5c309 Binary files /dev/null and b/docs/media/climblab.png differ diff --git a/lib/equ_gia/equ_gia_acceleration_margin.m b/lib/equ_gia/equ_gia_acceleration_margin.m index e163dad..6836e39 100644 --- a/lib/equ_gia/equ_gia_acceleration_margin.m +++ b/lib/equ_gia/equ_gia_acceleration_margin.m @@ -5,17 +5,18 @@ %%%%%% %%%%%% Created 2020-02-05 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-06-16 +%%%%%% Last update: 2021-02-12 +%%%%%% Kentaro Uno % % Considering the tumble stability with the addition of gripping forces to prevent the tumbling motion, the following % equation describe the limit condition for one tumbling axis % % m.a_gi.{(pg-pa)x(pg-pb)} = sum{Fj.{(pb-pj)x(pa-pj)}} + M0.(pa-pb) + F0.(pbxpa) % -% The acceleration margin is the following, considering all tumbling axes +% The GIA acceleration margin is the following, considering all tumbling axes % % a_gi.{(pg-pa)x(pg-pb)} -% acc_marg = ||a_gi_lim|| - ---------------------------- +% gia_marg = ||a_gi_lim|| - ---------------------------- % ||(pg-pa)x(pg-pb)|| % % @@ -27,30 +28,30 @@ % Function variables: % % OUTPUT -% acc_margin : Acceleration margin considering the stability polyhedron [m/s^2] (scalar) -% acc_margin_ab : Acceleration margin for each tumbling axis [m/s^2] (1xn vector) +% gia_margin : Acceleration margin considering the stability polyhedron [m/s^2] (scalar) +% gia_margin_ab : Acceleration margin for each tumbling axis [m/s^2] (1xn vector) % INPUT % polyhedron : Variables to define the shape of the polyhedron (struct) % gia : Gravito-Inertial Acceleration [m/s^2] (3x1 vector) % equ_flag : Flag of equilibrium condition (1: equilibrium, 0: not in equilibrium) (scalar) -function [acc_margin, acc_margin_ab] = equ_gia_acceleration_margin(polyhedron, gia, equ_flag) +function [gia_margin, gia_margin_ab] = equ_gia_acceleration_margin(polyhedron, gia, equ_flag) % Number of tumbling axes tumbling_axes_number = size(polyhedron.plane_vector,2); % Initialize variable -acc_margin_ab = zeros(1,tumbling_axes_number); +gia_margin_ab = zeros(1,tumbling_axes_number); if equ_flag == 0 % If not in equilibrium, margin is zero - acc_margin = 0; + gia_margin = 0; else for i = 1:tumbling_axes_number % Margin for i-th plane - acc_margin_ab(i) = norm(polyhedron.plane_point(:,i)) - gia'*polyhedron.plane_vector(:,i)/norm(polyhedron.plane_vector(:,i)); + gia_margin_ab(i) = norm(polyhedron.plane_point(:,i)) - gia'*polyhedron.plane_vector(:,i)/norm(polyhedron.plane_vector(:,i)); end - acc_margin = min(acc_margin_ab); + gia_margin = min(gia_margin_ab); end end diff --git a/lib/equ_gia/equ_gia_inclination_margin.m b/lib/equ_gia/equ_gia_inclination_margin.m index b6c8e89..fe490c6 100644 --- a/lib/equ_gia/equ_gia_inclination_margin.m +++ b/lib/equ_gia/equ_gia_inclination_margin.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-02-06 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-06-16 +%%%%%% Last update: 2021-02-12 +%%%%%% Kentaro Uno % % Considering the tumble stability with the addition of gripping forces to prevent the tumbling motion, the following % equation describe the limit condition for one tumbling axis @@ -14,9 +15,9 @@ % % The angular margin for the total acceleration is the following, considering all tumbling axes % -% (pg-pa)x(pg-pb).a_gi ||(a_gi_lim|| -% inc_marg = min(acos(-------------------------------) - acos(-------------)) -% ||(pg-pa)x(pg-pb)|| ||a_gi|| ||a_gi|| +% (pg-pa)x(pg-pb).a_gi ||(a_gi_lim|| +% gia_inc_marg = min(acos(-------------------------------) - acos(-------------)) +% ||(pg-pa)x(pg-pb)|| ||a_gi|| ||a_gi|| % % % a_gi: Gravito-inertial acceleration @@ -27,36 +28,36 @@ % Function variables: % % OUTPUT -% inclination_margin : Inclination margin for total acceleration considering the support polyhedron [deg] (scalar) -% inclination_margin_ab : Inclination margin for each tumbling axis [rad] (1xn vector) +% gia_inclination_margin : Inclination margin for total acceleration considering the support polyhedron [deg] (scalar) +% gia_inclination_margin_ab : Inclination margin for each tumbling axis [rad] (1xn vector) % INPUT % polyhedron : Variables to define the shape of the polyhedron (struct) % gia : Gravito-Inertial Acceleration [m/s^2] (3x1 vector) % equ_flag : Flag of equilibrium condition (1: equilibrium, 0: not in equilibrium) (scalar) -function [inclination_margin, inclination_margin_ab] = equ_gia_inclination_margin(polyhedron, gia, equ_flag) +function [gia_inclination_margin, gia_inclination_margin_ab] = equ_gia_inclination_margin(polyhedron, gia, equ_flag) % Number of tumbling axes tumbling_axes_number = size(polyhedron.plane_vector,2); % Initialize variable -inclination_margin_ab = zeros(1,tumbling_axes_number); +gia_inclination_margin_ab = zeros(1,tumbling_axes_number); if equ_flag == 0 % If not in equilibrium, margin is zero - inclination_margin = 0; + gia_inclination_margin = 0; else for i = 1:tumbling_axes_number if norm(gia) < norm(polyhedron.plane_point(:,i)) % If acceleration is smaller than limit, inclination does not affect margin - inclination_margin_ab(i) = pi; + gia_inclination_margin_ab(i) = pi; else % Margin for i-th plane - inclination_margin_ab(i) = acos(polyhedron.plane_vector(:,i)'*gia/(norm(polyhedron.plane_vector(:,i))*norm(gia))) ... + gia_inclination_margin_ab(i) = acos(polyhedron.plane_vector(:,i)'*gia/(norm(polyhedron.plane_vector(:,i))*norm(gia))) ... - acos(norm(polyhedron.plane_point(:,i))/norm(gia)); end end - inclination_margin = min(inclination_margin_ab)*180/pi; + gia_inclination_margin = min(gia_inclination_margin_ab)*180/pi; end end diff --git a/lib/equ_gia/equ_gia_polyhedron_calc.m b/lib/equ_gia/equ_gia_polyhedron_calc.m index 7c5e60c..3e0cd70 100644 --- a/lib/equ_gia/equ_gia_polyhedron_calc.m +++ b/lib/equ_gia/equ_gia_polyhedron_calc.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-01-20 %%%%%% by Warley Ribeiro -%%%%%% Last update: 2020-06-15 +%%%%%% Last update: 2020-08-20 % % Considering the tumble stability with the addition of gripping forces to prevent the tumbling motion, the following % equation describe the limit condition for one tumbling axis @@ -38,8 +38,9 @@ % polyhedron.edge_vec : Direction vector for the edges of the polyhedron [m] (3xn matrix) % polyhedron.edge_point : Position of a point in the line of the edge where the z position is null (3xn matrix) % polyhedron.vertex : Position of the corners of the polyhedron (3x(n+1) matrix) -% gia : Gravito-Inertial Acceleration vector [m/s^2] (3x1 vector) -% equ_flag : Flag of equilibrium condition (1: equilibrium, 0: not in equilibrium) (scalar) +% gia : Gravito-Inertial Acceleration vector [m/s^2] (3x1 vector) +% equ_flag : Flag of equilibrium condition (1: equilibrium, 0: not in equilibrium) (scalar) +% tumbling_axes_number: Number of possible tumbling axes (scalar) % INPUT % POS_e : End-effector positions POS_e = [p1 p2 ... pn] [m] (3xn matrix) % pg : Center of Gravity position [m] (3x1 vector) @@ -53,7 +54,7 @@ % floor_base : Vertical (z-axis) coordinate of floor for polyhedron base [m] (scalar) % expansion_factor : Expansion factor for the acceleration vector compared to the position vectors (scalar) -function [polyhedron, gia, equ_flag] = equ_gia_polyhedron_calc(POS_e, pg, a_g, mass, grasp_flag, F_hold, F0, M0, plot_on, ... +function [polyhedron, gia, equ_flag, tumbling_axes_number] = equ_gia_polyhedron_calc(POS_e, pg, a_g, mass, grasp_flag, F_hold, F0, M0, plot_on, ... floor_base, expansion_factor) global Gravity @@ -72,6 +73,9 @@ % Check equilibrium based on the current GIA and maximum acceleration limit equ_flag = 1; +if tumbling_axes_number == 0 + equ_flag = 0; +end for i = 1:tumbling_axes_number if gia'*n_ab_u(:,i) > norm(gia_limit_nab(:,i)) % Equilibrium flag @@ -83,17 +87,19 @@ polyhedron.plane_point = gia_limit_nab; polyhedron.plane_vector = n_ab; -if plot_on +if plot_on && tumbling_axes_number > 1 % Shrink vector and move to center of gravity polyhedron.plane_point_exp = expansion_factor*polyhedron.plane_point + pg; - % Calculate intersection lines + % Calculate intersection lines (Polyhedron Edges) for i = 1:tumbling_axes_number if i == tumbling_axes_number j = 1; else j = i+1; end + % Intersection line direction polyhedron.edge_vec(:,i) = cross(n_ab_u(:,i),n_ab_u(:,j)); + % Intersection line point (z = 0) if abs(polyhedron.edge_vec(3,i)) > 0.0001 polyhedron.edge_point(3,i) = 0; A = [n_ab_u(1,i) n_ab_u(2,i); @@ -101,13 +107,44 @@ B = [n_ab_u(:,i)'*polyhedron.plane_point_exp(:,i); n_ab_u(:,j)'*polyhedron.plane_point_exp(:,j)]; polyhedron.edge_point(1:2,i) = A\B; + else + % Intersection line point (y = 0) + if abs(polyhedron.edge_vec(2,i)) > 0.0001 + polyhedron.edge_point(2,i) = 0; + A = [n_ab_u(1,i) n_ab_u(3,i); + n_ab_u(1,j) n_ab_u(3,j)]; + B = [n_ab_u(:,i)'*polyhedron.plane_point_exp(:,i); + n_ab_u(:,j)'*polyhedron.plane_point_exp(:,j)]; + polyhedron.edge_point([1,3],i) = A\B; + % Intersection line point (x = 0) + else + polyhedron.edge_point(1,i) = 0; + A = [n_ab_u(2,i) n_ab_u(3,i); + n_ab_u(2,j) n_ab_u(3,j)]; + B = [n_ab_u(:,i)'*polyhedron.plane_point_exp(:,i); + n_ab_u(:,j)'*polyhedron.plane_point_exp(:,j)]; + polyhedron.edge_point(2:3,i) = A\B; + end end end % Calculate intersection points + % Compute support triangle surface + v1 = POS_e(:,tumbling_axes(1,1)) - POS_e(:,tumbling_axes(1,2)); + v2 = POS_e(:,tumbling_axes(2,1)) - POS_e(:,tumbling_axes(2,2)); + sup_plane_vector = cross(v1,v2); + sup_plane_point = POS_e(:,tumbling_axes(1,1)); for i = 1:tumbling_axes_number - % Lines and floor base - ind = (floor_base - polyhedron.edge_point(3,i))/polyhedron.edge_vec(3,i); + + % Intersection between lines and support triangle surface + if abs(polyhedron.edge_vec(:,i)'*sup_plane_vector) > 0.0001 + ind = (sup_plane_point - polyhedron.edge_point(:,i))'*sup_plane_vector / (polyhedron.edge_vec(:,i)'*sup_plane_vector); + else + disp('Polyhedron edge is parallel to surface. Unable to compute correct visualization'); + ind = 0; + end + +% ind = (floor_base - polyhedron.edge_point(3,i))/polyhedron.edge_vec(3,i); polyhedron.vertex(:,i) = polyhedron.edge_point(:,i) + ind*polyhedron.edge_vec(:,i); end % Between lines diff --git a/lib/equ_gia/equ_tumbling_axes_ab.m b/lib/equ_gia/equ_tumbling_axes_ab.m index fa2d8fb..c90aca8 100644 --- a/lib/equ_gia/equ_tumbling_axes_ab.m +++ b/lib/equ_gia/equ_tumbling_axes_ab.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-02-03 %%%%%% by Warley Ribeiro -%%%%%% Last update: 2020-06-10 +%%%%%% Last update: 2021-08-20 % % % Obtain all possible tumbling axes numbering, accordingly to the number of the supporting legs. This function requires that @@ -25,21 +25,34 @@ function [tumbling_axes, tumbling_axes_number] = equ_tumbling_axes_ab(n, grasp_flag) -cnt = 1; -for a = 1:n - if grasp_flag(a) == 1 - tumbling_axes(cnt,1) = a; - for b = a+1:n - if grasp_flag(b) == 1 - tumbling_axes(cnt,2) = b; - cnt = cnt + 1; - break +if sum(grasp_flag) > 1 + + cnt = 1; + for a = 1:n + if grasp_flag(a) == 1 + tumbling_axes(cnt,1) = a; + for b = a+1:n + if grasp_flag(b) == 1 + tumbling_axes(cnt,2) = b; + cnt = cnt + 1; + break + end end end end -end -% Close polygon with initial support point -tumbling_axes(cnt,2) = tumbling_axes(1,1); -% Total number of tumbling axes -tumbling_axes_number = cnt; + % Close polygon with initial support point + tumbling_axes(cnt,2) = tumbling_axes(1,1); + % Total number of tumbling axes + tumbling_axes_number = cnt; +else + % One or none supporting legs case + for a = 1:n + if grasp_flag(a) == 1 + tumbling_axes = [a a]; + else + tumbling_axes = [0 0]; + end + end + tumbling_axes_number = 0; +end \ No newline at end of file diff --git a/lib/equ_gia/gia_limit.m b/lib/equ_gia/gia_limit.m index 313f063..dbf54f5 100644 --- a/lib/equ_gia/gia_limit.m +++ b/lib/equ_gia/gia_limit.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-02-04 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-06-15 +%%%%%% Last update: 2020-08-20 % % Calculate maximum acceleration for the normal direction n_ab % @@ -39,6 +39,10 @@ % Initialize variables gia_limit_nab = zeros(3,tumbling_axes_number); +if tumbling_axes_number == 0 + gia_limit_nab(1:3) = zeros(3,1); +else + for i = 1:tumbling_axes_number a = tumbling_axes(i,1); b = tumbling_axes(i,2); % Tumbling axis initial and final points @@ -57,4 +61,8 @@ % Maximum GIA gia_limit_nab(:,i) = Mab/(mass*norm(n_ab(:,i)))*n_ab_u(:,i); +end + +end + end \ No newline at end of file diff --git a/lib/target_detection/config_target_detection_param.m b/lib/target_detection/config_target_detection_param.m index ea553b5..8078432 100644 --- a/lib/target_detection/config_target_detection_param.m +++ b/lib/target_detection/config_target_detection_param.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-08-17 %%%%%% Keigo Haji -%%%%%% Last update: 2020-10-01 +%%%%%% Last update: 2021-04-16 %%%%%% Keigo Haji % % @@ -87,7 +87,8 @@ matching_settings.mesh_threshold = 0.015; % mesh threshold [m] matching_settings.mesh_size = 0.015; % length of one side of mesh [m] - +matching_settings.delete_lower_targets = 'off'; +matching_settings.delete_lower_targets_threshold = 0.01; %[m] diff --git a/lib/target_detection/config_target_detection_testing_param.m b/lib/target_detection/config_target_detection_testing_param.m new file mode 100644 index 0000000..86d52d3 --- /dev/null +++ b/lib/target_detection/config_target_detection_testing_param.m @@ -0,0 +1,133 @@ +%%%%%% Configuration +%%%%%% config_target_detection_testing_param +%%%%%% +%%%%%% Define target detection parameters for test +%%%%%% +%%%%%% Created 2021-04-15 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-04-21 +%%%%%% Keigo Haji +% +% +% Load configurations for parameters defined for target detection algorithm +% +% Function variables: +% +% OUTPUT +% gripper_param : parameters for the gripper-mask +% map_param : parameters and settings for creating the terrain +% matrix +% matching_settings : settings for matching +% gripper_or_peaks : the way of detecting targets, 'gripper' or 'peaks' +% environment_param : Environment settings +% ani_settings ; Animation settings +% INPUT +% - +% + +function [gripper_param, map_param, matching_settings, gripper_or_peaks, environment_param, ani_settings] = config_target_detection_testing_param() +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% General Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% PCD file Name, 'allMediumHolds14', 'testfield_Apr8','rtabmap_Apr22' +map_param.pcd_file_name = 'rtabmap_Apr22.pcd'; +environment_param.surface_type = append('climbing_holds_',map_param.pcd_file_name); +gripper_or_peaks = 'gripper'; + +% Although the following lines are map_param, these parameters are not used in both main_sim and target_detection.m. +map_param.delete_lower_points_in_transformed_PCD = 'on'; +map_param.delete_lower_points_threshold = -0.01; %[m] + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Environment Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +environment_param.inc = 0; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Animation Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Surface related %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Surface viz. on/off +ani_settings.surface_show = 'on'; + %%% Surface grid color + ani_settings.grid_color = [0.9 0.9 0.9]; + %%% Surface color + ani_settings.surface_color = gray; + +%%% Graspable points viz. on/off +ani_settings.graspable_points_show = 'on'; + %%% Graspable points color + ani_settings.graspable_points_color = [0.0, 0.8, 0.0]; + %%% Graspable points alpha + ani_settings.graspable_points_alpha = 1.0; + %%% Graspable points marker + ani_settings.graspable_points_marker = 'o'; + %%% Graspable points size + ani_settings.graspable_points_size = 40; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gripper Parameters %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%% if detection_type is "gripper" %%%% +gripper_param.palm_diameter = 32; %[mm] +gripper_param.palm_diameter_of_finger_joints = 28; %[mm] +gripper_param.finger_length = 15; %[mm] between joints +gripper_param.spine_length = 15; %[mm] +gripper_param.spine_depth = 5; %[mm] +gripper_param.opening_angle = 75; %[deg] +gripper_param.closing_angle = 30; %[deg] + +% Measured parameters by CAD +gripper_param.opening_spine_radius = 37; % the distance from the center of palm to the farthest point of the spines +% when gripping 75 degrees shape [mm] (37.4mm) +gripper_param.opening_spine_depth = 5; % 5.26 [mm] +gripper_param.closing_height = 16; %[mm] Vertical distance between the tip of the spine and the bottom of the palm when closed + +% margins +gripper_param.margin_of_top_solid_diameter = 4; %[mm] +gripper_param.inside_margin_of_bottom_void_diameter = 2; %[mm] + +%%%% if detection_type is "peaks" %%%% +% Gripper-Mask Parameters just for peaks +% Set the size of a rectangle. Determine the search range to detect +% peak. +gripper_param.half_size_of_gripper_mask_for_peaks = 20; %[mm] +gripper_param.height_of_gripper_mask_for_peaks= 2; %[mm] + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Map Parameters and settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% If the area of the map is larger than a few meters, it may exceed the size limit of the 3D array that MATLAB can handle. +% To avoid this, we can exclude distant points. +map_param.delete_outliers = "off"; % "on" or "off" +map_param.outliers_threshold = 2; % [m] + +map_param.voxel_size = 0.0010; % length of one side of voxel [m] +map_param.voxel_threshold = 0.0010; % gap from center point of voxel [m] + % if the centroid of points in the voxel is farther than this value, the voxel is set 0 +map_param.transform = "off"; % "on" or "off" + % If you use fractal map or the map which is already transformed into the least-square plane, + % you should set "off" +map_param.interpolation = "off"; % raw data or interpolated map, "on" or "off" +map_param.interpolation_method = "linear"; % method of interpolation, "linear" or "natural" + + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Matching Settings %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% For a fractal terrain with point clouds at 1mm intervals, set +% matching_settings.threshold = 1000; +% and set matching_settings.threshold to about 120 +% to use the point clouds obtained by the RealSense camera. +% When you use climbing_holds_map_1m_x_1m_gridformat.mat, set +% matching_settings.threshold = 130; +% If it is detected in a depression, which is obviously not a convexity, +% this should be resolved by increasing the value. +matching_settings.threshold = 120; % number of solid voxels to guarantee the grasp [] voxels + +matching_settings.submatching = "on"; % only regular targets or including sub-targets, "on" or "off" +matching_settings.penalty_coefficient = 100; % penalty factor for impaired solid voxels +matching_settings.colony = "on"; % whether to thin out the graspable points from dense areas, "on" or "off" +matching_settings.colony_threshold = 4; % the number of vocels you think are close [] voxels + % This value is only used when map_param.interpolation == "OFF" && matching_settings.colony == "ON"; +matching_settings.mesh_threshold = 0.015; % mesh threshold [m] +matching_settings.mesh_size = 0.015; % length of one side of mesh [m] + +matching_settings.delete_lower_targets = 'on'; +matching_settings.delete_lower_targets_threshold = 0.01; %[m] + + + +end \ No newline at end of file diff --git a/lib/target_detection/main_target_detection.m b/lib/target_detection/main_target_detection.m new file mode 100644 index 0000000..0524afd --- /dev/null +++ b/lib/target_detection/main_target_detection.m @@ -0,0 +1,138 @@ +%%%%%% MAIN FILE ofTarget_Detection_for_Climbing_Robots +%%%%%% main_target_detection.m +%%%%%% +%%%%%% MAIN FILE +%%%%%% +%%%%%% Extract Target Points from a Given Point Cloud +%%%%%% +%%%%%% Created 2021-04-10 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-04-22 +%%%%%% Keigo Haji +%%%%%% +%%%%%% This files are used for target detection outside of the cilmblab. +%%%%%% Before executing in climblab, we can run the target detection as +%%%%%% same situation in cilmblab and check the result. +% +% [Note] +% This MAIN file is not executed in main_sim.m. +% This is used to convert a new PCD file acquired by a RealSense +% camera from a 3*N matrix to the grid format used by ClimbLab. +% After execution, check the results to see if the terrain data is not +% upside down, and the algorithm detect proper targets on the terrain. +% If there is no problem, pack the three variables of xyz together and save +% them as a .mat file. +% [Note 2] +% When you added new bouldering holds map taken by RealSense camera in +% ClimbLab, you must include 'climbing_holds' in the .mat file name. This +% is because we switch how to plot the surface in vis_surface depending on +% the name of the map. +% +clc; clear; close all; +tic; +global x y z; + +%% Load config file of config_target_detection_testing_param +[gripper_param, map_param, matching_settings, gripper_or_peaks, environment_param, ani_settings] = config_target_detection_testing_param(); + +%% Load designated PCD file and change the format from PCD to xyz + +% Load a point cloud from a PCD format file +rawPointCloudData = loadpcd(map_param.pcd_file_name); +rawPointCloudData = rawPointCloudData(1:3,:); + + +% Change the format from the 3*N matrx to the xy vectors and z matrix format. + % [x,y,z] = map_format_conversion(rawPointCloudData,map_param.interpolation_method); +[pcd,~,~] = pcd_transform(rawPointCloudData); +%%% Delete lower points probably located on the wall +if map_param.delete_lower_points_in_transformed_PCD == "on" + pcd = target_delete(pcd, map_param.delete_lower_points_threshold); +end +[~,x,y,z,~] = pcd_interpolate(pcd,map_param.interpolation_method); + + +%% Reshape the grid data to the 3*N matrix like ini_graspable_points + +% Reshape the data to the 3*N matrix like ini_graspable points. +[n,m]=size(z); +surface_param.graspable_points=zeros(3,n*m); +[X,Y] = meshgrid(x,y); +surface_param.graspable_points(1,:)=reshape(X,1,n*m); +surface_param.graspable_points(2,:)=reshape(Y,1,n*m); +surface_param.graspable_points(3,:)=reshape(z,1,n*m); + +% Set the min and max to adjust color of vis_surface like ini_surface +surface_param.min = min(surface_param.graspable_points(3,:)); +surface_param.max = max(surface_param.graspable_points(3,:)); + +% Extract only x,y and z positions of points +graspable_points = surface_param.graspable_points(1:3,:); + + +%% Execute target detection +%% The following lines are completely same as target_detection.m which is executed in ini_graspable_points + + +% Delete the points which located farther than outliers_threshold if the +% setting is "ON" +if map_param.delete_outliers == "on" + [graspable_points] = pcd_delete_outliers(graspable_points,map_param.outliers_threshold); +end + +% Offset the points +[graspable_points,offset_vector] = pcd_offset(graspable_points); + + + + +% Make a terrain matrix from the points +[terrain_matrix] = pcd_voxelize(graspable_points, map_param.voxel_size, map_param.voxel_threshold); + +% Make a gripper-mask +[gripper_mask] = grippermask(gripper_param, map_param.voxel_size, gripper_or_peaks); + +% Check the "grippability" and select the graspable points +[voxel_coordinates_of_graspable_points] = voxel_matching(terrain_matrix,gripper_mask,matching_settings, gripper_or_peaks); + +% Select the representative points from the matching graspable points if +% the setting is "on" +if matching_settings.colony == "on" + switch map_param.interpolation + case "on" + [voxel_coordinates_of_graspable_points] = colony_search(voxel_coordinates_of_graspable_points, matching_settings, map_param.voxel_size ,grid_size); + case "off" + [voxel_coordinates_of_graspable_points] = colony_search(voxel_coordinates_of_graspable_points, matching_settings, map_param.voxel_size); + end +end + +% Re-transform the frame +if map_param.transform == "on" + [graspable_points] = pcd_re_transform(voxel_coordinates_of_graspable_points,map_param.voxel_size,offset_vector,rotation_matrix,centroid_vector_of_plane); +elseif map_param.transform == "off" + [graspable_points] = pcd_re_transform(voxel_coordinates_of_graspable_points,map_param.voxel_size,offset_vector); +end + + +% Delete the targets whose z positions are less than threshold +if matching_settings.delete_lower_targets == "on" + [graspable_points] = target_delete(graspable_points,matching_settings.delete_lower_targets_threshold); +else + ; +end + +%% Inset the graspable_points to the surface_param like ini_graspable_points +% Insert the graspable_points to display them in vis_graspable_points +surface_param.graspable_points = graspable_points; + + +%% Visualize the result like main_sim visualization + +inc = environment_param.inc; +figure(1); clf; hold on; axis equal; +vis_surface(inc, ani_settings, environment_param, surface_param); +vis_graspable_points(surface_param,inc,ani_settings); + + +toc +% EOF \ No newline at end of file diff --git a/lib/target_detection/target_delete.m b/lib/target_detection/target_delete.m new file mode 100644 index 0000000..8cf11f0 --- /dev/null +++ b/lib/target_detection/target_delete.m @@ -0,0 +1,41 @@ +%%%%%% Target Delete +%%%%%% target_delete.m +%%%%%% +%%%%%% Select the targets located above the threshold in z-coordinate +%%%%%% +%%%%%% Created 2021-04-16 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-04-16 +%%%%%% Keigo Haji +% +% Delete the targets whose z positions are less than threshold +% +% +% Function variables: +% +% OUTPUT +% graspable_points : 3*N matrix contains position vectors of graspable points [m] (x;y;z) +% +% INPUT +% graspable_points : 3*M(>N) matrix contains position vectors of graspable points [m] (x;y;z) +% matching_settings : settings for matching +% +% +function [graspable_points] = target_delete(graspable_points,low_threshold) + +% Check the number of graspable points +[~,m] = size(graspable_points); + +% Replace the positions of targets located under the threshold to NaN values +for i=1:m + if graspable_points(3,i) < low_threshold + graspable_points(:,i) = [NaN; NaN; NaN]; + else + end +end + +% Delete NaN columns +graspable_points = rmmissing(graspable_points, 2); + +end + diff --git a/lib/target_detection/target_detection.m b/lib/target_detection/target_detection.m index c2e8304..d2fefda 100644 --- a/lib/target_detection/target_detection.m +++ b/lib/target_detection/target_detection.m @@ -7,7 +7,7 @@ %%%%%% %%%%%% Created 2020-09-02 %%%%%% Keigo Haji -%%%%%% Last update: 2020-10-16 +%%%%%% Last update: 2021-04-16 %%%%%% Keigo Haji % % Select points which can be grasped by the gripper from graspable points @@ -98,7 +98,15 @@ elseif map_param.transform == "off" [graspable_points] = pcd_re_transform(voxel_coordinates_of_graspable_points,map_param.voxel_size,offset_vector); end - + + +% Delete the targets whose z positions are less than threshold +if matching_settings.delete_lower_targets == "on" + [graspable_points] = target_delete(graspable_points,matching_settings.delete_lower_targets_threshold); +else + ; +end + end diff --git a/src/climb_main.m b/src/climb_main.m deleted file mode 100644 index cba02c5..0000000 --- a/src/climb_main.m +++ /dev/null @@ -1,106 +0,0 @@ -%%%%%% MAIN -%%%%%% climb_main -%%%%%% -%%%%%% Main file for climbing/walking simulation -%%%%%% -%%%%%% Created 2020-02-20 by Warley Ribeiro -%%%%%% Last update: 2020-10-16 by Kentaro Uno - -clc; clear; close all; -tic; ini_path(); variables_saved = []; - -%%% Select configuration 'default', 'USER', 'uno_crawl_param', 'nonperiodic_demo_param', 'gia_static', 'iSAIRAS_2020_demo_param' -config = 'iSAIRAS_2020_demo_param'; -%%% Load all initial parameters from configuration files stored in config/ -[robot_param, environment_param, gait_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... - plot_settings, gripper_param, map_param, matching_settings] = config_simulation(config); - -%%% Define a code for current set of simulations -run_cod = 'test'; - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize environment %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -global d_time; global Gravity; global Ez; global contact_f; global x; global y; global z; - -ini_environment(environment_param); -surface_param = ini_surface(environment_param); -surface_param = ini_graspable_points(environment_param, surface_param, gripper_param, map_param, matching_settings); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -[LP, SV, des_SV, F_grip, POS_e, ORI_e, cont_POS] = ini_robot(robot_param, gait_param, surface_param); -shape_robot = vis_create_robot_model(ani_settings, LP, F_grip); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize gait %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -[gait_param, path_planning_param, motion_planning_param] = ini_gait(gait_param); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize files %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -[run_id, run_date] = ini_id(robot_param, environment_param, gait_param, control_param); -video = ini_video_file(ani_settings, run_cod, run_id, run_date); - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Simulation loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -for time = 0:d_time:environment_param.time_max - % Display time - disp(time) - -% if norm(SV.R0(1:2) - gait_param.goal(1:2)) < 0.02 -% break -% end - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Path Planning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - path_planning_param = upd_path_planning(path_planning_param, gait_param, surface_param, des_SV, SV, LP, POS_e, ... - environment_param, robot_param, time); - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Motion Planning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - [motion_planning_param, des_SV] = upd_motion_planning(motion_planning_param, gait_param, ... - path_planning_param, LP, SV, des_SV, cont_POS, time); - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Evaluation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - equilibrium_eval_param = upd_equilibrium_eval(equilibrium_eval_param, surface_param, LP, SV, POS_e, F_grip); - - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot controller %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - SV = upd_control(SV, des_SV, control_param); - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Kinematics and dynamics %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - cont_POS = upd_collision_endeffector(LP, SV, POS_e, cont_POS); - SV = upd_ground_reaction_forces_spring_damper(LP, SV, surface_param, POS_e, cont_POS); - SV = upd_fwd_dynamics(environment_param, LP, SV, des_SV); - [POS_e, Qe_deg, Q0_deg, SV] = upd_fwd_kinematics(LP, SV); - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save selected variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - variables_saved = upd_variables_saved(variables_saved, save_settings, time, LP, SV, POS_e, equilibrium_eval_param); - - SV = upd_grasp_detach(LP, SV, des_SV, F_grip, environment_param, equilibrium_eval_param, variables_saved); -% SV.sup = des_SV.sup; - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Climbing animation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - % Visualization - if rem(time,1/ani_settings.frame_rate) == 0 && strcmp(ani_settings.display,'on') - inc = environment_param.inc; - figure(1); clf; hold on; - vis_robot(LP, SV, F_grip, POS_e, shape_robot, inc, ani_settings); - ani_settings = vis_animation_range(motion_planning_param,surface_param,ani_settings,inc); - vis_surface(inc, ani_settings); - vis_graspable_points(surface_param,inc,ani_settings); - vis_graspable_points_in_reachable_area(path_planning_param, inc, ani_settings); - vis_reachable_area(SV, LP, path_planning_param, inc, ani_settings, surface_param); - vis_goal(gait_param, inc, ani_settings); - vis_support_triangle(SV,POS_e,inc,ani_settings); - vis_com_projection(LP, SV, inc, ani_settings); - vis_next_desired_position(path_planning_param, inc, ani_settings); - vis_stability_polyhedron(inc, equilibrium_eval_param, ani_settings); - vis_vectors(inc,ani_settings,equilibrium_eval_param,LP,SV); - vis_animation_settings(ani_settings,surface_param,time); - vis_trajectory_4legged(environment_param,ani_settings,variables_saved); - writeVideo(video,getframe(1)); - end - -end -%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Saving variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -close(video); -data = sav_data_file(variables_saved, save_settings, run_cod, run_id, run_date); -% Plot graphs -vis_plot_graph(data, plot_settings, LP, inc, surface_param, path_planning_param); - -toc -% EOF \ No newline at end of file diff --git a/src/controller/upd_control.m b/src/controller/control/upd_control.m similarity index 100% rename from src/controller/upd_control.m rename to src/controller/control/upd_control.m diff --git a/src/controller/equilibrium/upd_equilibrium_eval.m b/src/controller/equilibrium/upd_equilibrium_eval.m index 8c04e41..fd2e610 100644 --- a/src/controller/equilibrium/upd_equilibrium_eval.m +++ b/src/controller/equilibrium/upd_equilibrium_eval.m @@ -4,8 +4,9 @@ %%%%%% Update equilibrium evaluation %%%%%% %%%%%% Created 2020-04-23 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-02 +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2021-08-20 +%%%%%% by Warley Ribeiro % % % Update equilibrium parameter evaluation @@ -17,13 +18,14 @@ % INPUT % equilibrium_eval_param : Parameters for equilibrium evaluation (class) % surface_param : Parameters for surface (class) +% ani_settings : Parameters for animation (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) +% LP.F_grip : Maximum gripping force [N] (scalar) -function equilibrium_eval_param = upd_equilibrium_eval(equilibrium_eval_param, surface_param, LP, SV, POS_e, F_grip) +function equilibrium_eval_param = upd_equilibrium_eval(equilibrium_eval_param, surface_param, ani_settings, LP, SV, POS_e) switch equilibrium_eval_param.type @@ -45,7 +47,7 @@ %%% Tumble stability % Tumbling moment Mab - equilibrium_eval_param = upd_tsm_tumbling_moment(equilibrium_eval_param, LP, SV, POS_e, F_grip); + equilibrium_eval_param = upd_tsm_tumbling_moment(equilibrium_eval_param, LP, SV, POS_e); % Tumbling criterion equilibrium_eval_param = upd_tsm_equilibrium_judgment(equilibrium_eval_param, LP, SV, POS_e); % TSM @@ -72,31 +74,89 @@ floor_base = surface_param.min; - if strcmp(equilibrium_eval_param.plot_polyhedron,'on') + if strcmp(ani_settings.gia_stable_region_show,'on') plot_on = 1; else plot_on = 0; end % To show stability polyhedron in cartesian space shrinked by a factor of "expansion_factor" - expansion_factor = equilibrium_eval_param.expansion_factor; + expansion_factor = ani_settings.acceleration_expansion_factor; + F_grip = LP.F_grip; - [polyhedron, gia, equ_flag] = equ_gia_polyhedron_calc(POS_e, CoM, a_g, mass, grasp_flag, F_grip, F0, M0, plot_on, ... - floor_base, expansion_factor); + [polyhedron, gia, equ_flag, tumbling_axes_number] = equ_gia_polyhedron_calc(POS_e, CoM, a_g, mass, grasp_flag, F_grip, ... + F0, M0, plot_on, floor_base, expansion_factor); % Acceleration Margin - [acc_margin, acc_margin_ab] = equ_gia_acceleration_margin(polyhedron, gia, equ_flag); + [gia_margin, gia_margin_ab] = equ_gia_acceleration_margin(polyhedron, gia, equ_flag); % Inclination Margin - [inclination_margin, inclination_margin_ab] = equ_gia_inclination_margin(polyhedron, gia, equ_flag); + [gia_inclination_margin, gia_inclination_margin_ab] = equ_gia_inclination_margin(polyhedron, gia, equ_flag); equilibrium_eval_param.polyhedron = polyhedron; equilibrium_eval_param.gia = gia; equilibrium_eval_param.equilibrium_flag = equ_flag; - equilibrium_eval_param.acc_margin = acc_margin; - equilibrium_eval_param.acc_margin_ab = acc_margin_ab; - equilibrium_eval_param.inclination_margin = inclination_margin; - equilibrium_eval_param.inclination_margin_ab = inclination_margin_ab; + equilibrium_eval_param.gia_margin = gia_margin; + equilibrium_eval_param.gia_margin_ab = gia_margin_ab; + equilibrium_eval_param.gia_inclination_margin = gia_inclination_margin; + equilibrium_eval_param.gia_inclination_margin_ab = gia_inclination_margin_ab; + equilibrium_eval_param.tumbling_axes_number = tumbling_axes_number; + +case 'tsm_and_gia' + %%% Force/Moment Calculations + %%% Calculate force and moment acting on the robot gravitational terms + % Calculate Center of Mass (CoM) + CoM = upd_CoM(LP, SV); + % Calculate gravitational terms + equilibrium_eval_param = upd_gravitational_force_moment(equilibrium_eval_param, LP, CoM); + % Calculate acceleration terms + equilibrium_eval_param = upd_inertial_force_moment_plus_rotational(equilibrium_eval_param, LP, SV); + %%% TSM calculatin + %%% Obtain Tumbling axes + equilibrium_eval_param = upd_tumbling_axes(equilibrium_eval_param, LP, SV); + %%% Tumble stability + % Tumbling moment Mab + equilibrium_eval_param = upd_tsm_tumbling_moment(equilibrium_eval_param, LP, SV, POS_e); + % Tumbling criterion + equilibrium_eval_param = upd_tsm_equilibrium_judgment(equilibrium_eval_param, LP, SV, POS_e); + % TSM + equilibrium_eval_param = upd_tsm(equilibrium_eval_param, LP); + %%% Equilibrium flag + equilibrium_eval_param.equilibrium_flag = equilibrium_eval_param.tsm_equilibrium_flag; + + %%% GIA calculatin + % Center of Gravity acceleration + a_g = equilibrium_eval_param.Fa/LP.mass; + % Robot mass + mass = LP.mass; + % grasp flag + grasp_flag = SV.sup; + % External Force and Moment + F0 = [0 0 0]'; + M0 = [0 0 0]'; + floor_base = surface_param.min; + if strcmp(ani_settings.gia_stable_region_show,'on') + plot_on = 1; + else + plot_on = 0; + end + % To show stability polyhedron in cartesian space shrinked by a factor of "expansion_factor" + expansion_factor = ani_settings.acceleration_expansion_factor; + F_grip = LP.F_grip; + [polyhedron, gia, equ_flag] = equ_gia_polyhedron_calc(POS_e, CoM, a_g, mass, grasp_flag, F_grip, F0, M0, plot_on, ... + floor_base, expansion_factor); + % Acceleration Margin + [gia_margin, gia_margin_ab] = equ_gia_acceleration_margin(polyhedron, gia, equ_flag); + % Inclination Margin + [gia_inclination_margin, gia_inclination_margin_ab] = equ_gia_inclination_margin(polyhedron, gia, equ_flag); + equilibrium_eval_param.polyhedron = polyhedron; + equilibrium_eval_param.gia = gia; + equilibrium_eval_param.equilibrium_flag = equ_flag; + equilibrium_eval_param.gia_margin = gia_margin; + equilibrium_eval_param.gia_margin_ab = gia_margin_ab; + equilibrium_eval_param.gia_inclination_margin = gia_inclination_margin; + equilibrium_eval_param.gia_inclination_margin_ab = gia_inclination_margin_ab; + otherwise disp('Invalid equilibrium evaluation type') diff --git a/src/controller/equilibrium/upd_tsm_tumbling_moment.m b/src/controller/equilibrium/upd_tsm_tumbling_moment.m index 825250b..e60f650 100644 --- a/src/controller/equilibrium/upd_tsm_tumbling_moment.m +++ b/src/controller/equilibrium/upd_tsm_tumbling_moment.m @@ -3,9 +3,10 @@ %%%%%% %%%%%% Calculate tumbling moment for axes %%%%%% -%%%%%% Created 2020-05-06 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-05-06 +%%%%%% Created: 2020-05-06 +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2020-12-21 +%%%%%% by Kentaro Uno % % % Calculate tumbling moment @@ -20,9 +21,9 @@ % 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) +% LP.F_grip : Maximum gripping force [N] (scalar) -function equilibrium_eval_param = upd_tsm_tumbling_moment(equilibrium_eval_param, LP, SV, POS_e, F_grip) +function equilibrium_eval_param = upd_tsm_tumbling_moment(equilibrium_eval_param, LP, SV, POS_e) % Initialize tumbling moment Mab equilibrium_eval_param.Mab = zeros(1,equilibrium_eval_param.tumbling_axes_number); @@ -45,7 +46,7 @@ equilibrium_eval_param.Mab(i) = Mb'*(pa-pb)/abs(norm(pa-pb)) + Fb'*(cross(pb,pa))/abs(norm(pa-pb)); % If there is a gripping force - if F_grip ~= 0 + if LP.F_grip ~= 0 % Check all possible gripping points besides the ones forming the tumbling axis for j = 1:LP.num_limb if j ~= a && j ~= b && SV.sup(j) == 1 @@ -63,7 +64,7 @@ else sgnx = 0; end - Fgripper = sgnx * F_grip * ng; + Fgripper = sgnx * LP.F_grip * ng; % Update tumbling moment with gripping force equilibrium_eval_param.Mab(i) = equilibrium_eval_param.Mab(i) - Fgripper'*(cross((pb-pj),(pa-pj))/abs(norm(pa-pb))); diff --git a/src/controller/gait/0_upd_graspable_points_in_reachable_area/upd_graspable_points_in_reachable_area.m b/src/controller/gait/0_upd_graspable_points_in_reachable_area/upd_graspable_points_in_reachable_area.m new file mode 100644 index 0000000..871fc3f --- /dev/null +++ b/src/controller/gait/0_upd_graspable_points_in_reachable_area/upd_graspable_points_in_reachable_area.m @@ -0,0 +1,143 @@ +%%%%%% Update +%%%%%% upd_graspable_points_in_reachable_area +%%%%%% +%%%%%% Update the graspable points in reachable area +%%%%%% +%%%%%% Created 2020-05-18 +%%%%%% Yusuke Koizumi +%%%%%% Last update: 2021-05-19 +%%%%%% Keigo Haji +% +% +% Update the graspable points in reachable area for gait planning +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.graspable_points_in_reachable_area: graspable +% points in the four limbs" reachable region +% +% INPUT +% SV : State values (SpaceDyn class) +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% surface_param.graspable_points : 3xN matrix of the graspable points in the map [m] +% surface_param.sensed_graspable_points : 3xN matrix of the sensed graspable points in the map [m] +% For the points that have not yet been sensed, the corresponding columns hold the NaN values. +% gait_planning_param : Parameters for path planning (class) +% sensing_camera_param : Parameters for sensing camera (class) + + +function [gait_planning_param] = upd_graspable_points_in_reachable_area(SV,des_SV,LP,surface_param,gait_planning_param, sensing_camera_param) +% If all legs are in support phase +if sum(des_SV.sup) == LP.num_limb + +% judge the graspable point_i is reachable by leg-i +p_from_Joint = zeros(3,1); +% prepare the array to store the graspable points in reachable area [[points], n, Limb number ] +gait_planning_param.graspable_points_in_reachable_area = []; + +% choose whether all grasped points are known or walk while sensing. +switch sensing_camera_param.sensing_flag + case 'off' + % Position of all the graspable points on the map relative to Base + graspable_points(3,:) = surface_param.graspable_points(3,:) - SV.R0(3,1); + + case 'on' + % Position of only sensed graspable points relative to Base + graspable_points(3,:) = surface_param.sensed_graspable_points(3,:) - SV.R0(3,1); +end + +% get an index of the swing leg +for i = 1 : 1 : LP.num_limb + count = 1; + for k = 1 : length(surface_param.graspable_points) + + diff1 = abs( LP.reachable_area.data_far(3,:) - graspable_points(3,k)); + diff2 = abs( LP.reachable_area.data_near(3,:) - graspable_points(3,k)); + % M_n: Min. value, I_n: the index + % *see also the usage of min() for more info.) + [~,I_1] = min(diff1(:)); + [~,I_2] = min(diff2(:)); + + if I_1(1) + x1 = LP.reachable_area.data_far(2,I_1(1)); + y1 = LP.reachable_area.data_far(3,I_1(1)); + end + if I_2(1) + x2 = LP.reachable_area.data_near(2,I_2(1)); + y2 = LP.reachable_area.data_near(3,I_2(1)); + end + + % MaxRange and MinRange are defined by the height of surface_param.graspable_points + % respectively. Note that surface_param.graspable_points's height is defined in the way + % that robot CoM is height = zero. + MaxRange = x1; + MinRange = x2; + MinRange_from_Joint = MinRange - LP.c0(1,1); + MaxRange_from_Joint = MaxRange - LP.c0(1,1); + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % choose whether all grasped points are known or walk while sensing + switch sensing_camera_param.sensing_flag + case 'off' + p_from_Joint = surface_param.graspable_points(:,k) - (SV.R0 + SV.A0*LP.c0(:,3*i-2)); + case 'on' + p_from_Joint = surface_param.sensed_graspable_points(:,k) - (SV.R0 + SV.A0*LP.c0(:,3*i-2)); + end + + d = norm(p_from_Joint(1:2)); + % ------------------The following is previous codes --------------- + % <<< Step 1 >>> + % if d < MaxRange_from_Joint && d > MinRange_from_Joint + % % <<< Step 2 >>> + % theta = - SV.Q0(3); % initialize theta with offsetting the yaw angle at this moment + % theta = theta + atan2(p_from_Joint(2), p_from_Joint(1)); + % switch i %leg-number + % case 1 + % case 2 + % if theta <= 0 + % theta = theta + 2*pi; + % end + % case 3 + % + % if theta <= 0 + % theta = theta + 2*pi; + % end + % case 4 + % theta = theta + 2*pi; + % end + % if theta > LP.Qi(3, (3*i-2)) + LP.joint_limit(1,1)*pi/180 && theta < LP.Qi(3, (3*i-2)) + LP.joint_limit(1,2)*pi/180 + % gait_planning_param.graspable_points_in_reachable_area(:,count,i) = [surface_param.graspable_points(1,k); surface_param.graspable_points(2,k); surface_param.graspable_points(3,k)]; + % count = count + 1; + % % plot3(surface_param.graspable_points(1,k), surface_param.graspable_points(2,k), surface_param.graspable_points(3,k),'.r',... + % % 'MarkerEdgeColor',[0.0 1.0 0.0],'MarkerSize',10) + % end + % end + % ----------------------------------------------------------------- + if d < MaxRange_from_Joint && d > MinRange_from_Joint + % <<< Step 2 >>> + Base2limbCoxa = SV.A0*LP.c0(:,3*i-2); + dot_tmp = dot(p_from_Joint(1:2), Base2limbCoxa(1:2)); + theta = acos(dot_tmp/d/norm(Base2limbCoxa(1:2))); % 0 to pi + cross_tmp = cross([p_from_Joint(1),p_from_Joint(2),0], [Base2limbCoxa(1),Base2limbCoxa(2),0]); + if cross_tmp(3) > 0 + if theta < abs(LP.joint_limit(1,1)*pi/180) + gait_planning_param.graspable_points_in_reachable_area(:,count,i) = [surface_param.graspable_points(1,k); surface_param.graspable_points(2,k); surface_param.graspable_points(3,k)]; + count = count + 1; + end + elseif cross_tmp(3) <= 0 + if theta < abs(LP.joint_limit(1,2)*pi/180) + gait_planning_param.graspable_points_in_reachable_area(:,count,i) = [surface_param.graspable_points(1,k); surface_param.graspable_points(2,k); surface_param.graspable_points(3,k)]; + count = count + 1; + end + end + end + + end + +end +end + diff --git a/src/controller/gait/2_upd_swing_num/upd_swing_num_based_on_num_of_graspable_options.m b/src/controller/gait/2_upd_swing_num/upd_swing_num_based_on_num_of_graspable_options.m new file mode 100644 index 0000000..7fb5d2b --- /dev/null +++ b/src/controller/gait/2_upd_swing_num/upd_swing_num_based_on_num_of_graspable_options.m @@ -0,0 +1,117 @@ +%%%%%% Update +%%%%%% upd_swing_num_and_grasping_point_based_on_graspable_options +%%%%%% +%%%%%% Update number of swing leg based on how many graspable points the +%%%%%% swing limb has in its reachable area in forward +%%%%%% +%%%%%% Created: 2020-07-02 +%%%%%% Kentaro Uno +%%%%%% Last updated: 2021-05-04 +%%%%%% Keigo Haji +% +% +% You can see the details of the planning method is shown in the following paper: +% -------------------------------------------------------------------- +% In: Proceedings of the International Symposium on Artificial Intelligence +% Robotics and Automation in Space +% Integration (iSAIRAS) 2020 by K. Uno et al. +% Proceedings Paper URL: +%%%%%% https://www.hou.usra.edu/meetings/isairas2020fullpapers/pdf/5027.pdf +% -------------------------------------------------------------------- +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for path planning (class) +% +% gait_planning_param.swing_number : Number of the selected leg to be the next swing leg (scalar) +% +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% surface_param : Parameters for surface (class) +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% inc : Surface inclination [deg] (scalar) +% base_height : Height of base relative to map [m] +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_swing_num_based_on_num_of_graspable_options(gait_planning_param, des_SV, SV, LP, POS_e, surface_param) +if sum(des_SV.sup) == LP.num_limb + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% Preparation of matrix that contains GP in RA, limb code, and score + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Graspable points in Reachable area + GP_in_RA = []; +% % Graspable Points in Reachable Area in front +% GP_in_RA_in_front = []; + % integrated matrix that contains [ [GP_in_RA_in_front]; limbCode; score ], where score is currently used dot product + GP_in_RA_in_front_with_limbCode_and_score = []; + dot_tmp = []; + count = 1; + + for i = 1 : 1 : LP.num_limb + % get the number of the graspable points which is located in front from the current position of the limb in its reachable area + + % Current position of swing leg on the surface + [ gait_planning_param.POS_cur(1,i), ... + gait_planning_param.POS_cur(2,i), ... + gait_planning_param.POS_cur(3,i) ] = get_map_pos_of_graspable_points(POS_e(1,i),POS_e(2,i),surface_param); + + % Current position of the base + gait_planning_param.base_cur = SV.R0; + + % Graspable points in Reachable area for limb i + switch i + case 1 + GP_in_RA(:,:,i) = gait_planning_param.LF_graspable_points_in_reachable_area; + case 2 + GP_in_RA(:,:,i) = gait_planning_param.LH_graspable_points_in_reachable_area; + case 3 + GP_in_RA(:,:,i) = gait_planning_param.RH_graspable_points_in_reachable_area; + case 4 + GP_in_RA(:,:,i) = gait_planning_param.RF_graspable_points_in_reachable_area; + end + + % Vector from Base position to goal + vec_bg = gait_planning_param.goal(:,gait_planning_param.goal_num) - SV.R0; + + for ind = 1:size(GP_in_RA(:,:,i),2) + vec_tmp = GP_in_RA(:,ind,i) - POS_e(:,i); + % too much long swing is excluded out from the possible next + % gripping points + if norm(vec_tmp) <= 0.2 + dot_tmp(1,ind,i) = dot(vec_bg(:,1), vec_tmp); + % if the following dot product is positive, the GP is located in + % front of this swing leg + if dot_tmp(1,ind,i) > 0 + %GP_in_RA_in_front(:,count,i) = GP_in_RA(:,ind,i); + GP_in_RA_in_front_with_limbCode_and_score(:,count) = [GP_in_RA(:,ind,i);i;dot_tmp(1,ind,i)]; + count = count + 1; + end + else + dot_tmp(1,ind,i) = 0; + end + end + end + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% Select new swing limb + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % decide the limb that has most graspable points in reachable area in + % front as the rentative swing limb --> next gripping points selection + gait_planning_param.num_of_graspable_points_in_front = [0;0;0;0]; + for i = 1 : 1 : LP.num_limb + for j = 1 : size(GP_in_RA_in_front_with_limbCode_and_score(:,:),2) + if GP_in_RA_in_front_with_limbCode_and_score(4,j) == i + gait_planning_param.num_of_graspable_points_in_front(i) = gait_planning_param.num_of_graspable_points_in_front(i) + 1; + end + end + end + [~,I] = max(gait_planning_param.num_of_graspable_points_in_front); + gait_planning_param.swing_number = I; + +end + +end \ No newline at end of file diff --git a/src/controller/gait/2_upd_swing_num/upd_swing_num_periodic_crawl.m b/src/controller/gait/2_upd_swing_num/upd_swing_num_periodic_crawl.m new file mode 100644 index 0000000..03b179f --- /dev/null +++ b/src/controller/gait/2_upd_swing_num/upd_swing_num_periodic_crawl.m @@ -0,0 +1,48 @@ +%%%%%% Update +%%%%%% upd_swing_num_periodic_crawl +%%%%%% +%%%%%% Update number of swing leg for the typical periodic crawl gait +%%%%%% +%%%%%% Created: 2020-04-13 +%%%%%% Warley Ribeiro +%%%%%% Last updated: 2020-11-18 +%%%%%% Kentaro Uno +% +% +% Update number of swing leg for crawl gait with fixed stride +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for path planning (class) +% +% gait_planning_param.swing_number : Number of the selected leg to be the next swing leg (scalar) +% +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_swing_num_periodic_crawl(gait_planning_param, des_SV, LP, time) + + %%% Select new swing leg + % If all legs are in support phase + if sum(des_SV.sup) == LP.num_limb + % First step + if time == 0 + gait_planning_param.swing_number = gait_planning_param.sequence(1); + else + sw_num = gait_planning_param.swing_number(1); + seq = gait_planning_param.sequence; + % Find previous swing leg + previous_index = find(seq==sw_num); + if previous_index == size(gait_planning_param.sequence,1) + gait_planning_param.swing_number = gait_planning_param.sequence(1); + else + gait_planning_param.swing_number = gait_planning_param.sequence(previous_index+1); + end + end + end + +end \ No newline at end of file diff --git a/src/controller/gait/2_upd_swing_num/upd_swing_num_periodic_timing.m b/src/controller/gait/2_upd_swing_num/upd_swing_num_periodic_timing.m new file mode 100644 index 0000000..6724a87 --- /dev/null +++ b/src/controller/gait/2_upd_swing_num/upd_swing_num_periodic_timing.m @@ -0,0 +1,39 @@ +%%%%%% Update +%%%%%% upd_swing_num_periodic_timing +%%%%%% +%%%%%% Update swing legs number based on periodic timing +%%%%%% +%%%%%% Created: 2021-04-19 +%%%%%% Warley Ribeiro +% +% +% Update number of swing leg for periodic gait, and may include several swing limbs at the same time +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for path planning (class) +% +% gait_planning_param.swing_number : Number of the selected leg to be the next swing leg (vector) +% +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% LP : Link parameters (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_swing_num_periodic_timing(gait_planning_param, LP, time) + % initialize variable + gait_planning_param.swing_number = []; + for i = 1:LP.num_limb + % Swing limb according to each limb swing timing + if time >= gait_planning_param.leg_T(1,i) && time < gait_planning_param.leg_T(2,i) + if isempty(gait_planning_param.swing_number) + gait_planning_param.swing_number = i; + % Include more swing limbs + else + gait_planning_param.swing_number(end+1) = i; + end + end + end + +end \ No newline at end of file diff --git a/src/controller/gait/2_upd_swing_num/upd_swing_timing.m b/src/controller/gait/2_upd_swing_num/upd_swing_timing.m new file mode 100644 index 0000000..cc8c2c1 --- /dev/null +++ b/src/controller/gait/2_upd_swing_num/upd_swing_timing.m @@ -0,0 +1,37 @@ +%%%%%% Update +%%%%%% upd_swing_timing +%%%%%% +%%%%%% Update swing legs timing +%%%%%% +%%%%%% Created: 2021-04-19 +%%%%%% Warley Ribeiro +% +% +% Update the timing for the swing phase +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for path planning (class) +% +% gait_planning_param.leg_T : initial and final swing time of each leg (2xn matrix) +% +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% LP : Link parameters (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_swing_timing(gait_planning_param, LP, time) + + global d_time; + + % Beginning of new cycle + if rem(time, gait_planning_param.T) < d_time + for i = 1:LP.num_limb + % Each leg initial and final swing time + gait_planning_param.leg_T(1,i) = time + gait_planning_param.phi(i); + gait_planning_param.leg_T(2,i) = gait_planning_param.leg_T(1,i) + gait_planning_param.T_d; + end + end + +end \ No newline at end of file diff --git a/src/controller/gait/2_upd_swing_num/upd_walking_sequence_based_on_moving_direction.m b/src/controller/gait/2_upd_swing_num/upd_walking_sequence_based_on_moving_direction.m new file mode 100644 index 0000000..b0b9183 --- /dev/null +++ b/src/controller/gait/2_upd_swing_num/upd_walking_sequence_based_on_moving_direction.m @@ -0,0 +1,75 @@ +%%%%%% Update +%%%%%% upd_walking_sequence_based_on_moving_direction +%%%%%% +%%%%%% Update walking sequence based on moving direction +%%%%%% +%%%%%% Created: 2021-05-04 +%%%%%% Keigo Haji +%%%%%% Last updated: +%%%%%% - +% +% +% Update walking sequence for crawl gait based on moving direction +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for path planning (class) +% +% gait_planning_param.sequence +% +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_walking_sequence_based_on_moving_direction(gait_planning_param, des_SV, SV, LP, time) + +% If all legs are in support phase +if sum(des_SV.sup) == LP.num_limb + % Plan before starting the hind leg swing period + if strcmp(gait_planning_param.crawl_gait_direction_change, 'on') && gait_planning_param.swing_number == gait_planning_param.sequence(1) + + % Set the Vector_Base_to_Goal + vec_bg = gait_planning_param.goal(:,gait_planning_param.goal_num) - SV.R0; + + %%% When the new sequence starts, Re-set the Walking sequence for + %%% crawl gait based on the current goal direction + cross_vec_tmp1 = cross([1,1,0],[vec_bg(1),vec_bg(2),0]); + if cross_vec_tmp1(3) < 0 + cross_vec_tmp2 = cross([1,-1,0],[vec_bg(1),vec_bg(2),0]); + if cross_vec_tmp2(3) < 0 + % Limb 1&2 are hind legs + gait_planning_param.sequence = [1; 4; 2; 3]; + elseif cross_vec_tmp2(3) >= 0 + % Limb 2&3 are hind legs + gait_planning_param.sequence = [2; 1; 3; 4]; + end + elseif cross_vec_tmp1(3) >= 0 + cross_vec_tmp2 = cross([1,-1,0],[vec_bg(1),vec_bg(2),0]); + if cross_vec_tmp2(3) < 0 + % Limb 1&4 are hind legs + gait_planning_param.sequence = [4; 3; 1; 2]; + elseif cross_vec_tmp2(3) >= 0 + % Limb 3&4 are hind legs + gait_planning_param.sequence = [3; 2; 4; 1]; + end + end + % If the new sequence is diffent from previous sequence, we + % need to stop swinging limb once to adjsut base position. + if gait_planning_param.swing_number ~= gait_planning_param.sequence(1) && contains(gait_planning_param.type, 'crawl') + gait_planning_param.crawl_gait_sequence_change_flag = true; + % The swing limb number is set as the last number of the new + % sequence. This is because the first swing leg of the sequence + % is selected in the next period, since the swing leg is the + % next index in the previous sequence. + gait_planning_param.swing_number = gait_planning_param.sequence(end); + else + gait_planning_param.crawl_gait_sequence_change_flag = false; + gait_planning_param.swing_number = gait_planning_param.sequence(1); + end + end +end +end + diff --git a/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_crawl_fixed_stride.m b/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_crawl_fixed_stride.m new file mode 100644 index 0000000..e487eb3 --- /dev/null +++ b/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_crawl_fixed_stride.m @@ -0,0 +1,62 @@ +%%%%%% Update +%%%%%% upd_swing_next_pos_crawl_fixed_stride +%%%%%% +%%%%%% Update next grasping position for crawl gait +%%%%%% +%%%%%% Created: 2020-04-13 +%%%%%% Warley Ribeiro +%%%%%% Last updated: 2021-04-29 +%%%%%% Keigo Haji +% +% +% Update next grasping position for crawl gait with fixed stride, based on the map +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.POS_cur : Current position of the swing leg when selecting a new one [m] (3x1 vector) +% gait_planning_param.POS_next : Next desired position of the swing leg [m] (3x1 vector) +% gait_planning_param.leg_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% des_SV : Desired state values (SpaceDyn class) +% SV : State values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_swing_next_pos_crawl_fixed_stride(gait_planning_param, des_SV, SV, LP, POS_e, time) + +%%% Select next grasping point +% If all legs are in support phase +if sum(des_SV.sup) == LP.num_limb + % Current swing leg + i = gait_planning_param.swing_number; + + for j=1:LP.num_limb + % Current position of swing leg on the surface + [gait_planning_param.POS_cur(1,j),gait_planning_param.POS_cur(2,j),... + gait_planning_param.POS_cur(3,j)] = get_map_pos(POS_e(1,j),POS_e(2,j)); + end + + % Next desired position + gait_planning_param.POS_next = POS_e; % initialize for all limb + + % Set the direction of stride to goal + vec_base_to_goal = gait_planning_param.goal(1:2,gait_planning_param.goal_num) - SV.R0(1:2); + unit_vec_base_to_goal = vec_base_to_goal/ norm(vec_base_to_goal); + fix_stride_to_goal = unit_vec_base_to_goal * gait_planning_param.step_length; + + gait_planning_param.POS_next(:,i) = gait_planning_param.POS_cur(:,i) + [fix_stride_to_goal(1); fix_stride_to_goal(2); 0]; + % Next desired position on the surface + [gait_planning_param.POS_next(1,i),gait_planning_param.POS_next(2,i),gait_planning_param.POS_next(3,i)] = ... + get_map_pos(gait_planning_param.POS_next(1,i),gait_planning_param.POS_next(2,i)); + + % Time for current and desired positions + gait_planning_param.leg_T = [time; time + gait_planning_param.T_d]; +end + + +end \ No newline at end of file diff --git a/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_max_stride_to_goal.m b/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_max_stride_to_goal.m new file mode 100644 index 0000000..afdc5c5 --- /dev/null +++ b/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_max_stride_to_goal.m @@ -0,0 +1,104 @@ +%%%%%% Update +%%%%%% upd_swing_next_pos_max_stride_to_goal +%%%%%% +%%%%%% Update next swing limb's grasping position in such a way to obtain +%%%%%% the longest stride from the graspable point options. +%%%%%% +%%%%%% Created: 2020-06-12 +%%%%%% Yusuke Koizumi +%%%%%% Last update: 2021-07-06 +%%%%%% Keigo Haji +% +% +% You can see the details of the planning method is shown in the following paper: +% -------------------------------------------------------------------- +% In: Proceedings of the IEEE/SICE International Symposium on System +% Integration (SII) 2019 by K. Uno et al. +% Proceedings Paper URL: +% https://ieeexplore.ieee.org/document/8700455 +% -------------------------------------------------------------------- +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.POS_cur : Current position of the swing leg when selecting a new one [m] (3x1 vector) +% gait_planning_param.POS_next : Next desired position of the swing leg [m] (3x1 vector) +% gait_planning_param.leg_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) +% gait_planning_param.base_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% SV : State values (SpaceDyn class) +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% time : Simulation time [s] (scalar) + + +function gait_planning_param = upd_swing_next_pos_max_stride_to_goal(gait_planning_param, SV, des_SV, LP, POS_e, time, surface_param) +if sum(des_SV.sup) == LP.num_limb + i = gait_planning_param.swing_number; + + % Current position of swing leg on the surface + [gait_planning_param.POS_cur(1,i),gait_planning_param.POS_cur(2,i),... + gait_planning_param.POS_cur(3,i)] = get_map_pos_of_graspable_points(POS_e(1,i),POS_e(2,i),surface_param); + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% dot product calculation for each GP_in_RA + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + switch i + case 1 + GP_in_RA = gait_planning_param.LF_graspable_points_in_reachable_area; + case 2 + GP_in_RA = gait_planning_param.LH_graspable_points_in_reachable_area; + case 3 + GP_in_RA = gait_planning_param.RH_graspable_points_in_reachable_area; + case 4 + GP_in_RA = gait_planning_param.RF_graspable_points_in_reachable_area; + end + + % Set the Vector_Base_to_Goal) + vec_bg = gait_planning_param.goal(:,gait_planning_param.goal_num) - SV.R0; + + % dot product calculation for each GP_in_RA + dot_tmp = zeros(1,size(GP_in_RA,2)); + gait_planning_param.POS_next = POS_e; + for ind = 1:size(GP_in_RA,2) + vec_tmp = GP_in_RA(1:2,ind) - POS_e(1:2,i); + % if the stride is too large, it is eliminated + if norm(vec_tmp) <= gait_planning_param.allowable_max_stride + dot_tmp(1,ind) = dot(vec_bg(1:2,1), vec_tmp); + else + dot_tmp(1,ind) = nan; + end + end + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% Select new grasping point + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + [~,gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride] = max(dot_tmp); + + %%% new grasping point is selected. + gait_planning_param.POS_next(:, i) = GP_in_RA(:,gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride); + + %%% Only if the all options does not satisfy the kinematic feasibility, the + %%% current same position is selected, which should pass the check. + if min( isnan(dot_tmp) ) == 1 % if all elements are nan, the input vector's minimum value is 1 + disp("The number of limb that has no feasible next graspable points: "); + disp( i ); + error("in upd_swing_next_pos_max_stride_to_goal(): The best next gripping point does not exist. The solver does not update the next grasping point and base position."); + gait_planning_param.POS_next(:,i) = POS_e(:,i); +% gait_planning_param.base_next = SV.R0; + end + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% Time for current and desired positions + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Timing + gait_planning_param.leg_T = [time; time + gait_planning_param.T_d]; + + +end + +end \ No newline at end of file diff --git a/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_periodic_fixed_stride.m b/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_periodic_fixed_stride.m new file mode 100644 index 0000000..bbf9d64 --- /dev/null +++ b/src/controller/gait/3_upd_swing_next_pos/upd_swing_next_pos_periodic_fixed_stride.m @@ -0,0 +1,69 @@ +%%%%%% Update +%%%%%% upd_swing_next_pos_periodic_fixed_stride +%%%%%% +%%%%%% Update next grasping position for crawl gait +%%%%%% +%%%%%% Created: 2021-04-20 +%%%%%% Warley Ribeiro +% +% +% Update next grasping position for crawl gait with fixed stride, based on the map +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.POS_cur : Current position of the swing leg when selecting a new one [m] (3x1 vector) +% gait_planning_param.POS_next : Next desired position of the swing leg [m] (3x1 vector) +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_swing_next_pos_periodic_fixed_stride(gait_planning_param, LP, POS_e, time) + + global d_time; + + % Initialize current position and next position + if time == 0 + for j=1:LP.num_limb + % Current position of swing leg on the surface + [gait_planning_param.POS_cur(1,j),gait_planning_param.POS_cur(2,j),... + gait_planning_param.POS_cur(3,j)] = get_map_pos(POS_e(1,j),POS_e(2,j)); + end + gait_planning_param.POS_next = gait_planning_param.POS_cur; + end + + % Check if limb is beginning to swing + for i = 1:LP.num_limb + if abs(time - gait_planning_param.leg_T(1,i)) < d_time/2 + % Update current position + gait_planning_param.POS_cur(:,i) = gait_planning_param.POS_next(:,i); + % Next desired position + gait_planning_param.POS_next(:,i) = gait_planning_param.POS_cur(:,i) + [gait_planning_param.step_length; 0; 0]; + + % Next desired position on the surface + [gait_planning_param.POS_next(1,i),gait_planning_param.POS_next(2,i),gait_planning_param.POS_next(3,i)] = ... + get_map_pos(gait_planning_param.POS_next(1,i),gait_planning_param.POS_next(2,i)); + end + end + + % Record the footholds of leg i + if time == 0 + for j=1:LP.num_limb + gait_planning_param.footholds_history_limb(:,gait_planning_param.footholds_count_limb(j),j) = POS_e(:,j); + gait_planning_param.footholds_count_limb(j) = gait_planning_param.footholds_count_limb(j) + 1; + end + end + for i = 1:LP.num_limb + if abs(time - gait_planning_param.leg_T(1,i)) < d_time + gait_planning_param.footholds_history_limb(:,gait_planning_param.footholds_count_limb(i),i) = gait_planning_param.POS_next(:,i); + gait_planning_param.footholds_count_limb(i) = gait_planning_param.footholds_count_limb(i) + 1; + end + end + + + +end \ No newline at end of file diff --git a/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_CoM_projection_on_intersection_of_diagonals.m b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_CoM_projection_on_intersection_of_diagonals.m new file mode 100644 index 0000000..8f896e9 --- /dev/null +++ b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_CoM_projection_on_intersection_of_diagonals.m @@ -0,0 +1,116 @@ +%%%%%% Update +%%%%%% upd_base_next_pos_CoM_projection_on_intersection_of_diagonals +%%%%%% +%%%%%% Update the kinematic feasibility flag (true or false) +%%%%%% +%%%%%% Created: 2020-11-18 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-04-11 +%%%%%% Kentaro Uno +% +% +% You can see the details of the planning method is shown in the following paper: +% -------------------------------------------------------------------- +% In: Proceedings of the IEEE/SICE International Symposium on System +% Integration (SII) 2019 by K. Uno et al. +% Proceedings Paper URL: +% https://ieeexplore.ieee.org/document/8700455 +% -------------------------------------------------------------------- +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.base_cur : Current position of the base when selecting a new one [m] (3x1 vector) +% gait_planning_param.base_next : Next desired position of the base [m] (3x1 vector) +% gait_planning_param.base_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) +% INPUT +% gait_planning_param : Parameters for path planning (class) +% surface_param : Parameters for surface (class) +% SV : State values (SpaceDyn class) +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% inc : Surface inclination [deg] (scalar) +% base_height : Height of base relative to map [m] +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_base_next_pos_CoM_projection_on_intersection_of_diagonals(SV,des_SV,LP,gait_planning_param,POS_e,inc,surface_param,base_height,time) + % If all legs are in support phase + if sum(des_SV.sup) == LP.num_limb + + % Initialize the next position of the base to be planned + gait_planning_param.base_next = SV.R0; + + % Update the current position of the base + gait_planning_param.base_cur = SV.R0; + + % prepare some temporary variables in advance + L1_x = zeros(2,1); % x positions of first diagonal line (connecting limb 1 and limb 3 supporting points) + L1_y = zeros(2,1); % y positions of first diagonal line (connecting limb 1 and limb 3 supporting points) + L2_x = zeros(2,1); % x positions of second diagonal line (connecting limb 2 and limb 4 supporting points) + L2_y = zeros(2,1); % y positions of second diagonal line (connecting limb 2 and limb 4 supporting points) + + for j = 1:LP.num_limb + if j == gait_planning_param.swing_number + switch j + case 1 + L1_x(1) = gait_planning_param.POS_next(1,j); + L1_y(1) = gait_planning_param.POS_next(2,j); + case 2 + L2_x(1) = gait_planning_param.POS_next(1,j); + L2_y(1) = gait_planning_param.POS_next(2,j); + case 3 + L1_x(2) = gait_planning_param.POS_next(1,j); + L1_y(2) = gait_planning_param.POS_next(2,j); + case 4 + L2_x(2) = gait_planning_param.POS_next(1,j); + L2_y(2) = gait_planning_param.POS_next(2,j); + end + else + switch j + case 1 + L1_x(1) = POS_e(1,j); + L1_y(1) = POS_e(2,j); + case 2 + L2_x(1) = POS_e(1,j); + L2_y(1) = POS_e(2,j); + case 3 + L1_x(2) = POS_e(1,j); + L1_y(2) = POS_e(2,j); + case 4 + L2_x(2) = POS_e(1,j); + L2_y(2) = POS_e(2,j); + end + end + end + + % Calculate distances + Dx12 = L1_x(1)-L1_x(2); + Dx34 = L2_x(1)-L2_x(2); + Dy12 = L1_y(1)-L1_y(2); + Dy34 = L2_y(1)-L2_y(2); + Dx24 = L1_x(2)-L2_x(2); + Dy24 = L1_y(2)-L2_y(2); + % Solve the equation + ts = [Dx12 -Dx34; Dy12 -Dy34] \ [-Dx24; -Dy24]; + % Take weighted combinations of points on the line + P = ts(1)*[L1_x(1); L1_y(1)] + (1-ts(1))*[L1_x(2); L1_y(2)]; + + % figure();hold on;grid on; + % plot(L1_x,L1_y); + % plot(L2_x,L2_y); + % plot(P(1),P(2),'ko'); + % plot(POS_e(1,:),POS_e(2,:),'bo'); + gait_planning_param.base_next(1,1) = P(1) + norm(base_height)*tan(inc*pi/180); + gait_planning_param.base_next(2,1) = P(2); + gait_planning_param.base_next(3,1) = base_height + surface_param.floor_level; + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% Time for current and desired positions + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Timing + gait_planning_param.base_T = [time; time + gait_planning_param.T_d]; + +end \ No newline at end of file diff --git a/src/controller/path/upd_base_next_crawl_fixed_stride.m b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_crawl_fixed_stride.m similarity index 52% rename from src/controller/path/upd_base_next_crawl_fixed_stride.m rename to src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_crawl_fixed_stride.m index 98e7725..d3e539f 100644 --- a/src/controller/path/upd_base_next_crawl_fixed_stride.m +++ b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_crawl_fixed_stride.m @@ -1,11 +1,12 @@ %%%%%% Update -%%%%%% upd_base_next_crawl_fixed_stride +%%%%%% upd_base_next_pos_crawl_fixed_stride %%%%%% %%%%%% Update next base position for crawl gait %%%%%% -%%%%%% Created 2020-04-13 +%%%%%% Created: 2020-04-13 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-15 +%%%%%% Last updated: 2020-11-18 +%%%%%% Kentaro Uno % % % Update next base position for crawl gait with fixed stride, considering that the base moves through the entire cycle @@ -13,32 +14,31 @@ % Function variables: % % OUTPUT -% path_planning_param : Parameters for path planning (class) +% gait_planning_param : Parameters for gait planning (class) % -% path_planning_param.base_cur : Current position of the base when selecting a new one [m] (3x1 vector) -% path_planning_param.base_next : Next desired position of the base [m] (3x1 vector) -% path_planning_param.base_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) +% gait_planning_param.base_cur : Current position of the base when selecting a new one [m] (3x1 vector) +% gait_planning_param.base_next : Next desired position of the base [m] (3x1 vector) +% gait_planning_param.base_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) % INPUT -% path_planning_param : Parameters for path planning (class) -% gait_param : Parameters for gait (class) +% gait_planning_param : Parameters for gait planning (class) % des_SV : Desired state values (SpaceDyn class) % SV : State values (SpaceDyn class) % LP : Link parameters (SpaceDyn class) % time : Simulation time [s] (scalar) -function path_planning_param = upd_base_next_crawl_fixed_stride(path_planning_param, gait_param, des_SV, SV, LP, time) +function gait_planning_param = upd_base_next_pos_crawl_fixed_stride(gait_planning_param, des_SV, SV, LP, time) %%% Select next position for the base % If all legs are in support phase if sum(des_SV.sup) == LP.num_limb % Begin of a new cycle - if path_planning_param.swing_number == gait_param.sequence(1) + if gait_planning_param.swing_number == gait_planning_param.sequence(1) % Current position of the base - path_planning_param.base_cur = SV.R0; + gait_planning_param.base_cur = SV.R0; % Next position of the base - path_planning_param.base_next = path_planning_param.base_cur + [gait_param.step_length; 0; 0]; + gait_planning_param.base_next = gait_planning_param.base_cur + [gait_planning_param.step_length; 0; 0]; % Timing - path_planning_param.base_T = [time; time + gait_param.T]; + gait_planning_param.base_T = [time; time + gait_planning_param.T]; end end diff --git a/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_crawl_gait.m b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_crawl_gait.m new file mode 100644 index 0000000..cefb76c --- /dev/null +++ b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_crawl_gait.m @@ -0,0 +1,179 @@ +%%%%%% Update +%%%%%% upd_base_next_pos_crawl_gait +%%%%%% +%%%%%% Update next base position for crawl gait +%%%%%% +%%%%%% Created: 2021-04-29 +%%%%%% Keigo Haji +%%%%%% Last updated: 2021-05-12 +%%%%%% Kentaro Uno +% +% +% Update next base position for crawl gait, considering that the +% intersection of a diagonal line and a moving direction vector +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.base_cur : Current position of the base when selecting a new one [m] (3x1 vector) +% gait_planning_param.base_next : Next desired position of the base [m] (3x1 vector) +% gait_planning_param.base_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) +% INPUT +% gait_planning_param : Parameters for path planning (class) +% surface_param : Parameters for surface (class) +% SV : State values (SpaceDyn class) +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% inc : Surface inclination [deg] (scalar) +% base_height : Height of base relative to map [m] +% time : Simulation time [s] (scalar) + + + +function gait_planning_param = upd_base_next_pos_crawl_gait(gait_planning_param, surface_param, des_SV, SV, LP, POS_e, base_height, inc, time) + +%%% Select next position for the base + +% If all legs are in support phase +if sum(des_SV.sup) == LP.num_limb + + % When the sequence is not changed, robot keeps walking and the + % nexe base position is set on a intersection of a diagonal line + % and a moving direction vector + % Plan before starting the hind leg swing period + if gait_planning_param.crawl_gait_sequence_change_flag == false ... + && (gait_planning_param.swing_number == gait_planning_param.sequence(1) || gait_planning_param.swing_number == gait_planning_param.sequence(3)) + + % Save current position of the base: this is w.r.t. the frame fixed + % on the inclined plane + gait_planning_param.base_cur = SV.R0; + + % Set the Vector_Base_to_Goal + vec_bg = gait_planning_param.goal(:,gait_planning_param.goal_num) - SV.R0; + + %%% Set the next base based on a intersection of a diagonal line and a moving direction vector + %%% NOTE THAT: This is done in the inertial frame which z axis is parallel to + %%% the gravity vector + gait_planning_param.POS_next_wrt_inertial_frame = (rpy2dc([0;pi*inc/180;0])) * gait_planning_param.POS_next; + POS_e_wrt_inertial_frame = (rpy2dc([0;pi*inc/180;0])) * POS_e; + gait_planning_param.base_cur_wrt_inertial_frame = (rpy2dc([0;pi*inc/180;0])) * gait_planning_param.base_cur; + vec_bg_wrt_inertial_frame = (rpy2dc([0;pi*inc/180;0])) * vec_bg; + + + % prepare some temporary variables in advance + L1_x = zeros(2,1); % x positions of first diagonal line (connecting limb 1 and limb 3 supporting points) + L1_y = zeros(2,1); % y positions of first diagonal line (connecting limb 1 and limb 3 supporting points) + L2_x = zeros(2,1); % x positions of second diagonal line (connecting limb 2 and limb 4 supporting points) + L2_y = zeros(2,1); % y positions of second diagonal line (connecting limb 2 and limb 4 supporting points) + L3_x = zeros(2,1); % x positions of line connecting currenct base pos and moving direction + L3_y = zeros(2,1); % y positions of line connecting currenct base pos and moving direction + + % Insert the positions of next POS_e + for j = 1:LP.num_limb + if j == gait_planning_param.swing_number + switch j + case 1 + L1_x(1) = gait_planning_param.POS_next_wrt_inertial_frame(1,j); + L1_y(1) = gait_planning_param.POS_next_wrt_inertial_frame(2,j); + case 2 + L2_x(1) = gait_planning_param.POS_next_wrt_inertial_frame(1,j); + L2_y(1) = gait_planning_param.POS_next_wrt_inertial_frame(2,j); + case 3 + L1_x(2) = gait_planning_param.POS_next_wrt_inertial_frame(1,j); + L1_y(2) = gait_planning_param.POS_next_wrt_inertial_frame(2,j); + case 4 + L2_x(2) = gait_planning_param.POS_next_wrt_inertial_frame(1,j); + L2_y(2) = gait_planning_param.POS_next_wrt_inertial_frame(2,j); + end + else + switch j + case 1 + L1_x(1) = POS_e_wrt_inertial_frame(1,j); + L1_y(1) = POS_e_wrt_inertial_frame(2,j); + case 2 + L2_x(1) = POS_e_wrt_inertial_frame(1,j); + L2_y(1) = POS_e_wrt_inertial_frame(2,j); + case 3 + L1_x(2) = POS_e_wrt_inertial_frame(1,j); + L1_y(2) = POS_e_wrt_inertial_frame(2,j); + case 4 + L2_x(2) = POS_e_wrt_inertial_frame(1,j); + L2_y(2) = POS_e_wrt_inertial_frame(2,j); + end + end + end + + % Insert the positions of current base positons and + % vector_base_to_goal in L3 + L3_x = [gait_planning_param.base_cur_wrt_inertial_frame(1); gait_planning_param.base_cur_wrt_inertial_frame(1) + vec_bg_wrt_inertial_frame(1)]; + L3_y = [gait_planning_param.base_cur_wrt_inertial_frame(2); gait_planning_param.base_cur_wrt_inertial_frame(2) + vec_bg_wrt_inertial_frame(2)]; + + if gait_planning_param.swing_number == 2 || gait_planning_param.swing_number == 4 + % Calculate the intersection of vec_bg and the diagonal line + % connecting limb 2&4 + Dx12 = L3_x(2)-L3_x(1); + Dx34 = L2_x(2)-L2_x(1); + Dy12 = L3_y(2)-L3_y(1); + Dy34 = L2_y(2)-L2_y(1); + Dx24 = L3_x(1)-L2_x(1); + Dy24 = L3_y(1)-L2_y(1); + elseif gait_planning_param.swing_number == 1 || gait_planning_param.swing_number == 3 + % Calculate the intersection of vec_bg and the diagonal line + % connecting limb 1&3 + Dx12 = L3_x(2)-L3_x(1); + Dx34 = L1_x(2)-L1_x(1); + Dy12 = L3_y(2)-L3_y(1); + Dy34 = L1_y(2)-L1_y(1); + Dx24 = L3_x(1)-L1_x(1); + Dy24 = L3_y(1)-L1_y(1); + end + + % When the diagonal line is almost parallel to vec_bg, the + % determinant of the following matrix cannot be calculated. + % Thus, the robot base will not move because the intersection + % cannot be found. + if norm(det([Dx12 -Dx34; Dy12 -Dy34])) < 0.00001 + gait_planning_param.base_next = gait_planning_param.base_cur; + else + % Solve the equation + ts = [Dx12 -Dx34; Dy12 -Dy34] \ [-Dx24; -Dy24]; + % Take weighted combinations of points on the line of vec_bg + gait_planning_param.base_next_wrt_inertial_frame = ts(1)*[L3_x(2); L3_y(2)] + (1-ts(1))*[L3_x(1); L3_y(1)]; + + gait_planning_param.desired_displacement_wrt_inertial_frame = ... + gait_planning_param.base_next_wrt_inertial_frame - gait_planning_param.base_cur_wrt_inertial_frame(1:2); + %%% Set the next desired base position w.r.t. the frame used in + %%% the ClimbLab back, that is fixed onto the inclined plane + gait_planning_param.base_next = [ gait_planning_param.base_cur(1) + gait_planning_param.desired_displacement_wrt_inertial_frame(1)/cos(pi*inc/180); + gait_planning_param.base_cur(2) + gait_planning_param.desired_displacement_wrt_inertial_frame(2); + base_height + surface_param.floor_level ]; % z value of the desired base position is updated. + end + + %%% Timing + gait_planning_param.base_T = [time; time + gait_planning_param.T/2]; + + %%% Set the half point for feasibility check + gait_planning_param.base_next_midpoint = (gait_planning_param.base_next + gait_planning_param.base_cur)/2; + + %%% When The walking sequence is updated and changed %%% + elseif gait_planning_param.crawl_gait_sequence_change_flag == true + %%% To keep TSM >= 0, stop swinging limb once, and moved base COM to the intersection of two diagonal lines + % The swing limb number was set as the last number of the new + % sequence in upd_walking_sequence_based_on_moving_direction. + % This is because the first swing leg of the sequence is selected + % in the next period, since the swing leg is the next index in the + % previous sequence. + + % Set the Next desired positions of limbs + gait_planning_param.POS_next = POS_e; % Not move limbs + % Set the next base pos based on the intersection of + % diagonal lines + gait_planning_param = upd_base_next_pos_CoM_projection_on_intersection_of_diagonals(SV,des_SV,LP,gait_planning_param,POS_e,inc,surface_param,base_height,time); + + end +end +end + diff --git a/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_periodic_fixed_stride.m b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_periodic_fixed_stride.m new file mode 100644 index 0000000..eb130bb --- /dev/null +++ b/src/controller/gait/4_upd_base_next_pos/upd_base_next_pos_periodic_fixed_stride.m @@ -0,0 +1,40 @@ +%%%%%% Update +%%%%%% upd_base_next_pos_periodic_fixed_stride +%%%%%% +%%%%%% Update next base position for periodic gait +%%%%%% +%%%%%% Created: 2021-04-20 +%%%%%% Warley Ribeiro +% +% +% Update next base position for periodic gait with fixed stride, considering that the base moves through the entire cycle +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.base_cur : Current position of the base when selecting a new one [m] (3x1 vector) +% gait_planning_param.base_next : Next desired position of the base [m] (3x1 vector) +% gait_planning_param.base_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% SV : State values (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_base_next_pos_periodic_fixed_stride(gait_planning_param, SV, time) + + global d_time; + + %%% Select next position for the base + % Beginning of new cycle + if rem(time, gait_planning_param.T) < d_time + % Current position of the base + gait_planning_param.base_cur = SV.R0; + % Next position of the base + gait_planning_param.base_next = gait_planning_param.base_cur + [gait_planning_param.step_length; 0; 0]; + % Timing + gait_planning_param.base_T = [time; time + gait_planning_param.T]; + end + +end \ No newline at end of file diff --git a/src/controller/gait/5_upd_kinematic_feasibility/upd_kinematic_feasibility.m b/src/controller/gait/5_upd_kinematic_feasibility/upd_kinematic_feasibility.m new file mode 100644 index 0000000..032f8fc --- /dev/null +++ b/src/controller/gait/5_upd_kinematic_feasibility/upd_kinematic_feasibility.m @@ -0,0 +1,183 @@ +%%%%%% Update +%%%%%% upd_kinematic_feasibility +%%%%%% +%%%%%% Update next base position in the way that the CoM projection is put +%%%%%% on the intersection of the two diagonal lines +%%%%%% +%%%%%% Created: 2020-11-18 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-07-06 +%%%%%% Keigo Haji +% +% +% You can see the details of the planning method is shown in the following paper: +% -------------------------------------------------------------------- +% In: Proceedings of the IEEE/SICE International Symposium on System +% Integration (SII) 2019 by K. Uno et al. +% Proceedings Paper URL: +% https://ieeexplore.ieee.org/document/8700455 +% -------------------------------------------------------------------- +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% +% gait_planning_param.kinematic_feasibility_check_is_OK : 1 or 0 +% INPUT +% SV : State values (SpaceDyn class +% des_SV : Desired state values (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% gait_planning_param : Parameters for gait planning (class) + +function gait_planning_param = upd_kinematic_feasibility(SV, des_SV, LP, POS_e, gait_planning_param) +% If all legs are in support phase +if sum(des_SV.sup) == LP.num_limb + +RequirementCheck_CNT = 0; +i = gait_planning_param.swing_number; + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% Requirement Check of Supporting Leg flag + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + for k = 1:LP.num_limb + 1 + + if k <= 4 % k = 1 -- 4 for current supporting points + pos_next = POS_e(:,k); + if k == i && gait_planning_param.feasibility_check_number_of_legs == 4 + % When gait_planning_param.feasibility_check_number_of_legs is set as 4. + % Do not check if the swing leg can grasp the point that it is currently grasping after it moves. + continue; + end + elseif k == 5 % k == 5 for one next grasping point for the swing limb + k = i; + pos_next = gait_planning_param.POS_next(:,k); + end + + diff1 = abs( LP.reachable_area.data_far(3,:) - ( pos_next(3,1)-SV.R0(3) ) ); + diff2 = abs( LP.reachable_area.data_near(3,:) - ( pos_next(3,1)-SV.R0(3) ) ); + [~,I_1] = min(diff1(:)); + [~,I_2] = min(diff2(:)); + % x1 = LP.reachable_area.data_far(2,I_1(1));% x2 = LP.reachable_area.data_near(2,I_2(1));% MaxRange = x1;% MinRange = x2;% MinRange_from_Joint = MinRange - LP.c0(1,1);% MaxRange_from_Joint = MaxRange - LP.c0(1,1); + MinRange_from_Joint = LP.reachable_area.data_near(2,I_2(1)) - LP.c0(1,1); + MaxRange_from_Joint = LP.reachable_area.data_far(2,I_1(1)) - LP.c0(1,1); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + if contains(gait_planning_param.type, 'crawl') ... + && (gait_planning_param.swing_number == gait_planning_param.sequence(1) || gait_planning_param.swing_number == gait_planning_param.sequence(3)) + p_check = pos_next + ( - ( gait_planning_param.base_next_midpoint - SV.R0 ) ); + else + p_check = pos_next + ( - ( gait_planning_param.base_next - SV.R0 ) ); + end + +% p_check = pos_next + ( - ( gait_planning_param.base_next - SV.R0 ) ); + p_from_CoM = p_check(:) - SV.R0; + % p_from_CoM = POS_e(:,k) - gait_planning_param.base_next; +% p_from_Joint = p_from_CoM - LP.c0(:, 3*k-2); +% p_from_Joint = p_from_CoM - SV.A0*LP.c0(:, 3*k-2); + p_from_Joint = p_from_CoM - rpy2dc(gait_planning_param.base_orientation_next)' * LP.c0(:, 3*k-2); + d = norm( p_from_Joint(1:2) ); + % ------------------The following is previous codes --------------- + % % <<< Step 1 >>> + % if d < MaxRange_from_Joint && d > MinRange_from_Joint + % % <<< Step 2 >>> + % theta = - SV.Q0(3); % initialize theta with offsetting the yaw angle at this moment + % theta = theta + atan2(p_from_Joint(2), p_from_Joint(1)); + % switch k %leg-number + % case 1 + % case 2 + % if theta <= 0 + % theta = theta + 2*pi; + % end + % case 3 + % if theta <= 0 + % theta = theta + 2*pi; + % end + % case 4 + % theta = theta + 2*pi; + % end + % if theta > ( LP.Qi(3, (3*k-2)) + deg2rad(LP.joint_limit(1,1)) ) && ... + % theta < ( LP.Qi(3, (3*k-2)) + deg2rad(LP.joint_limit(1,2)) ) + % RequirementCheck_CNT = RequirementCheck_CNT + 1; + % else + % % flag = 0; + % break; + % end + % else + % % flag = 0; + % break; + % end + % ----------------------------------------------------------------- + % <<< Step 1 >>> + if d < MaxRange_from_Joint && d > MinRange_from_Joint + % <<< Step 2 >>> + Base2limbCoxa = rpy2dc(gait_planning_param.base_orientation_next)'*LP.c0(:,3*k-2); + dot_tmp = dot(p_from_Joint(1:2), Base2limbCoxa(1:2)); + theta = acos(dot_tmp/d/norm(Base2limbCoxa(1:2))); % 0 to pi + cross_tmp = cross([p_from_Joint(1),p_from_Joint(2),0], [Base2limbCoxa(1),Base2limbCoxa(2),0]); + if cross_tmp(3) > 0 + if theta < abs(LP.joint_limit(1,1)*pi/180) + RequirementCheck_CNT = RequirementCheck_CNT + 1; + end + elseif cross_tmp(3) <= 0 + if theta < abs(LP.joint_limit(1,2)*pi/180) + RequirementCheck_CNT = RequirementCheck_CNT + 1; + end + else + % flag = 0; + break; + end + else + % flag = 0; + break; + end + end + + if gait_planning_param.feasibility_check_number_of_legs == 5 && RequirementCheck_CNT == 5 % four current supporting points + one next grasping point for the swing limb + gait_planning_param.kinematic_feasibility_check_is_OK = true; + elseif gait_planning_param.feasibility_check_number_of_legs == 4 && RequirementCheck_CNT == 4 % three current supporting points + one next grasping point for the swing limb + gait_planning_param.kinematic_feasibility_check_is_OK = true; + else + gait_planning_param.kinematic_feasibility_check_is_OK = false; + end + + %% If the next desired landing point is same as last time (i.e. the + % norm of the distance is too small), it should be removed from the + % options. + if strcmp(gait_planning_param.feasibility_check_excluding_same_graspable_point_switch, 'on') + if norm(gait_planning_param.POS_next(:, i)-POS_e(:, i)) <= 0.005 % this threshold can be smaller + gait_planning_param.kinematic_feasibility_check_is_OK = false; + end + end + + %% If the next desired landing point is same as other legs (i.e. the + % norm of the distance is too small), it should be removed from the + % options. + for j = 1:LP.num_limb + if norm(gait_planning_param.POS_next(:, i)-POS_e(:, j)) <= 0.005 + if j ~= i + gait_planning_param.kinematic_feasibility_check_is_OK = false; + end + end + end + + %% If the last solution did not pass the feasibility check, the option is removed. + if ~gait_planning_param.kinematic_feasibility_check_is_OK + % to substitute the null into the matrix, we cannnot use three + % dimensional matrix --> we should prepare them for each limb + switch i + case 1 + gait_planning_param.LF_graspable_points_in_reachable_area(:, gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride) = [nan,nan,nan]; + case 2 + gait_planning_param.LH_graspable_points_in_reachable_area(:, gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride) = [nan,nan,nan]; + case 3 + gait_planning_param.RH_graspable_points_in_reachable_area(:, gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride) = [nan,nan,nan]; + case 4 + gait_planning_param.RF_graspable_points_in_reachable_area(:, gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride) = [nan,nan,nan]; + end + end +end +end \ No newline at end of file diff --git a/src/controller/gait/6_upd_gait_history/upd_gait_history_recording.m b/src/controller/gait/6_upd_gait_history/upd_gait_history_recording.m new file mode 100644 index 0000000..db1fb0f --- /dev/null +++ b/src/controller/gait/6_upd_gait_history/upd_gait_history_recording.m @@ -0,0 +1,44 @@ +%%%%%% Update +%%%%%% upd_gait_history_recording +%%%%%% +%%%%%% Update gait planning variables history +%%%%%% +%%%%%% Created: 2021-04-20 +%%%%%% Keigo Haji +%%%%%% Last updated: 2021-04-20 +%%%%%% Keigo Haji +% +% +% Update the gait planning variables history and record them +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% LP : Link parameters (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% des_SV : Desired state values (SpaceDyn class) +% time : Simulation time [s] (scalar) +% +% +function gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time) +% If all legs are in support phase +if sum(des_SV.sup) == LP.num_limb + % Record the initial footholds of leg i in time == 0 + i = gait_planning_param.swing_number; + if time == 0 + for j=1:LP.num_limb + gait_planning_param.footholds_history_limb(:,gait_planning_param.footholds_count_limb(j),j) = POS_e(:,j); + end + end + + % Update and added the footholds of leg i and count + gait_planning_param.footholds_count_limb(i) = gait_planning_param.footholds_count_limb(i) + 1; + gait_planning_param.footholds_history_limb(:,gait_planning_param.footholds_count_limb(i),i) = gait_planning_param.POS_next(:,i); + gait_planning_param.swing_number_history = horzcat(gait_planning_param.swing_number_history, gait_planning_param.swing_number); + +end +end + diff --git a/src/controller/gait/ini_gait.m b/src/controller/gait/ini_gait.m index f7e0b27..f61cc94 100644 --- a/src/controller/gait/ini_gait.m +++ b/src/controller/gait/ini_gait.m @@ -1,113 +1,252 @@ %%%%%% Initialize %%%%%% ini_gait %%%%%% -%%%%%% Initialize gait parameters +%%%%%% Initialize gait planning parameters %%%%%% -%%%%%% Created 2020-04-08 +%%%%%% Created: 2020-04-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 -% +%%%%%% Last updated: 2021-06-28 +%%%%%% Keigo Haji % % Initialize parameters for gait, based on the selected gait type % % Function variables: % % OUTPUT -% gait_param : Parameters for gait (class) -% path_planning_param : Parameters for path planning (class) +% gait_planning_param : Parameters for gait planning (class) % motion_planning_param : Parameters for motion planning (class) % INPUT -% gait_param : Parameters for gait (class) +% SV : State values (SpaceDyn class) -function [gait_param, path_planning_param, motion_planning_param] = ini_gait(gait_param) +function [gait_planning_param, motion_planning_param] = ini_gait(gait_planning_param, SV) -path_planning_param = []; motion_planning_param = []; +% initialize the limb trajectories logging arrays +motion_planning_param.trajectories.LF = []; +motion_planning_param.trajectories.LH = []; +motion_planning_param.trajectories.RH = []; +motion_planning_param.trajectories.RF = []; + +%%% Initialize goals' count as passing points +gait_planning_param.goal_num = 1; + + +%%% Set the Walking sequence for crawl gait based on the moving direction +% The sequences can be distinguished by the first swing limb, which is +% designated by a different number. +if contains(gait_planning_param.type, 'crawl') || contains(gait_planning_param.type, 'diagonal') + if isempty(gait_planning_param.sequence) + gait_planning_param.crawl_gait_direction_change = 'on'; + vec_bg = gait_planning_param.goal(1:3, gait_planning_param.goal_num) - SV.R0; + % Calculate cross product to decide the direction which robot goes + cross_vec_tmp1 = cross([1,1,0],[vec_bg(1),vec_bg(2),0]); + if cross_vec_tmp1(3) < 0 + cross_vec_tmp2 = cross([1,-1,0],[vec_bg(1),vec_bg(2),0]); + if cross_vec_tmp2(3) < 0 + % Limb 1&2 are hind legs + gait_planning_param.sequence = [1; 4; 2; 3]; + elseif cross_vec_tmp2(3) >= 0 + % Limb 2&3 are hind legs + gait_planning_param.sequence = [2; 1; 3; 4]; + end + elseif cross_vec_tmp1(3) >= 0 + cross_vec_tmp2 = cross([1,-1,0],[vec_bg(1),vec_bg(2),0]); + if cross_vec_tmp2(3) < 0 + % Limb 1&4 are hind legs + gait_planning_param.sequence = [4; 3; 1; 2]; + elseif cross_vec_tmp2(3) >= 0 + % Limb 3&4 are hind legs + gait_planning_param.sequence = [3; 2; 4; 1]; + end + end + else + gait_planning_param.crawl_gait_direction_change = 'off'; + gait_planning_param.crawl_gait_sequence_change_flag = false; + end +else + ; +end -switch gait_param.type +switch gait_planning_param.type case 'do_nothing' - path_planning_param = []; - motion_planning_param = []; -case 'crawl_fixed_stride' +case 'crawl_fixed_stride' % Swing period for one leg - gait_param.T_d = (1 - gait_param.beta)*gait_param.T; + gait_planning_param.T_d = (1 - gait_planning_param.beta)*gait_planning_param.T; % Half swing period - gait_param.T_dr = gait_param.T_d/2; + gait_planning_param.T_dr = gait_planning_param.T_d/2; % Supporting period for one leg - gait_param.T_dd = gait_param.beta*gait_param.T; + gait_planning_param.T_dd = gait_planning_param.beta*gait_planning_param.T; % All legs supporting period - gait_param.T_ddd = (2*gait_param.beta - 1.5)*gait_param.T; + gait_planning_param.T_ddd = (2*gait_planning_param.beta - 1.5)*gait_planning_param.T; % Swing phase for each leg [s] phi = [0; - gait_param.T_d + gait_param.T_ddd; - 2*gait_param.T_d + gait_param.T_ddd; - 3*gait_param.T_d + 2*gait_param.T_ddd;]'; + gait_planning_param.T_d + gait_planning_param.T_ddd; + 2*gait_planning_param.T_d + gait_planning_param.T_ddd; + 3*gait_planning_param.T_d + 2*gait_planning_param.T_ddd;]'; % Swing phase for each limb based on the sequence for i=1:4 - gait_param.phi(gait_param.sequence(i)) = phi(i); + gait_planning_param.phi(gait_planning_param.sequence(i)) = phi(i); end - -case 'crawl_uno_ver' + +case 'periodic_crawl_gait' + + % Swing period for one leg + gait_planning_param.T_d = (1 - gait_planning_param.beta)*gait_planning_param.T; + % Half swing period + gait_planning_param.T_dr = gait_planning_param.T_d/2; + % Supporting period for one leg + gait_planning_param.T_dd = gait_planning_param.beta*gait_planning_param.T; + % All legs supporting period + gait_planning_param.T_ddd = (2*gait_planning_param.beta - 1.5)*gait_planning_param.T; + + % Swing phase for each leg [s] + phi = [0; + gait_planning_param.T_d + gait_planning_param.T_ddd; + 2*gait_planning_param.T_d + gait_planning_param.T_ddd; + 3*gait_planning_param.T_d + 2*gait_planning_param.T_ddd;]'; + + % Swing phase for each limb based on the sequence + for i=1:4 + gait_planning_param.phi(gait_planning_param.sequence(i)) = phi(i); + end + +case 'diagonal_gait_fixed_stride' %%% the followings are same as 'crawl_fixed_stride' + % Swing period for one leg + gait_planning_param.T_d = (1 - gait_planning_param.beta)*gait_planning_param.T; + % Half swing period + gait_planning_param.T_dr = gait_planning_param.T_d/2; + % Supporting period for one leg + gait_planning_param.T_dd = gait_planning_param.beta*gait_planning_param.T; + % All legs supporting period + gait_planning_param.T_ddd = (2*gait_planning_param.beta - 1.5)*gait_planning_param.T; + + % Swing phase for each leg [s] + phi = [0; + gait_planning_param.T_d + gait_planning_param.T_ddd; + 2*gait_planning_param.T_d + gait_planning_param.T_ddd; + 3*gait_planning_param.T_d + 2*gait_planning_param.T_ddd;]'; + + % Swing phase for each limb based on the sequence + for i=1:4 + gait_planning_param.phi(gait_planning_param.sequence(i)) = phi(i); + end + +case 'crawl_gait_for_discrete_footholds' + + % Swing period for one leg + gait_planning_param.T_d = (1 - gait_planning_param.beta)*gait_planning_param.T; + % Half swing period + gait_planning_param.T_dr = gait_planning_param.T_d/2; + % Supporting period for one leg + gait_planning_param.T_dd = gait_planning_param.beta*gait_planning_param.T; + % All legs supporting period + gait_planning_param.T_ddd = (2*gait_planning_param.beta - 1.5)*gait_planning_param.T; + + % Swing phase for each leg [s] + phi = [0; + gait_planning_param.T_d + gait_planning_param.T_ddd; + 2*gait_planning_param.T_d + gait_planning_param.T_ddd; + 3*gait_planning_param.T_d + 2*gait_planning_param.T_ddd;]'; + + % Swing phase for each limb based on the sequence + for i=1:4 + gait_planning_param.phi(gait_planning_param.sequence(i)) = phi(i); + end + +case 'diagonal_gait_for_discrete_footholds' % Swing period for one leg - gait_param.T_d = (1 - gait_param.beta)*gait_param.T; + gait_planning_param.T_d = (1 - gait_planning_param.beta)*gait_planning_param.T; % Half swing period - gait_param.T_dr = gait_param.T_d/2; + gait_planning_param.T_dr = gait_planning_param.T_d/2; % Supporting period for one leg - gait_param.T_dd = gait_param.beta*gait_param.T; + gait_planning_param.T_dd = gait_planning_param.beta*gait_planning_param.T; % All legs supporting period - gait_param.T_ddd = (2*gait_param.beta - 1.5)*gait_param.T; + gait_planning_param.T_ddd = (2*gait_planning_param.beta - 1.5)*gait_planning_param.T; % Swing phase for each leg [s] phi = [0; - gait_param.T_d + gait_param.T_ddd; - 2*gait_param.T_d + gait_param.T_ddd; - 3*gait_param.T_d + 2*gait_param.T_ddd;]'; + gait_planning_param.T_d + gait_planning_param.T_ddd; + 2*gait_planning_param.T_d + gait_planning_param.T_ddd; + 3*gait_planning_param.T_d + 2*gait_planning_param.T_ddd;]'; % Swing phase for each limb based on the sequence for i=1:4 - gait_param.phi(gait_param.sequence(i)) = phi(i); + gait_planning_param.phi(gait_planning_param.sequence(i)) = phi(i); end % for now, the below is exact same as the case of 'crawl_uno_ver' (20200703) -case 'nonperiodic_uno_ver' +case 'nonperiodic_gait_for_discrete_footholds' % Swing period for one leg - gait_param.T_d = (1 - gait_param.beta)*gait_param.T; + gait_planning_param.T_d = (1 - gait_planning_param.beta)*gait_planning_param.T; % Half swing period - gait_param.T_dr = gait_param.T_d/2; + gait_planning_param.T_dr = gait_planning_param.T_d/2; % Supporting period for one leg - gait_param.T_dd = gait_param.beta*gait_param.T; + gait_planning_param.T_dd = gait_planning_param.beta*gait_planning_param.T; % All legs supporting period - gait_param.T_ddd = (2*gait_param.beta - 1.5)*gait_param.T; + gait_planning_param.T_ddd = (2*gait_planning_param.beta - 1.5)*gait_planning_param.T; + +% % Swing phase for each leg [s] +% phi = [0; +% gait_planning_param.T_d + gait_planning_param.T_ddd; +% 2*gait_planning_param.T_d + gait_planning_param.T_ddd; +% 3*gait_planning_param.T_d + 2*gait_planning_param.T_ddd;]'; + +% % Swing phase for each limb based on the sequence +% for i=1:4 +% gait_planning_param.phi(gait_planning_param.sequence(i)) = phi(i); +% end + +case 'GIA_opt_based_pose_planner' % This is under development and does not work in the current version + + % Swing period for one leg + gait_planning_param.T_d = (1 - gait_planning_param.beta)*gait_planning_param.T; + % Half swing period + gait_planning_param.T_dr = gait_planning_param.T_d/2; + % Supporting period for one leg + gait_planning_param.T_dd = gait_planning_param.beta*gait_planning_param.T; + % All legs supporting period + gait_planning_param.T_ddd = (2*gait_planning_param.beta - 1.5)*gait_planning_param.T; % Swing phase for each leg [s] phi = [0; - gait_param.T_d + gait_param.T_ddd; - 2*gait_param.T_d + gait_param.T_ddd; - 3*gait_param.T_d + 2*gait_param.T_ddd;]'; + gait_planning_param.T_d + gait_planning_param.T_ddd; + 2*gait_planning_param.T_d + gait_planning_param.T_ddd; + 3*gait_planning_param.T_d + 2*gait_planning_param.T_ddd;]'; % Swing phase for each limb based on the sequence for i=1:4 - gait_param.phi(gait_param.sequence(i)) = phi(i); + gait_planning_param.phi(gait_planning_param.sequence(i)) = phi(i); end + gait_planning_param.base_move = false; otherwise disp('Invalid gait type') end -% Initialize the valuables to record footprint and com projection history -path_planning_param.footholds_history_limb = []; +% Initialize the base orientation value for gait planning +gait_planning_param.base_orientation = SV.Q0; +gait_planning_param.base_orientation_next = SV.Q0; -path_planning_param.footholds_count_limb = ones(4,1); +% Initialize the valuables to record footprint history +gait_planning_param.footholds_history_limb = []; +gait_planning_param.footholds_count_limb = ones(4,1); + +% Initialize the valuables to record com projection % @TODO: make a array to record the CoM projection -% path_planning_param.com_projection_history = []; +gait_planning_param.com_projection_history = []; + +% Initialize the valuables to record swing number +gait_planning_param.swing_number = []; +gait_planning_param.swing_number_history = []; + end \ No newline at end of file diff --git a/src/controller/gait/upd_gait_planning.m b/src/controller/gait/upd_gait_planning.m new file mode 100644 index 0000000..784c887 --- /dev/null +++ b/src/controller/gait/upd_gait_planning.m @@ -0,0 +1,255 @@ +%%%%%% Update +%%%%%% upd_gait_planning +%%%%%% +%%%%%% Update gait planning variables +%%%%%% +%%%%%% Created: 2020-04-10 +%%%%%% Warley Ribeiro +%%%%%% Last updated: 2021-05-12 +%%%%%% Keigo Haji +% +% +% Update the gait planning based on the gait type +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait planning (class) +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% surface_param : Parameters for surface (class) +% des_SV : Desired state values (SpaceDyn class) +% 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) +% sensing_camera_param : Parameters for sensing camera (class) +% time : Simulation time [s] (scalar) + +function gait_planning_param = upd_gait_planning(gait_planning_param, surface_param, des_SV, SV, LP, POS_e, environment_param, robot_param, sensing_camera_param, time) +global d_time; +inc = environment_param.inc; +base_height = robot_param.base_height; +switch gait_planning_param.type + +case 'periodic_crawl_gait' + %%% 1) Moving direction is defined by the config parameter + %%% 2) Update swing timing of legs and swinging leg number + gait_planning_param = upd_swing_timing(gait_planning_param, LP, time); + gait_planning_param = upd_swing_num_periodic_timing(gait_planning_param, LP, time); + %%% 3) Select the next grasping point + gait_planning_param = upd_swing_next_pos_periodic_fixed_stride(gait_planning_param, LP, POS_e, time); + gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time); + %%% 4) Select the next position for the base + gait_planning_param = upd_base_next_pos_crawl_fixed_stride(gait_planning_param, des_SV, SV, LP, time); + +case 'crawl_fixed_stride' + + %%% 1) Select the new swing leg + gait_planning_param = upd_swing_num_periodic_crawl(gait_planning_param, des_SV, LP, time); + %%% 2) Update walking sequence based on the moving direction + gait_planning_param = upd_walking_sequence_based_on_moving_direction(gait_planning_param, des_SV, SV, LP, time); + %%% 3) Select the next grasping point of the selected swing limb + gait_planning_param = upd_swing_next_pos_crawl_fixed_stride(gait_planning_param, des_SV, SV, LP, POS_e, time); + %%% 4) Select the next position for the base + gait_planning_param = upd_base_next_pos_crawl_gait(gait_planning_param, surface_param, des_SV, SV, LP, POS_e, base_height, inc, time); + %%% 5) Record the footholds of swing leg + gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time); + +case 'do_nothing' + +case 'diagonal_gait_fixed_stride' + +if sum(des_SV.sup) == LP.num_limb + %%% 1) Select the new swing leg + gait_planning_param = upd_swing_num_periodic_crawl(gait_planning_param, des_SV, LP, time); + %%% 2) Select the next grasping point of the selected swing limb + gait_planning_param = upd_swing_next_pos_crawl_fixed_stride(gait_planning_param, des_SV, SV, LP, POS_e, time); + %%% 3) Record the footholds of swing leg + gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time); + %%% 4) Select the next position for the base + gait_planning_param = upd_base_next_pos_CoM_projection_on_intersection_of_diagonals(SV,des_SV,LP,gait_planning_param,POS_e,inc,surface_param,base_height,time); +end + +case 'crawl_gait_for_discrete_footholds' + +if sum(des_SV.sup) == LP.num_limb + %%% 0) update the graspable points in all reachable region of limbs + gait_planning_param = upd_graspable_points_in_reachable_area(SV,des_SV,LP,surface_param, gait_planning_param, sensing_camera_param); + + %%% 1) Select new swing leg + gait_planning_param = upd_swing_num_periodic_crawl(gait_planning_param, des_SV, LP, time); + + %%% 2) Update Moving Direction (TO BE ADDED) + + %%% 3) Update walking sequence based on the moving direction + gait_planning_param = upd_walking_sequence_based_on_moving_direction(gait_planning_param, des_SV, SV, LP, time); + + + + %%% Necessary temporary variable declaration @TODO: this should be in + %%% the function and remove "sum(des_SV.sup) == LP.num_limb" from this + %%% switch sentence brock. + % kinematics feasibility chech flag + gait_planning_param.kinematic_feasibility_check_is_OK = false; + % Graspable points in Reachable area of each limb @TODO: probably we do + % not have to prepare this temp variables for each limb by each. --> we + % can make one single variable for all + gait_planning_param.LF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,1); + gait_planning_param.LH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,2); + gait_planning_param.RH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,3); + gait_planning_param.RF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,4); + + gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride = 0; % Index number used to identify the max value in the vector + while ~gait_planning_param.kinematic_feasibility_check_is_OK + %%% 4) Select the next grasping point of the selected swing limb + gait_planning_param = upd_swing_next_pos_max_stride_to_goal(gait_planning_param, SV, des_SV, LP, POS_e, time, surface_param); + %%% 5) Plan the next position for the base + gait_planning_param = upd_base_next_pos_crawl_gait(gait_planning_param, surface_param, des_SV, SV, LP, POS_e, base_height, inc, time); + %%% 6) Requirement check if the planned motion is kinematically feasible or not. If this is not satisfied, go back to 2) + gait_planning_param = upd_kinematic_feasibility(SV, des_SV, LP, POS_e, gait_planning_param); + end + + %%% 6) Record the footholds of swing leg + gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time); + +end + +case 'diagonal_gait_for_discrete_footholds' + +if sum(des_SV.sup) == LP.num_limb + %%% 0) update the graspable points in all reachable region of limbs + gait_planning_param = upd_graspable_points_in_reachable_area(SV,des_SV,LP,surface_param, gait_planning_param, sensing_camera_param); + + %%% 1) Select new swing leg + gait_planning_param = upd_swing_num_periodic_crawl(gait_planning_param, des_SV, LP, time); + + %%% 2) Update Moving Direction (TO BE ADDED) + + %%% 3) Update walking sequence based on the moving direction + gait_planning_param = upd_walking_sequence_based_on_moving_direction(gait_planning_param, des_SV, SV, LP, time); + + %%% Necessary temporary variable declaration @TODO: this should be in + %%% the function and remove "sum(des_SV.sup) == LP.num_limb" from this + %%% switch sentence brock. + % kinematics feasibility chech flag + gait_planning_param.kinematic_feasibility_check_is_OK = false; + % Graspable points in Reachable area of each limb @TODO: probably we do + % not have to prepare this temp variables for each limb by each. --> we + % can make one single variable for all + gait_planning_param.LF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,1); + gait_planning_param.LH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,2); + gait_planning_param.RH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,3); + gait_planning_param.RF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,4); + + gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride = 0; % Index number used to identify the max value in the vector + while ~gait_planning_param.kinematic_feasibility_check_is_OK + %%% 4) Select the next grasping point of the selected swing limb + gait_planning_param = upd_swing_next_pos_max_stride_to_goal(gait_planning_param, SV, des_SV, LP, POS_e, time, surface_param); + %%% 5) Plan the next position for the base + gait_planning_param = upd_base_next_pos_CoM_projection_on_intersection_of_diagonals(SV,des_SV,LP,gait_planning_param,POS_e,inc,surface_param,base_height,time); + %%% 6) Requirement check if the planned motion is kinematically feasible or not. If this is not satisfied, go back to 2) + gait_planning_param = upd_kinematic_feasibility(SV, des_SV, LP, POS_e, gait_planning_param); + end + + %%% 6) Record the footholds of swing leg + gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time); +end + + + +case 'nonperiodic_gait_for_discrete_footholds' + +if sum(des_SV.sup) == LP.num_limb % @TODO: this if sentence is written in each following function as well, so might be removed. + %%% 0) update the graspable points in all reachable region of limbs + gait_planning_param = upd_graspable_points_in_reachable_area(SV,des_SV,LP,surface_param, gait_planning_param, sensing_camera_param); + + %%% Necessary temporary variable declaration @TODO: this should be in + %%% the function and remove "sum(des_SV.sup) == LP.num_limb" from this + %%% switch sentence brock. + gait_planning_param.kinematic_feasibility_check_is_OK = false; + % Graspable points in Reachable area of each limb @TODO: probably we do + % not have to prepare this temp variables for each limb by each. --> we + % can make one single variable for all + gait_planning_param.LF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,1); + gait_planning_param.LH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,2); + gait_planning_param.RH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,3); + gait_planning_param.RF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,4); + + %%% 1) Update Moving Direction (TO BE ADDED) + + while ~gait_planning_param.kinematic_feasibility_check_is_OK + %%% 2) Select new swing leg + gait_planning_param = upd_swing_num_based_on_num_of_graspable_options(gait_planning_param, des_SV, SV, LP, POS_e, surface_param); + %%% 3) Select the next grasping point of the selected swing limb + gait_planning_param = upd_swing_next_pos_max_stride_to_goal(gait_planning_param, SV, des_SV, LP, POS_e, time, surface_param); + %%% 4) Plan the next position for the base + gait_planning_param = upd_base_next_pos_CoM_projection_on_intersection_of_diagonals(SV,des_SV,LP,gait_planning_param,POS_e,inc,surface_param,base_height,time); + %%% 5) Requirement check if the planned motion is kinematically feasible or not. If this is not satisfied, go back to 2) + gait_planning_param = upd_kinematic_feasibility(SV, des_SV, LP, POS_e, gait_planning_param); + end + + %%% 6) Record the footholds of swing leg + gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time); + +end + +case 'GIA_opt_based_pose_planner' % this is under development and does not work in the current version. + +if sum(des_SV.sup) == LP.num_limb && (round(rem(round(time - d_time,4),gait_planning_param.T_d),3) == 0 || time == 0) + %%% 0) update the graspable points in all reachable region of limbs + gait_planning_param = upd_graspable_points_in_reachable_area(SV,des_SV,LP,surface_param, gait_planning_param, sensing_camera_param); + %%% Select next motion part + if ~gait_planning_param.base_move + gait_planning_param = upd_swing_num_periodic_crawl(gait_planning_param, des_SV, LP, time); + % move base to a safe position before swing leg motion + if gait_planning_param.swing_number == 3 || gait_planning_param.swing_number == 2 + gait_planning_param.base_move = true; % flag + end + else + gait_planning_param.base_move = false; + end + %%% Necessary temporary variable declaration @TODO: this should be in + %%% the function and remove "sum(des_SV.sup) == LP.num_limb" from this + %%% switch sentence brock. + % kinematics feasibility check flag +% gait_planning_param.kinematic_feasibility_check_is_OK = false; + % Graspable points in Reachable area of each limb @TODO: probably we do + % not have to prepare this temp variables for each limb by each. --> we + % can make one single variable for all + gait_planning_param.LF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,1); + gait_planning_param.LH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,2); + gait_planning_param.RH_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,3); + gait_planning_param.RF_graspable_points_in_reachable_area = gait_planning_param.graspable_points_in_reachable_area(:,:,4); + + gait_planning_param.index_of_graspable_points_in_reachable_area_to_allow_max_stride = 0; % Index number used to identify the max value in the vector + + switch gait_planning_param.base_move + % update the next desired position and orientation for base before swing leg motion if base_move is true + case true + % Timing + gait_planning_param.base_T = [time; time + gait_planning_param.T_d]; + %%% 3) Plan the next position for the base + gait_planning_param = upd_base_next_pos_GIA_opt(gait_planning_param, des_SV, SV, LP, POS_e, time); + gait_planning_param.POS_cur = POS_e; + gait_planning_param.POS_next = POS_e; + case false + %%% 2) Select the next grasping point of the selected swing limb + gait_planning_param.base_next = SV.R0; + gait_planning_param.base_cur = SV.R0; + gait_planning_param = upd_swing_next_pos_max_stride_to_goal(gait_planning_param, SV, des_SV, LP, POS_e, time, surface_param); + % Timing + gait_planning_param.leg_T = [time; time + gait_planning_param.T_d]; + gait_planning_param.base_T = [time; time + gait_planning_param.T_d]; + %%% 3) Record the footholds of swing leg + gait_planning_param = upd_gait_history_recording(gait_planning_param, LP, POS_e, des_SV, time); + end + +end +otherwise + disp('Invalid gait type'); + +end + +end \ No newline at end of file diff --git a/src/controller/motion/upd_base_ori.m b/src/controller/motion/upd_base_ori.m new file mode 100644 index 0000000..1b77782 --- /dev/null +++ b/src/controller/motion/upd_base_ori.m @@ -0,0 +1,29 @@ +%%%%%% Update +%%%%%% upd_base_ori +%%%%%% +%%%%%% Calculate current desired orientation for base +%%%%%% +%%%%%% Created 2020-04-14 +%%%%%% Yusuke Koizumi +%%%%%% Last update: 2020-12-21 +% +% +% Calculate orientation for base +% +% Function variables: +% +% OUTPUT +% des_SV : Desired state variables (SpaceDyn class) +% INPUT +% des_SV : Desired state variables (SpaceDyn class) +% gait_planning_param : Parameters for motion planning (class) +% LP : Link Parameters (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function des_SV = upd_base_ori(des_SV, gait_planning_param, LP, time) +if gait_planning_param.base_move + des_SV.Q0 = ((gait_planning_param.base_orientation_next-gait_planning_param.base_orientation)*(time - gait_planning_param.base_T(1))/(gait_planning_param.base_T(2) - gait_planning_param.base_T(1))) + gait_planning_param.base_orientation; + des_SV.A0 = rpy2dc(des_SV.Q0)'; +end + +end \ No newline at end of file diff --git a/src/controller/motion/upd_base_traject_spline3_one_segment.m b/src/controller/motion/upd_base_traject_spline3_one_segment.m index 2b226a9..fb3d318 100644 --- a/src/controller/motion/upd_base_traject_spline3_one_segment.m +++ b/src/controller/motion/upd_base_traject_spline3_one_segment.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-14 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-14 +%%%%%% Last update: 2021-05-04 +%%%%%% Keigo Haji % % % Update coefficients for cubic spline of base trajectory @@ -13,38 +14,44 @@ % Function variables: % % OUTPUT -% motion_planning_param : Parameters for motion planning (class) -% path_planning_param : Parameters for path planning (class) +% motion_planning_param : Parameters for motion planning (class) +% gait_planning_param : Parameters for Gait Planning (class) % % motion_planning_param.spline_coeff_base: Coefficients for spline curve (3x1x(m+1)x2 matrix)) % 1st dim: x-y-z coordinates (1 to 3) % 2nd dim: leg number (1 to n) % 3rd dim: coefficient index (1 to 4) % INPUT -% path_planning_param : Parameters for path planning (class) -% gait_param : Parameters for gait (class) +% gait_planning_param : Parameters for Gait Planning (class) % motion_planning_param : Parameters for motion planning (class) % des_SV : Desired State variable (SpaceDyn class) % LP : Link Parameters (SpaceDyn class) -function [motion_planning_param, path_planning_param] = upd_base_traject_spline3_one_segment(path_planning_param, gait_param, motion_planning_param, des_SV, LP) +function [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment(gait_planning_param, motion_planning_param, des_SV, LP) % Define new trajectory for base % If all legs are in support phase - if sum(des_SV.sup) == LP.num_limb - % Begin of a new cycle - if path_planning_param.swing_number == gait_param.sequence(1) || strcmp(gait_param.type,'crawl_uno_ver') || strcmp(gait_param.type,'nonperiodic_uno_ver') - % x, y and z - for j = 1:3 - % Single segment - CC_1 = upd_spline3_pos_vel(path_planning_param.base_cur(j), path_planning_param.base_next(j),... - 0, 0, path_planning_param.base_T(1), path_planning_param.base_T(2)); - for k = 1:size(CC_1) - motion_planning_param.spline_coeff_base(j,1,k) = CC_1(k); % xyz coord, leg_num, spline_coeff, segment_num - end - end - end - end + if sum(des_SV.sup) == LP.num_limb + % Begin of a new cycle @TODO: instead of using the gait name + % switch, use gait_planning_param to change move base at each step + % or only at the beginning of each cycle + if (contains(gait_planning_param.type, 'crawl') && gait_planning_param.swing_number == gait_planning_param.sequence(1)) ... + || (contains(gait_planning_param.type, 'crawl') && gait_planning_param.swing_number == gait_planning_param.sequence(3)) ... + || (contains(gait_planning_param.type, 'crawl') && gait_planning_param.crawl_gait_sequence_change_flag == true) ... + || contains(gait_planning_param.type,'diagonal_gait') ... + || strcmp(gait_planning_param.type,'nonperiodic_gait_for_discrete_footholds') ... + || strcmp(gait_planning_param.type,'GIA_opt_based_pose_planner') + % x, y and z + for j = 1:3 + % Single segment + CC_1 = upd_spline3_pos_vel(gait_planning_param.base_cur(j), gait_planning_param.base_next(j),... + 0, 0, gait_planning_param.base_T(1), gait_planning_param.base_T(2)); + for k = 1:size(CC_1) + motion_planning_param.spline_coeff_base(j,1,k) = CC_1(k); % xyz coord, leg_num, spline_coeff, segment_num + end + end + end + end end \ No newline at end of file diff --git a/src/controller/motion/upd_base_traject_spline3_one_segment_time.m b/src/controller/motion/upd_base_traject_spline3_one_segment_time.m new file mode 100644 index 0000000..5ebfe4c --- /dev/null +++ b/src/controller/motion/upd_base_traject_spline3_one_segment_time.m @@ -0,0 +1,58 @@ +%%%%%% Update +%%%%%% upd_base_traject_spline3_one_segment_time +%%%%%% +%%%%%% Update spline trajectory coefficients for swing leg +%%%%%% +%%%%%% Created 2021-04-20 +%%%%%% Warley Ribeiro +%%%%%% Last update: 2021-04-29 +%%%%%% Keigo Haji +% +% +% Update coefficients for cubic spline of base trajectory +% +% Function variables: +% +% OUTPUT +% motion_planning_param : Parameters for motion planning (class) +% gait_planning_param : Parameters for Gait Planning (class) +% +% motion_planning_param.spline_coeff_base: Coefficients for spline curve (3x1x(m+1)x2 matrix)) +% 1st dim: x-y-z coordinates (1 to 3) +% 2nd dim: leg number (1 to n) +% 3rd dim: coefficient index (1 to 4) +% INPUT +% gait_planning_param : Parameters for Gait Planning (class) +% motion_planning_param : Parameters for motion planning (class) +% des_SV : Desired State variable (SpaceDyn class) +% time : Simulation time [s] (scalar) +% LP : Link parameters (SpaceDyn class) + +function [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment_time(gait_planning_param, motion_planning_param, time, LP) + +% Define new trajectory for base + global d_time; + % Beginning of new cycle + if rem(time, gait_planning_param.T) < d_time + % Begin of a new cycle @TODO: instead of using the gait name + % switch, use gait_planning_param to change move base at each step + % or only at the beginning of each cycle + if gait_planning_param.swing_number == gait_planning_param.sequence(1) ... + || contains(gait_planning_param.type,'diagonal_gait') ... + || strcmp(gait_planning_param.type,'crawl_gait_for_discrete_footholds') ... + || strcmp(gait_planning_param.type,'nonperiodic_gait_for_discrete_footholds') ... + || strcmp(gait_planning_param.type,'GIA_opt_based_pose_planner') + % x, y and z + for j = 1:3 + % Single segment + CC_1 = upd_spline3_pos_vel(gait_planning_param.base_cur(j), gait_planning_param.base_next(j),... + 0, 0, gait_planning_param.base_T(1), gait_planning_param.base_T(2)); + for k = 1:size(CC_1) + motion_planning_param.spline_coeff_base(j,1,k) = CC_1(k); % xyz coord, leg_num, spline_coeff, segment_num + end + end + end + end + + +end \ No newline at end of file diff --git a/src/controller/motion/upd_base_traject_spline3_two_segments.m b/src/controller/motion/upd_base_traject_spline3_two_segments.m index a4249ff..e582586 100644 --- a/src/controller/motion/upd_base_traject_spline3_two_segments.m +++ b/src/controller/motion/upd_base_traject_spline3_two_segments.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-14 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-14 +%%%%%% Last update: 2020-11-18 +%%%%%% Kentaro Uno % % % Update coefficients for cubic spline of base trajectory @@ -14,7 +15,7 @@ % % OUTPUT % motion_planning_param : Parameters for motion planning (class) -% path_planning_param : Parameters for path planning (class) +% gait_planning_param : Parameters for Gait Planning (class) % % motion_planning_param.spline_coeff_base: Coefficients for spline curve (3x1x(m+1)x2 matrix)) % 1st dim: x-y-z coordinates (1 to 3) @@ -22,24 +23,23 @@ % 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_planning_param : Parameters for gait planning (class) % motion_planning_param : Parameters for motion planning (class) % des_SV : Desired State variable (SpaceDyn class) % LP : Link Parameters (SpaceDyn class) -function [motion_planning_param, path_planning_param] = upd_base_traject_spline3_two_segments(path_planning_param, gait_param, motion_planning_param, des_SV, LP) +function [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP) % Define new trajectory for base % If all legs are in support phase if sum(des_SV.sup) == LP.num_limb % Begin of a new cycle - if path_planning_param.swing_number == gait_param.sequence(1) + if gait_planning_param.swing_number == gait_planning_param.sequence(1) % x, y and z for j = 1:3 % First segment - CC_1 = upd_spline3_pos_vel(path_planning_param.base_cur(j), path_planning_param.base_next(j),... - 0, 0, path_planning_param.base_T(1), path_planning_param.base_T(2)); + CC_1 = upd_spline3_pos_vel(gait_planning_param.base_cur(j), gait_planning_param.base_next(j),... + 0, 0, gait_planning_param.base_T(1), gait_planning_param.base_T(2)); for k = 1:size(CC_1) motion_planning_param.spline_coeff_base(j,1,k,1) = CC_1(k); % xyz coord, leg_num, spline_coeff, segment_num motion_planning_param.spline_coeff_base(j,1,k,2) = CC_1(k); diff --git a/src/controller/motion/upd_gripper_state_time.m b/src/controller/motion/upd_gripper_state_time.m index dd636e2..d5f7e4b 100644 --- a/src/controller/motion/upd_gripper_state_time.m +++ b/src/controller/motion/upd_gripper_state_time.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-17 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 +%%%%%% Last update: 2021-04-26 +%%%%%% Warley Ribeiro % % % Update the state of the gripper, if it is open or closed, based on the timing for the crawl gait @@ -15,16 +16,27 @@ % OUTPUT % des_SV : Desired state variables (SpaceDyn class) % INPUT -% gait_param : Parameters for gait (class) -% path_planning_param : Parameters for path planning (class) +% gait_planning_param : Parameters for gait planning (class) % des_SV : Desired state variables (SpaceDyn class) % time : Simulation time [s] (scalar) -function des_SV = upd_gripper_state_time(gait_param, path_planning_param, des_SV, time) +function des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time) + +global d_time -% Release gripper -des_SV.sup(path_planning_param.swing_number) = 0; -% Close gripper -if rem(time,gait_param.T_d) == 0 && time~=0 - des_SV.sup(path_planning_param.swing_number) = 1; +for i = 1:LP.num_limb + if i == gait_planning_param.swing_number + % Release gripper of swing limb + des_SV.sup(i) = 0; + % Close gripper of swing limb when reaching next grasping position + if size(gait_planning_param.leg_T,2) > 1 + close_timing = gait_planning_param.leg_T(2,i); + else + close_timing = gait_planning_param.leg_T(2); + end + if time >= (close_timing - d_time) && time~=0 + des_SV.sup(i) = 1; + end + end end + diff --git a/src/controller/motion/upd_gripper_state_time_for_intermittent_gait.m b/src/controller/motion/upd_gripper_state_time_for_intermittent_gait.m new file mode 100644 index 0000000..170e938 --- /dev/null +++ b/src/controller/motion/upd_gripper_state_time_for_intermittent_gait.m @@ -0,0 +1,31 @@ +%%%%%% Update +%%%%%% upd_gripper_state_time_for_intermittent_gait +%%%%%% +%%%%%% Change current gripper open/close state. For a gait that moves legs and base separately +%%%%%% +%%%%%% Created 2020-04-17 +%%%%%% Warley Ribeiro +%%%%%% Last update: 2020-12-21 +%%%%%% Yusuke Koizumi +% +% +% Update the state of the gripper, if it is open or closed, based on the timing for the crawl gait +% +% Function variables: +% +% OUTPUT +% des_SV : Desired state variables (SpaceDyn class) +% INPUT +% gait_planning_param : Parameters for gait planning (class) +% des_SV : Desired state variables (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function des_SV = upd_gripper_state_time_for_intermittent_gait(gait_planning_param, des_SV, time) +if ~gait_planning_param.base_move +% Release gripper +des_SV.sup(gait_planning_param.swing_number) = 0; +% Close gripper +if rem(time,gait_planning_param.T_d) == 0 && time~=0 + des_SV.sup(gait_planning_param.swing_number) = 1; +end +end \ No newline at end of file diff --git a/src/controller/motion/upd_inverse_kinematics.m b/src/controller/motion/upd_inverse_kinematics.m index a6ebef3..5c6348b 100644 --- a/src/controller/motion/upd_inverse_kinematics.m +++ b/src/controller/motion/upd_inverse_kinematics.m @@ -4,8 +4,9 @@ %%%%%% Obtain inverse kinematics %%%%%% %%%%%% Created 2020-04-17 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2021-02-12 +%%%%%% by Kentaro Uno % % % Update desired joint space configuration state from inverse kinematics @@ -24,5 +25,20 @@ % Update desired base POS des_SV.R0 = motion_planning_param.des_R0'; for i = 1:LP.num_limb - des_SV.q(3*i-2:3*i) = get_i_kine_3dof(LP, des_SV, motion_planning_param.des_POS_e(:,i), i); + + if LP.joint_allocation_type == 'mammal' + des_SV.q(3*i-2:3*i) = get_i_kine_mammal_config_3dof_limb(LP, des_SV, motion_planning_param.des_POS_e(:,i), i); + elseif LP.joint_allocation_type == 'insect' + des_SV.q(3*i-2:3*i) = get_i_kine_insect_config_3dof_limb(LP, des_SV, motion_planning_param.des_POS_e(:,i), i); + end + +% if LP.joint_limit(1,1)>= rad2deg( des_SV.q(3*i-2) ) || rad2deg( des_SV.q(3*i-2) ) >= LP.joint_limit(1,2) || ... +% LP.joint_limit(2,1)>= rad2deg( des_SV.q(3*i-1) ) || rad2deg( des_SV.q(3*i-1) ) >= LP.joint_limit(2,2) || ... +% LP.joint_limit(3,1)>= rad2deg( des_SV.q(3*i ) ) || rad2deg( des_SV.q(3*i ) ) >= LP.joint_limit(3,2) +% disp("the number of limb that had a IK problem: "); +% disp( i ); +% disp("the solution of IK is as follows [deg]: "); +% disp( rad2deg(des_SV.q(3*i-2:3*i)) ); +% error("in upd_inverse_kinematics(): the solution of IK escapes from the LP.joint_limitation"); +% end end diff --git a/src/controller/motion/upd_motion_planning.m b/src/controller/motion/upd_motion_planning.m index 20169e6..4550a85 100644 --- a/src/controller/motion/upd_motion_planning.m +++ b/src/controller/motion/upd_motion_planning.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-10 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 +%%%%%% Last update: 2021-04-29 +%%%%%% Keigo Haji % % % Update motion planning variables: desired trajectories, current step positions and state variable @@ -17,8 +18,7 @@ % 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) +% gait_planning_param : Parameters for gait planning (class) % LP : Link parameters (SpaceDyn class) % SV : State variables (SpaceDyn class) % des_SV : Desired state variables (SpaceDyn class) @@ -26,76 +26,175 @@ % time : Simulation time [s] (scalar) -function [motion_planning_param, des_SV] = upd_motion_planning(motion_planning_param, gait_param, path_planning_param, LP, SV, des_SV, cont_POS, time) +function [motion_planning_param, des_SV] = upd_motion_planning(motion_planning_param, gait_planning_param, LP, SV, des_SV, cont_POS, POS_e, time) +switch gait_planning_param.type -switch gait_param.type +case 'periodic_crawl_gait' + %%% Update trajectory + % Define new trajectory for swing leg + [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments_time(gait_planning_param, motion_planning_param, time, LP); + % Define new trajectory for base + [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment_time(gait_planning_param, motion_planning_param, time, LP); + + %%% Update desired motion for current time-step + % Desired position for legs for the current step + motion_planning_param = upd_swing_current_des_POS_spline3_two_segments_time(gait_planning_param, motion_planning_param, LP, cont_POS, time); + % Desired position for base for the current step + motion_planning_param = upd_base_current_des_POS_spline3_one_segment(gait_planning_param, motion_planning_param, LP, time); + + % Inverse Kinematics + des_SV = upd_inverse_kinematics(motion_planning_param, LP, des_SV); + %%% Update desired state variables + % Desired gripper state + des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time); case 'crawl_fixed_stride' + + % If the waking sequence is changed, the trajectory generation of the + % swing limb is skipped, because it is not made into a swing leg. + if gait_planning_param.crawl_gait_sequence_change_flag == false + %%% Update trajectory + % Define new trajectory for swing leg + [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP); + %%% Update desired motion for current time-step + % Desired position for legs for the current step + motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(gait_planning_param, motion_planning_param, LP, cont_POS, time); + end + + %%% Update trajectory + % Define new trajectory for base + [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment(gait_planning_param, motion_planning_param, des_SV, LP); + %%% Update desired motion for current time-step + % Desired position for base for the current step + motion_planning_param = upd_base_current_des_POS_spline3_one_segment(gait_planning_param, motion_planning_param, LP, time); + + %%% Update desired state variables + % Inverse Kinematics + des_SV = upd_inverse_kinematics(motion_planning_param, LP, des_SV); + % Desired gripper state + des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time); + +case 'diagonal_gait_fixed_stride' %%% Update trajectory % Define new trajectory for swing leg - [motion_planning_param, path_planning_param] = upd_swing_traject_spline3_two_segments(path_planning_param,gait_param,motion_planning_param, des_SV, LP); + [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP); % Define new trajectory for base - [motion_planning_param, path_planning_param] = upd_base_traject_spline3_one_segment(path_planning_param, gait_param, motion_planning_param, des_SV, LP); + [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment(gait_planning_param, motion_planning_param, des_SV, LP); %%% Update desired motion for current time-step % Desired position for legs for the current step - motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(path_planning_param, motion_planning_param, LP, cont_POS, time); + motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(gait_planning_param, motion_planning_param, LP, cont_POS, time); % Desired position for base for the current step - motion_planning_param = upd_base_current_des_POS_spline3_one_segment(path_planning_param, motion_planning_param, LP, time); + motion_planning_param = upd_base_current_des_POS_spline3_one_segment(gait_planning_param, motion_planning_param, LP, time); %%% Update desired state variables - % Desired gripper state - des_SV = upd_gripper_state_time(gait_param, path_planning_param, des_SV, time); % Inverse Kinematics des_SV = upd_inverse_kinematics(motion_planning_param, LP, des_SV); + % Desired gripper state + des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time); case 'do_nothing' des_SV = des_SV; -case 'crawl_uno_ver' +case 'crawl_gait_for_discrete_footholds' + + % If the waking sequence is changed, the trajectory generation of the + % swing limb is skipped, because it is not made into a swing leg. + if gait_planning_param.crawl_gait_sequence_change_flag == false + %%% Update trajectory + % Define new trajectory for swing leg + [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP); + %%% Update desired motion for current time-step + % Desired position for legs for the current step + motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(gait_planning_param, motion_planning_param, LP, cont_POS, time); + end + + %%% Update trajectory + % Define new trajectory for base + [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment(gait_planning_param, motion_planning_param, des_SV, LP); + %%% Update desired motion for current time-step + % Desired position for base for the current step + motion_planning_param = upd_base_current_des_POS_spline3_one_segment(gait_planning_param, motion_planning_param, LP, time); + + %%% Update desired state variables + % Inverse Kinematics + des_SV = upd_inverse_kinematics(motion_planning_param, LP, des_SV); + % Desired gripper state + des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time); + +case 'diagonal_gait_for_discrete_footholds' + %%% Update trajectory + % Define new trajectory for swing leg + [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP); + %%% Update desired motion for current time-step + % Desired position for legs for the current step + motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(gait_planning_param, motion_planning_param, LP, cont_POS, time); + + %%% Update trajectory + % Define new trajectory for base + [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment(gait_planning_param, motion_planning_param, des_SV, LP); + %%% Update desired motion for current time-step + % Desired position for base for the current step + motion_planning_param = upd_base_current_des_POS_spline3_one_segment(gait_planning_param, motion_planning_param, LP, time); + + %%% Update desired state variables + % Inverse Kinematics + des_SV = upd_inverse_kinematics(motion_planning_param, LP, des_SV); + % Desired gripper state + des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time); + +case 'nonperiodic_gait_for_discrete_footholds' %%% Update trajectory % Define new trajectory for swing leg - [motion_planning_param, path_planning_param] = upd_swing_traject_spline3_two_segments(path_planning_param,gait_param,motion_planning_param, des_SV, LP); + [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP); % Define new trajectory for base - [motion_planning_param, path_planning_param] = upd_base_traject_spline3_one_segment(path_planning_param, gait_param, motion_planning_param, des_SV, LP); + [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment(gait_planning_param, motion_planning_param, des_SV, LP); %%% Update desired motion for current time-step % Desired position for legs for the current step - motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(path_planning_param, motion_planning_param, LP, cont_POS, time); + motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(gait_planning_param, motion_planning_param, LP, cont_POS, time); % Desired position for base for the current step - motion_planning_param = upd_base_current_des_POS_spline3_one_segment(path_planning_param, motion_planning_param, LP, time); + motion_planning_param = upd_base_current_des_POS_spline3_one_segment(gait_planning_param, motion_planning_param, LP, time); %%% Update desired state variables - % Desired gripper state - des_SV = upd_gripper_state_time(gait_param, path_planning_param, des_SV, time); % Inverse Kinematics des_SV = upd_inverse_kinematics(motion_planning_param, LP, des_SV); - -% for now, the below is exact same as the case of 'crawl_uno_ver' (20200703) -case 'nonperiodic_uno_ver' + % Desired gripper state + des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time); + +case 'GIA_opt_based_pose_planner' % This is under development and does not work in the current version %%% Update trajectory % Define new trajectory for swing leg - [motion_planning_param, path_planning_param] = upd_swing_traject_spline3_two_segments(path_planning_param,gait_param,motion_planning_param, des_SV, LP); + [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP); % Define new trajectory for base - [motion_planning_param, path_planning_param] = upd_base_traject_spline3_one_segment(path_planning_param, gait_param, motion_planning_param, des_SV, LP); + [motion_planning_param, gait_planning_param] = upd_base_traject_spline3_one_segment(gait_planning_param, motion_planning_param, des_SV, LP); %%% Update desired motion for current time-step % Desired position for legs for the current step - motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(path_planning_param, motion_planning_param, LP, cont_POS, time); + motion_planning_param = upd_swing_current_des_POS_spline_for_intermittent_gait(gait_planning_param, motion_planning_param, LP, cont_POS, time); % Desired position for base for the current step - motion_planning_param = upd_base_current_des_POS_spline3_one_segment(path_planning_param, motion_planning_param, LP, time); + motion_planning_param = upd_base_current_des_POS_spline3_one_segment(gait_planning_param, motion_planning_param, LP, time); + des_SV = upd_base_ori(des_SV, gait_planning_param, LP, time); %%% Update desired state variables - % Desired gripper state - des_SV = upd_gripper_state_time(gait_param, path_planning_param, des_SV, time); % Inverse Kinematics des_SV = upd_inverse_kinematics(motion_planning_param, LP, des_SV); - + % Desired gripper state + des_SV = upd_gripper_state_time(gait_planning_param, LP, des_SV, time); otherwise disp('Invalid gait type') end + +%%% update four limb's trajectories +motion_planning_param.trajectories.LF = [motion_planning_param.trajectories.LF POS_e(:,1)]; +motion_planning_param.trajectories.LH = [motion_planning_param.trajectories.LH POS_e(:,2)]; +motion_planning_param.trajectories.RH = [motion_planning_param.trajectories.RH POS_e(:,3)]; +motion_planning_param.trajectories.RF = [motion_planning_param.trajectories.RF POS_e(:,4)]; + + + end \ No newline at end of file diff --git a/src/controller/motion/upd_swing_current_des_POS_spline3_two_segments.m b/src/controller/motion/upd_swing_current_des_POS_spline3_two_segments.m index 8ce99f9..ff52db7 100644 --- a/src/controller/motion/upd_swing_current_des_POS_spline3_two_segments.m +++ b/src/controller/motion/upd_swing_current_des_POS_spline3_two_segments.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-14 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-16 +%%%%%% Last update: 2020-11-18 +%%%%%% Kentaro Uno % % % Calculate current position for the swing limb considering a cubic spline curve connected by a mid-point after half of the @@ -17,7 +18,7 @@ % % OUTPUT % motion_planning_param : Parameters for motion planning (class) -% path_planning_param : Parameters for path planning (class) +% gait_planning_param : Parameters for Gait Planning (class) % % motion_planning_param.spline_coeff_base: Coefficients for spline curve (3x1x(m+1)x2 matrix)) % 1st dim: x-y-z coordinates (1 to 3) @@ -25,19 +26,19 @@ % 3rd dim: coefficient index (1 to 4) % 4th dim: segment number (1 or 2) % INPUT -% path_planning_param : Parameters for path planning (class) +% gait_planning_param : Parameters for Gait Planning (class) % motion_planning_param : Parameters for motion planning (class) % LP : Link parameters (SpaceDyn class) % cont_POS : Contact position [m] (3xn matrix) % time : Simulation time [s] (scalar) -function motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(path_planning_param, motion_planning_param, LP, cont_POS, time) +function motion_planning_param = upd_swing_current_des_POS_spline3_two_segments(gait_planning_param, motion_planning_param, LP, cont_POS, time) for i = 1:LP.num_limb % Current swing limb - if i == path_planning_param.swing_number + if i == gait_planning_param.swing_number % Check if before or after mid-point - if time <= 0.5*(path_planning_param.leg_T(1) + path_planning_param.leg_T(2)) + if time <= 0.5*(gait_planning_param.leg_T(1) + gait_planning_param.leg_T(2)) j = 1; else j = 2; diff --git a/src/controller/motion/upd_swing_current_des_POS_spline3_two_segments_time.m b/src/controller/motion/upd_swing_current_des_POS_spline3_two_segments_time.m new file mode 100644 index 0000000..0e889c6 --- /dev/null +++ b/src/controller/motion/upd_swing_current_des_POS_spline3_two_segments_time.m @@ -0,0 +1,55 @@ +%%%%%% Update +%%%%%% upd_swing_current_des_POS_spline3_two_segments_time +%%%%%% +%%%%%% Calculate current desired position for the swing limb from the cubic spline trajectory +%%%%%% +%%%%%% Created 2020-04-20 +%%%%%% Warley Ribeiro +% +% +% Calculate current position for the swing limb considering a cubic spline curve connected by a mid-point after half of the +% swing period +% +% x(t) = a0 + a1*t + a2*t^2 + a3*t^3 +% +% Function variables: +% +% OUTPUT +% motion_planning_param : Parameters for motion planning (class) +% gait_planning_param : Parameters for Gait Planning (class) +% +% motion_planning_param.spline_coeff_base: Coefficients for spline curve (3x1x(m+1)x2 matrix)) +% 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_planning_param : Parameters for Gait Planning (class) +% motion_planning_param : Parameters for motion planning (class) +% LP : Link parameters (SpaceDyn class) +% cont_POS : Contact position [m] (3xn matrix) +% time : Simulation time [s] (scalar) + +function motion_planning_param = upd_swing_current_des_POS_spline3_two_segments_time(gait_planning_param, motion_planning_param, LP, cont_POS, time) + +for i = 1:LP.num_limb + % Current swing limb + if i == gait_planning_param.swing_number + % Check if before or after mid-point + if time <= 0.5*(gait_planning_param.leg_T(1,i) + gait_planning_param.leg_T(2,i)) + j = 1; + else + j = 2; + end + % Desired position for swing leg + motion_planning_param.des_POS_e(:,i) = motion_planning_param.spline_coeff_leg(:,i,1,j) + ... + motion_planning_param.spline_coeff_leg(:,i,2,j)*time + ... + motion_planning_param.spline_coeff_leg(:,i,3,j)*time.^2 + ... + motion_planning_param.spline_coeff_leg(:,i,4,j)*time.^3; + else + % Desired position for support leg + motion_planning_param.des_POS_e(:,i) = cont_POS(:,i); + end +end + +end \ No newline at end of file diff --git a/src/controller/motion/upd_swing_current_des_POS_spline_for_intermittent_gait.m b/src/controller/motion/upd_swing_current_des_POS_spline_for_intermittent_gait.m new file mode 100644 index 0000000..56b4cf1 --- /dev/null +++ b/src/controller/motion/upd_swing_current_des_POS_spline_for_intermittent_gait.m @@ -0,0 +1,61 @@ +%%%%%% Update +%%%%%% upd_swing_current_des_POS_spline3_two_segments_for_intermittent_gait +%%%%%% +%%%%%% Calculate current desired position for the swing limb from the cubic +%%%%%% spline trajectory, For a gait that moves legs and base separately +%%%%%% +%%%%%% Created 2020-04-14 +%%%%%% Warley Ribeiro +%%%%%% Last update: 2020-12-21 +%%%%%% Yusuke Koizumi +% +% +% Calculate current position for the swing limb considering a cubic spline curve connected by a mid-point after half of the +% swing period +% +% x(t) = a0 + a1*t + a2*t^2 + a3*t^3 +% +% Function variables: +% +% OUTPUT +% motion_planning_param : Parameters for motion planning (class) +% gait_planning_param : Parameters for Gait Planning (class) +% +% motion_planning_param.spline_coeff_base: Coefficients for spline curve (3x1x(m+1)x2 matrix)) +% 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_planning_param : Parameters for Gait Planning (class) +% motion_planning_param : Parameters for motion planning (class) +% LP : Link parameters (SpaceDyn class) +% cont_POS : Contact position [m] (3xn matrix) +% time : Simulation time [s] (scalar) + +function motion_planning_param = upd_swing_current_des_POS_spline_for_intermittent_gait(gait_planning_param, motion_planning_param, LP, cont_POS, time) +if ~gait_planning_param.base_move + for i = 1:LP.num_limb + % Current swing limb + if i == gait_planning_param.swing_number + % Check if before or after mid-point + if time <= 0.5*(gait_planning_param.leg_T(1) + gait_planning_param.leg_T(2)) + j = 1; + else + j = 2; + end + % Desired position for swing leg + motion_planning_param.des_POS_e(:,i) = motion_planning_param.spline_coeff_leg(:,i,1,j) + ... + motion_planning_param.spline_coeff_leg(:,i,2,j)*time + ... + motion_planning_param.spline_coeff_leg(:,i,3,j)*time.^2 + ... + motion_planning_param.spline_coeff_leg(:,i,4,j)*time.^3; + else + % Desired position for support leg + motion_planning_param.des_POS_e(:,i) = cont_POS(:,i); + end + end +else + motion_planning_param.des_POS_e = cont_POS; +end + +end \ No newline at end of file diff --git a/src/controller/motion/upd_swing_traject_spline3_two_segments.m b/src/controller/motion/upd_swing_traject_spline3_two_segments.m index ba5a237..cf90182 100644 --- a/src/controller/motion/upd_swing_traject_spline3_two_segments.m +++ b/src/controller/motion/upd_swing_traject_spline3_two_segments.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-14 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-14 +%%%%%% Last updated: 2020-11-18 +%%%%%% Kentaro Uno % % % Update coefficients for cubic spline of swing leg trajectory @@ -14,7 +15,7 @@ % % OUTPUT % motion_planning_param : Parameters for motion planning (class) -% path_planning_param : Parameters for path planning (class) +% gait_planning_param : Parameters for Gait Planning (class) % % motion_planning_param.spline_coeff_leg: Coefficients for spline curve (3xnx(m+1)x2 matrix)) % 1st dim: x-y-z coordinates (1 to 3) @@ -22,36 +23,35 @@ % 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_planning_param : Parameters for Gait Planning (class) % motion_planning_param : Parameters for motion planning (class) % LP : Link parameters (SpaceDyn class) % des_SV : Desired state variables (SpaceDyn class) -function [motion_planning_param, path_planning_param] = upd_swing_traject_spline3_two_segments(path_planning_param, gait_param, motion_planning_param, des_SV, LP) +function [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments(gait_planning_param, motion_planning_param, des_SV, LP) % Define new trajectory for swing leg % If all legs are in support phase if sum(des_SV.sup) == LP.num_limb - i = path_planning_param.swing_number; + i = gait_planning_param.swing_number; % Midpoint position - path_planning_param.POS_mid(1:2,i) = 0.5*(path_planning_param.POS_cur(1:2,i) + path_planning_param.POS_next(1:2,i)); - path_planning_param.POS_mid(3,i) = path_planning_param.POS_cur(3,i) + gait_param.step_height; + gait_planning_param.POS_mid(1:2,i) = 0.5*(gait_planning_param.POS_cur(1:2,i) + gait_planning_param.POS_next(1:2,i)); + gait_planning_param.POS_mid(3,i) = gait_planning_param.POS_cur(3,i) + gait_planning_param.step_height; % Midpoint acceleration xm_dd = [ 0; 0; 0 ]; - t_m = 0.5*(path_planning_param.leg_T(1) + path_planning_param.leg_T(2)); + t_m = 0.5*(gait_planning_param.leg_T(1) + gait_planning_param.leg_T(2)); % x and y for j = 1:2 % First segment - CC_1 = upd_spline3_pos_vel_acc(path_planning_param.POS_cur(j,i), path_planning_param.POS_mid(j,i),... - 0, xm_dd(j), path_planning_param.leg_T(1), t_m); + CC_1 = upd_spline3_pos_vel_acc(gait_planning_param.POS_cur(j,i), gait_planning_param.POS_mid(j,i),... + 0, xm_dd(j), gait_planning_param.leg_T(1), t_m); % Second segment - CC_2 = upd_spline3_pos_acc_vel(path_planning_param.POS_mid(j,i), path_planning_param.POS_next(j,i),... - xm_dd(j), 0, t_m, path_planning_param.leg_T(2)); + CC_2 = upd_spline3_pos_acc_vel(gait_planning_param.POS_mid(j,i), gait_planning_param.POS_next(j,i),... + xm_dd(j), 0, t_m, gait_planning_param.leg_T(2)); for k = 1:size(CC_1) motion_planning_param.spline_coeff_leg(j,i,k,1) = CC_1(k); % xyz coord, leg_num, spline_coeff, segment_num @@ -61,11 +61,11 @@ % z j = 3; % First segment - CC_1 = upd_spline3_pos_vel(path_planning_param.POS_cur(j,i), path_planning_param.POS_mid(j,i),... - 0, 0, path_planning_param.leg_T(1), t_m); + CC_1 = upd_spline3_pos_vel(gait_planning_param.POS_cur(j,i), gait_planning_param.POS_mid(j,i),... + 0, 0, gait_planning_param.leg_T(1), t_m); % Second segment - CC_2 = upd_spline3_pos_vel(path_planning_param.POS_mid(j,i), path_planning_param.POS_next(j,i),... - 0, 0, t_m, path_planning_param.leg_T(2)); + CC_2 = upd_spline3_pos_vel(gait_planning_param.POS_mid(j,i), gait_planning_param.POS_next(j,i),... + 0, 0, t_m, gait_planning_param.leg_T(2)); for k = 1:size(CC_1) motion_planning_param.spline_coeff_leg(j,i,k,1) = CC_1(k); motion_planning_param.spline_coeff_leg(j,i,k,2) = CC_2(k); diff --git a/src/controller/motion/upd_swing_traject_spline3_two_segments_time.m b/src/controller/motion/upd_swing_traject_spline3_two_segments_time.m new file mode 100644 index 0000000..e80dab7 --- /dev/null +++ b/src/controller/motion/upd_swing_traject_spline3_two_segments_time.m @@ -0,0 +1,77 @@ +%%%%%% Update +%%%%%% upd_swing_traject_spline3_two_segments_time +%%%%%% +%%%%%% Update spline trajectory coefficients for swing leg +%%%%%% +%%%%%% Created 2021-04-20 +%%%%%% Warley Ribeiro +% +% +% Update coefficients for cubic spline of swing leg trajectory +% +% Function variables: +% +% OUTPUT +% motion_planning_param : Parameters for motion planning (class) +% gait_planning_param : Parameters for Gait Planning (class) +% +% motion_planning_param.spline_coeff_leg: Coefficients for spline curve (3xnx(m+1)x2 matrix)) +% 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_planning_param : Parameters for Gait Planning (class) +% motion_planning_param : Parameters for motion planning (class) +% time : Simulation time (scalar) +% LP : Link parameters (SpaceDyn class) + +function [motion_planning_param, gait_planning_param] = upd_swing_traject_spline3_two_segments_time(gait_planning_param, motion_planning_param, time, LP) + +global d_time; + +% Define new trajectory for swing leg +% Check if limb is beginning to swing +for i = 1:LP.num_limb + if abs(time - gait_planning_param.leg_T(1,i)) < d_time + + + % Midpoint position + gait_planning_param.POS_mid(1:2,i) = 0.5*(gait_planning_param.POS_cur(1:2,i) + gait_planning_param.POS_next(1:2,i)); + gait_planning_param.POS_mid(3,i) = gait_planning_param.POS_cur(3,i) + gait_planning_param.step_height; + % Midpoint acceleration + xm_dd = [ 0; 0; 0 ]; + t_m = 0.5*(gait_planning_param.leg_T(1,i) + gait_planning_param.leg_T(2,i)); + + + % x and y + for j = 1:2 + % First segment + CC_1 = upd_spline3_pos_vel_acc(gait_planning_param.POS_cur(j,i), gait_planning_param.POS_mid(j,i),... + 0, xm_dd(j), gait_planning_param.leg_T(1,i), t_m); + % Second segment + CC_2 = upd_spline3_pos_acc_vel(gait_planning_param.POS_mid(j,i), gait_planning_param.POS_next(j,i),... + xm_dd(j), 0, t_m, gait_planning_param.leg_T(2,i)); + + for k = 1:size(CC_1) + motion_planning_param.spline_coeff_leg(j,i,k,1) = CC_1(k); % xyz coord, leg_num, spline_coeff, segment_num + motion_planning_param.spline_coeff_leg(j,i,k,2) = CC_2(k); + end + end + % z + j = 3; + % First segment + CC_1 = upd_spline3_pos_vel(gait_planning_param.POS_cur(j,i), gait_planning_param.POS_mid(j,i),... + 0, 0, gait_planning_param.leg_T(1,i), t_m); + % Second segment + CC_2 = upd_spline3_pos_vel(gait_planning_param.POS_mid(j,i), gait_planning_param.POS_next(j,i),... + 0, 0, t_m, gait_planning_param.leg_T(2,i)); + for k = 1:size(CC_1) + motion_planning_param.spline_coeff_leg(j,i,k,1) = CC_1(k); + motion_planning_param.spline_coeff_leg(j,i,k,2) = CC_2(k); + end + end +end + + +end \ No newline at end of file diff --git a/src/controller/path/upd_graspable_points_in_reachable_area.m b/src/controller/path/upd_graspable_points_in_reachable_area.m deleted file mode 100644 index 3c7d016..0000000 --- a/src/controller/path/upd_graspable_points_in_reachable_area.m +++ /dev/null @@ -1,94 +0,0 @@ -%%%%%% Update -%%%%%% upd_graspable_points_in_reachable_area -%%%%%% -%%%%%% Update the graspable points in reachable area -%%%%%% -%%%%%% Created 2020-05-18 -%%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-09-11 -%%%%%% Kentaro Uno -% -% -% Update the graspable points in reachable area for path planning -% -% Function variables: -% -% OUTPUT -% path_planning_param : Parameters for path planning (class) -% INPUT -% SV : State values (SpaceDyn class) -% LP : Link parameters (SpaceDyn class) -% surface_param.graspable_points : 3xN matrix of the graspable points in the map [m] -% path_planning_param : Parameters for path planning (class) - -function [path_planning_param] = upd_graspable_points_in_reachable_area(SV,LP,surface_param,path_planning_param) -% judge the graspable point_i is reachable by leg-i -p_from_Joint = zeros(3,1); -% prepare the array to store the graspable points in reachable area [[points], n, Limb number ] -path_planning_param.graspable_points_in_reachable_area = []; - -% Position of graspable points relative to Base -graspable_points(3,:) = surface_param.graspable_points(3,:) - SV.R0(3,1); - -% get an index of the swing leg -for i = 1 : 1 : LP.num_limb; - count = 1; - for k = 1 : length(surface_param.graspable_points) - - diff1 = abs( LP.reachable_area.data_far(3,:) - graspable_points(3,k)); - diff2 = abs( LP.reachable_area.data_near(3,:) - graspable_points(3,k)); - % M_n: Min. value, I_n: the index - % *see also the usage of min() for more info.) - [~,I_1] = min(diff1(:)); - [~,I_2] = min(diff2(:)); - - if I_1(1) - x1 = LP.reachable_area.data_far(2,I_1(1)); - y1 = LP.reachable_area.data_far(3,I_1(1)); - end - if I_2(1) - x2 = LP.reachable_area.data_near(2,I_2(1)); - y2 = LP.reachable_area.data_near(3,I_2(1)); - end - - % MaxRange and MinRange are defined by the height of surface_param.graspable_points - % respectively. Note that surface_param.graspable_points's height is defined in the way - % that robot CoM is height = zero. - MaxRange = x1; - MinRange = x2; - MinRange_from_Joint = MinRange - LP.c0(1,1); - MaxRange_from_Joint = MaxRange - LP.c0(1,1); - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - p_from_Joint = surface_param.graspable_points(:,k) - (SV.R0 + SV.A0*LP.c0(:,3*i-2)); - d = norm(p_from_Joint(1:2)); - % <<< Step 1 >>> - if d < MaxRange_from_Joint && d > MinRange_from_Joint - % <<< Step 2 >>> - theta = atan2(p_from_Joint(2), p_from_Joint(1)); - switch i %leg-number - case 1 - case 2 - if theta <= 0 - theta = theta + 2*pi; - end - case 3 - - if theta <= 0 - theta = theta + 2*pi; - end - case 4 - theta = theta + 2*pi; - end - if theta > LP.Qi(3, (3*i-2)) + LP.joint_limit(1,1)*pi/180 && theta < LP.Qi(3, (3*i-2)) + LP.joint_limit(1,2)*pi/180 - path_planning_param.graspable_points_in_reachable_area(:,count,i) = [surface_param.graspable_points(1,k); surface_param.graspable_points(2,k); surface_param.graspable_points(3,k)]; - count = count + 1; - % plot3(surface_param.graspable_points(1,k), surface_param.graspable_points(2,k), surface_param.graspable_points(3,k),'.r',... - % 'MarkerEdgeColor',[0.0 1.0 0.0],'MarkerSize',10) - end - end -end - -end -end - diff --git a/src/controller/path/upd_path_planning.m b/src/controller/path/upd_path_planning.m deleted file mode 100644 index 4707412..0000000 --- a/src/controller/path/upd_path_planning.m +++ /dev/null @@ -1,66 +0,0 @@ -%%%%%% Update -%%%%%% upd_path_planning -%%%%%% -%%%%%% Update path planning variables -%%%%%% -%%%%%% Created 2020-04-10 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-11 by Kentaro Uno -% -% -% Update the path planning based on the gait type -% -% Function variables: -% -% OUTPUT -% path_planning_param : Parameters for path planning (class) -% INPUT -% path_planning_param : Parameters for path planning (class) -% gait_param : Parameters for gait (class) -% surface_param : Parameters for surface (class) -% des_SV : Desired state values (SpaceDyn class) -% 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) - -inc = environment_param.inc; -base_height = robot_param.base_height; -switch gait_param.type - -case 'crawl_fixed_stride' - % Select new swing leg - path_planning_param = upd_swing_number_crawl_fixed_stride(path_planning_param, gait_param, des_SV, LP, time); - % Select next grasping point - path_planning_param = upd_swing_next_pos_crawl_fixed_stride(path_planning_param, gait_param, des_SV, LP, POS_e, time); - % Select next position for the base - path_planning_param = upd_base_next_crawl_fixed_stride(path_planning_param, gait_param, des_SV, SV, LP, time); - -case 'do_nothing' - path_planning_param = []; - -case 'crawl_uno_ver' - - % update the graspable points in all reachable region of limbs - path_planning_param = upd_graspable_points_in_reachable_area(SV,LP,surface_param,path_planning_param); - % Select new swing leg - path_planning_param = upd_swing_number_crawl_fixed_stride(path_planning_param, gait_param, des_SV, LP, time); - % Select next grasping point of the selected swing limb - path_planning_param = upd_swing_next_pos_crawl_uno_ver(path_planning_param, gait_param, surface_param, SV, des_SV, LP, POS_e, inc, base_height, time); - -case 'nonperiodic_uno_ver' - - % update the graspable points in all reachable region of limbs - path_planning_param = upd_graspable_points_in_reachable_area(SV,LP,surface_param,path_planning_param); - % Select new swing leg and its next grasping point of the selected swing limb - path_planning_param = upd_swing_num_and_grasping_point_based_on_graspable_options(path_planning_param, gait_param, surface_param, des_SV, SV, LP, POS_e, inc, base_height, time); - - otherwise - disp('Invalid gait type'); -end - -end \ No newline at end of file diff --git a/src/controller/path/upd_swing_next_pos_crawl_fixed_stride.m b/src/controller/path/upd_swing_next_pos_crawl_fixed_stride.m deleted file mode 100644 index eb4b60e..0000000 --- a/src/controller/path/upd_swing_next_pos_crawl_fixed_stride.m +++ /dev/null @@ -1,62 +0,0 @@ -%%%%%% Update -%%%%%% upd_swing_next_pos_crawl_fixed_stride -%%%%%% -%%%%%% Update next grasping position for crawl gait -%%%%%% -%%%%%% Created 2020-04-13 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-13 -% -% -% Update next grasping position for crawl gait with fixed stride, based on the map -% -% Function variables: -% -% OUTPUT -% path_planning_param : Parameters for path planning (class) -% -% path_planning_param.POS_cur : Current position of the swing leg when selecting a new one [m] (3x1 vector) -% path_planning_param.POS_next : Next desired position of the swing leg [m] (3x1 vector) -% path_planning_param.leg_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) -% INPUT -% path_planning_param : Parameters for path planning (class) -% gait_param : Parameters for gait (class) -% des_SV : Desired state values (SpaceDyn class) -% LP : Link parameters (SpaceDyn class) -% POS_e : Position of the end-effector [m] (3xnum_limb matrix) -% time : Simulation time [s] (scalar) - -function path_planning_param = upd_swing_next_pos_crawl_fixed_stride(path_planning_param, gait_param, des_SV, LP, POS_e, time) - - %%% Select next grasping point - % If all legs are in support phase - if sum(des_SV.sup) == LP.num_limb - % Current swing leg - i = path_planning_param.swing_number; - - % Current position of swing leg on the surface - [path_planning_param.POS_cur(1,i),path_planning_param.POS_cur(2,i),... - path_planning_param.POS_cur(3,i)] = get_map_pos(POS_e(1,i),POS_e(2,i)); - % Next desired position - path_planning_param.POS_next(:,i) = path_planning_param.POS_cur(:,i) + [gait_param.step_length; 0; 0]; - - % Next desired position on the surface - [path_planning_param.POS_next(1,i),path_planning_param.POS_next(2,i),path_planning_param.POS_next(3,i)] = ... - get_map_pos(path_planning_param.POS_next(1,i),path_planning_param.POS_next(2,i)); - - % Record the footholds of leg i - if time == 0 - for j=1:LP.num_limb - path_planning_param.footholds_history_limb(:,path_planning_param.footholds_count_limb(j),j) = POS_e(:,j); - path_planning_param.footholds_count_limb(j) = path_planning_param.footholds_count_limb(j) + 1; - end - end - path_planning_param.footholds_history_limb(:,path_planning_param.footholds_count_limb(i),i) = path_planning_param.POS_next(:,i); - path_planning_param.footholds_count_limb(i) = path_planning_param.footholds_count_limb(i) + 1; - - % Time for current and desired positions - path_planning_param.leg_T = [time; time + gait_param.T_d]; - end - - -end \ No newline at end of file diff --git a/src/controller/path/upd_swing_next_pos_crawl_uno_ver.m b/src/controller/path/upd_swing_next_pos_crawl_uno_ver.m deleted file mode 100644 index 4ba98cb..0000000 --- a/src/controller/path/upd_swing_next_pos_crawl_uno_ver.m +++ /dev/null @@ -1,239 +0,0 @@ -%%%%%% Update -%%%%%% upd_swing_next_pos_crawl_uno_ver -%%%%%% -%%%%%% Update next grasping position for crawl gait -%%%%%% -%%%%%% Created 2020-06-12 -%%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-09-11 by Kentaro Uno -% -% -% Update next grasping position for crawl gait using Uno-san's method -% -% Function variables: -% -% OUTPUT -% path_planning_param : Parameters for path planning (class) -% -% path_planning_param.POS_cur : Current position of the swing leg when selecting a new one [m] (3x1 vector) -% path_planning_param.POS_next : Next desired position of the swing leg [m] (3x1 vector) -% path_planning_param.base_next : Next desired position of the base [m] (3x1 vector) -% path_planning_param.leg_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) -% path_planning_param.base_T : Initial and final time for the movement between current and desired position [s] (2x1 vector) -% INPUT -% path_planning_param : Parameters for path planning (class) -% gait_param : Parameters for gait (class) -% surface_param : Parameters for surface (class) -% SV : State values (SpaceDyn class) -% des_SV : Desired state values (SpaceDyn class) -% LP : Link parameters (SpaceDyn class) -% POS_e : Position of the end-effector [m] (3xnum_limb matrix) -% inc : Surface inclination [deg] (scalar) -% base_height : Height of base relative to map [m] -% time : Simulation time [s] (scalar) - -function path_planning_param = upd_swing_next_pos_crawl_uno_ver(path_planning_param, gait_param, surface_param, SV, des_SV, LP, POS_e, inc, base_height, time) -if sum(des_SV.sup) == LP.num_limb && ~isempty(path_planning_param.graspable_points_in_reachable_area) - i = path_planning_param.swing_number; - % Current position of swing leg on the surface - [path_planning_param.POS_cur(1,i),path_planning_param.POS_cur(2,i),... - path_planning_param.POS_cur(3,i)] = get_map_pos(POS_e(1,i),POS_e(2,i)); - - % Current position of the base - path_planning_param.base_cur = SV.R0; - % Graspable points in Reachable area - GP_in_RA = []; - GP_in_RA = path_planning_param.graspable_points_in_reachable_area(:,:,i); - % Projection point (current position in a map) - [~,~,c] = get_map_pos(SV.R0(1),SV.R0(2)); - % Base_proj = [a;b;c]; - Base_proj = [SV.R0(1:2,1);c]; - - % Vector from CoM projecton point to goal - vec_sg = gait_param.goal - Base_proj; - % vec_sg = vec_sg/norm(vec_sg); - - dot_tmp = zeros(1,size(GP_in_RA,2)); - path_planning_param.POS_next = POS_e; - for ind = 1:size(GP_in_RA,2) - vec_tmp = GP_in_RA(1:2,ind) - POS_e(1:2,i); - if norm(vec_tmp) <= 0.2 - dot_tmp(1,ind) = dot(vec_sg(1:2,1), vec_tmp); - else - dot_tmp(1,ind) = 0; - end - end - - RequirementCheck_flag = false; - path_planning_param.POS_last_selected(:, i) = path_planning_param.POS_next(:, i); - while ~RequirementCheck_flag - [~,I] = max(dot_tmp); - % disp([GP_in_RA;dot_tmp]); - path_planning_param.POS_next(:, i) = GP_in_RA(:,I); - path_planning_param.base_next = Base_Pos(SV,LP,path_planning_param,POS_e,inc,surface_param,base_height); - - % plot3(GP_in_RA(1,:),GP_in_RA(2,:),GP_in_RA(3,:),'.','MarkerSize',20); - % disp('Next gripping point is :') - % disp([GP_in_RA(:,I);dot_tmp(:,I)]); - - RequirementCheck_flag = true; - % RequirementCheck_flag = Requirement_Check(RequirementCheck_flag,LP,path_planning_param); - RequirementCheck_flag = Requirement_Check(SV,LP,POS_e,path_planning_param); - % Also, if the next desired landing point is same as last time, it should - % be removed from the options - if norm(path_planning_param.POS_next(:, i)-POS_e(:, i)) <= 0.01 - RequirementCheck_flag = false - end - - if ~RequirementCheck_flag - GP_in_RA(:, I) = []; - dot_tmp(I) = []; - end - - if isempty(GP_in_RA) - % disp('best next gripping point does not exist!!!'); - path_planning_param.POS_next(:,i) = POS_e(:,i); - path_planning_param.base_next = SV.R0; - break; - end - end - % Time for current and desired positions - path_planning_param.leg_T = [time; time + gait_param.T_d]; - % Timing - path_planning_param.base_T = [time; time + gait_param.T_d]; - - - % Record the footholds of leg i - if time == 0 - for j=1:LP.num_limb - path_planning_param.footholds_history_limb(:,path_planning_param.footholds_count_limb(j),j) = POS_e(:,j); - path_planning_param.footholds_count_limb(j) = path_planning_param.footholds_count_limb(j) + 1; - end - end - path_planning_param.footholds_history_limb(:,path_planning_param.footholds_count_limb(i),i) = path_planning_param.POS_next(:,i); - path_planning_param.footholds_count_limb(i) = path_planning_param.footholds_count_limb(i) + 1; - -end -end - -function [ Base_Position ] = Base_Pos(SV,LP,path_planning_param,POS_e,inc,surface_param,base_height) -Base_Position = SV.R0; - -L1_x = zeros(2,1); -L1_y = zeros(2,1); -L2_x = zeros(2,1); -L2_y = zeros(2,1); - -for j = 1:LP.num_limb - if j == path_planning_param.swing_number - switch j - case 1 - L1_x(1) = path_planning_param.POS_next(1,j); - L1_y(1) = path_planning_param.POS_next(2,j); - case 2 - L2_x(1) = path_planning_param.POS_next(1,j); - L2_y(1) = path_planning_param.POS_next(2,j); - case 3 - L1_x(2) = path_planning_param.POS_next(1,j); - L1_y(2) = path_planning_param.POS_next(2,j); - case 4 - L2_x(2) = path_planning_param.POS_next(1,j); - L2_y(2) = path_planning_param.POS_next(2,j); - end - else - switch j - case 1 - L1_x(1) = POS_e(1,j); - L1_y(1) = POS_e(2,j); - case 2 - L2_x(1) = POS_e(1,j); - L2_y(1) = POS_e(2,j); - case 3 - L1_x(2) = POS_e(1,j); - L1_y(2) = POS_e(2,j); - case 4 - L2_x(2) = POS_e(1,j); - L2_y(2) = POS_e(2,j); - end - end -end - -% Calculate distances -Dx12 = L1_x(1)-L1_x(2); -Dx34 = L2_x(1)-L2_x(2); -Dy12 = L1_y(1)-L1_y(2); -Dy34 = L2_y(1)-L2_y(2); -Dx24 = L1_x(2)-L2_x(2); -Dy24 = L1_y(2)-L2_y(2); -% Solve the equation -ts = [Dx12 -Dx34; Dy12 -Dy34] \ [-Dx24; -Dy24]; -% Take weighted combinations of points on the line -P = ts(1)*[L1_x(1); L1_y(1)] + (1-ts(1))*[L1_x(2); L1_y(2)]; - -% figure();hold on;grid on; -% plot(L1_x,L1_y); -% plot(L2_x,L2_y); -% plot(P(1),P(2),'ko'); -% plot(POS_e(1,:),POS_e(2,:),'bo'); -Base_Position(1,1) = P(1) + norm(surface_param.floor_level)*tan(inc*pi/180); -Base_Position(2,1) = P(2); -Base_Position(3,1) = base_height + surface_param.floor_level; -end - -function flag = Requirement_Check(SV,LP,POS_e,path_planning_param) -RequirementCheck_CNT = 0; -% Requirement Check of Supporting Leg flag -for k = 1:LP.num_limb - if path_planning_param.swing_number == k - pos_next = path_planning_param.POS_next(:,k); - else - pos_next = POS_e(:,k); - end - pos_next(3,1) = pos_next(3,1) - SV.R0(3); - diff1 = abs( LP.reachable_area.data_far(3,:) - pos_next(3,1) ); - diff2 = abs( LP.reachable_area.data_near(3,:) - pos_next(3,1) ); - [~,I_1] = min(diff1(:)); - [~,I_2] = min(diff2(:)); - % x1 = LP.reachable_area.data_far(2,I_1(1));% x2 = LP.reachable_area.data_near(2,I_2(1));% MaxRange = x1;% MinRange = x2;% MinRange_from_Joint = MinRange - LP.c0(1,1);% MaxRange_from_Joint = MaxRange - LP.c0(1,1); - MinRange_from_Joint = LP.reachable_area.data_near(2,I_2(1)) - LP.c0(1,1); - MaxRange_from_Joint = LP.reachable_area.data_far(2,I_1(1)) - LP.c0(1,1); - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - p_check = pos_next + ( - ( path_planning_param.base_next - SV.R0 ) ); - p_from_CoM = p_check(:) - SV.R0; - % p_from_CoM = POS_e(:,k) - path_planning_param.base_next; - p_from_Joint = p_from_CoM - LP.c0(:, 3*k-2); - d = norm( p_from_Joint(1:2,1)); - % <<< Step 1 >>> - if d < MaxRange_from_Joint && d > MinRange_from_Joint - % <<< Step 2 >>> - theta = atan2(p_from_Joint(2), p_from_Joint(1)); - switch k %leg-number - case 1 - case 2 - if theta <= 0 - theta = theta + 2*pi; - end - case 3 - if theta <= 0 - theta = theta + 2*pi; - end - case 4 - theta = theta + 2*pi; - end - if theta > LP.Qi(3, (3*k-2)) - pi/3 && theta < LP.Qi(3, (3*k-2)) + pi/3 - RequirementCheck_CNT = RequirementCheck_CNT + 1; - else -% flag = 0; - break; - end - else -% flag = 0; - break; - end -end -if RequirementCheck_CNT == 4 - flag = true; -else - flag = false; -end -end \ No newline at end of file diff --git a/src/controller/path/upd_swing_num_and_grasping_point_based_on_graspable_options.m b/src/controller/path/upd_swing_num_and_grasping_point_based_on_graspable_options.m deleted file mode 100644 index 6d59b59..0000000 --- a/src/controller/path/upd_swing_num_and_grasping_point_based_on_graspable_options.m +++ /dev/null @@ -1,294 +0,0 @@ -%%%%%% Update -%%%%%% upd_swing_num_and_grasping_point_based_on_graspable_options -%%%%%% -%%%%%% Update number of swing leg based on how many graspable points the -%%%%%% swing limb has in its reachable area in forward -%%%%%% -%%%%%% Created 2020-07-02 -%%%%%% Kentaro Uno -%%%%%% Last update: 2020-09-11 -% -% -% -% Function variables: -% -% OUTPUT -% path_planning_param : Parameters for path planning (class) -% -% INPUT -% path_planning_param : Parameters for path planning (class) -% gait_param : Parameters for gait (class) -% surface_param : Parameters for surface (class) -% des_SV : Desired state values (SpaceDyn class) -% LP : Link parameters (SpaceDyn class) -% POS_e : Position of the end-effector [m] (3xnum_limb matrix) -% inc : Surface inclination [deg] (scalar) -% base_height : Height of base relative to map [m] -% time : Simulation time [s] (scalar) - -function path_planning_param = upd_swing_num_and_grasping_point_based_on_graspable_options(path_planning_param, gait_param, surface_param, des_SV, SV, LP, POS_e, inc, base_height, time) -if sum(des_SV.sup) == LP.num_limb - % Graspable points in Reachable area - GP_in_RA = []; - % Graspable Points in Reachable Area in front - GP_in_RA_in_front = []; - - % integrated matrix that contains [ [GP_in_RA_in_front]; limbCode; score ], where score is currently used dot product - GP_in_RA_in_front_with_limbCode_and_score = []; - dot_tmp = []; - count = 1; - - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - %%% Preparation of matrix that contains GP in RA, limb code, and score - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - for i = 1 : 1 : LP.num_limb - % get the number of the graspable points which is located in front from the current position of the limb in its reachable area - - % Current position of swing leg on the surface - [ path_planning_param.POS_cur(1,i), ... - path_planning_param.POS_cur(2,i), ... - path_planning_param.POS_cur(3,i) ] = get_map_pos(POS_e(1,i),POS_e(2,i)); - - % Current position of the base - path_planning_param.base_cur = SV.R0; - - % Graspable points in Reachable area for limb i - GP_in_RA(:,:,i) = path_planning_param.graspable_points_in_reachable_area(:,:,i); - - - % Projection point (current position in a map) - [~,~,c] = get_map_pos(SV.R0(1),SV.R0(2)); - % Base_proj - Base_proj = [SV.R0(1:2,1);c]; - - % Vector from CoM projecton point to goal - vec_sg = gait_param.goal - Base_proj; - - for ind = 1:size(GP_in_RA(:,:,i),2) - vec_tmp = GP_in_RA(:,ind,i) - POS_e(:,i); - % too much long swing is excluded out from the possible next - % gripping points - if norm(vec_tmp) <= 0.2 - dot_tmp(1,ind,i) = dot(vec_sg(:,1), vec_tmp); - % if the following dot product is positive, the GP is located in - % front of this swing leg - if dot_tmp(1,ind,i) > 0 - %GP_in_RA_in_front(:,count,i) = GP_in_RA(:,ind,i); - GP_in_RA_in_front_with_limbCode_and_score(:,count) = [GP_in_RA(:,ind,i);i;dot_tmp(1,ind,i)]; - count = count + 1; - end - else - dot_tmp(1,ind,i) = 0; - end - end - end - - RequirementCheck_flag = false; - while ~RequirementCheck_flag - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - %%% Select new swing leg - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - % decide the limb that has most graspable points in reachable area in - % front as the rentative swing limb --> next gripping points selection - num_of_graspable_points_in_front = [0;0;0;0]; - for i = 1 : 1 : LP.num_limb - for j = 1 : size(GP_in_RA_in_front_with_limbCode_and_score(:,:),2) - if GP_in_RA_in_front_with_limbCode_and_score(4,j) == i - num_of_graspable_points_in_front(i) = num_of_graspable_points_in_front(i) + 1; - end - end - end - [~,I] = max(num_of_graspable_points_in_front); - path_planning_param.swing_number = I; - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - %%% Select new grasping point - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - swing_number = path_planning_param.swing_number; - score = []; - for j = 1 : size(GP_in_RA_in_front_with_limbCode_and_score(:,:),2) - if GP_in_RA_in_front_with_limbCode_and_score(4,j) == swing_number - score = horzcat(score, GP_in_RA_in_front_with_limbCode_and_score(5,j)); - end - end - [~,I] = max(score); - for i = 1 : 1 : swing_number - 1 - I = I + num_of_graspable_points_in_front(i); - end - - path_planning_param.POS_next(:, swing_number) ... - = GP_in_RA_in_front_with_limbCode_and_score(1:3,I); - - path_planning_param.base_next = Base_Pos(SV,LP,path_planning_param,POS_e,inc,surface_param,base_height); - - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - %%% Requirement Check if the planned motion is kinematically feasible or not - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - RequirementCheck_flag = true; - RequirementCheck_flag = Requirement_Check(SV,LP,POS_e,path_planning_param); - % Also, if the next desired landing point is same as last time, it should - % be removed from the options - if norm(path_planning_param.POS_next(:, swing_number)-POS_e(:, swing_number)) <= 0.01 - RequirementCheck_flag = false - end - - if ~RequirementCheck_flag - GP_in_RA_in_front_with_limbCode_and_score(:,I) = []; - end - - if isempty(GP_in_RA_in_front_with_limbCode_and_score) - disp('best next gripping point does not exist!!!'); - % if there is no graspable points that can pass the requirement - % check, the next position is to be the current position and - % escape from the while loop, that means, this gait cannot make - % the robot move forward. - path_planning_param.POS_next(:,swing_number) = path_planning_param.POS_cur(:,swing_number); - path_planning_param.base_next = path_planning_param.base_cur; - break; - end - end - - - % Time for current and desired positions - path_planning_param.leg_T = [time; time + gait_param.T_d]; - % Timing - path_planning_param.base_T = [time; time + gait_param.T_d]; - - - % Record the footholds of leg i - if time == 0 - for j=1:LP.num_limb - path_planning_param.footholds_history_limb(:,path_planning_param.footholds_count_limb(j),j) = POS_e(:,j); - path_planning_param.footholds_count_limb(j) = path_planning_param.footholds_count_limb(j) + 1; - end - end - path_planning_param.footholds_history_limb(:,path_planning_param.footholds_count_limb(swing_number),swing_number) = path_planning_param.POS_next(:,swing_number); - path_planning_param.footholds_count_limb(i) = path_planning_param.footholds_count_limb(swing_number) + 1; - - - -end -end - -function [ Base_Position ] = Base_Pos(SV,LP,path_planning_param,POS_e,inc,surface_param,base_height) -Base_Position = SV.R0; - -L1_x = zeros(2,1); -L1_y = zeros(2,1); -L2_x = zeros(2,1); -L2_y = zeros(2,1); - -for j = 1:LP.num_limb - if j == path_planning_param.swing_number - switch j - case 1 - L1_x(1) = path_planning_param.POS_next(1,j); - L1_y(1) = path_planning_param.POS_next(2,j); - case 2 - L2_x(1) = path_planning_param.POS_next(1,j); - L2_y(1) = path_planning_param.POS_next(2,j); - case 3 - L1_x(2) = path_planning_param.POS_next(1,j); - L1_y(2) = path_planning_param.POS_next(2,j); - case 4 - L2_x(2) = path_planning_param.POS_next(1,j); - L2_y(2) = path_planning_param.POS_next(2,j); - end - else - switch j - case 1 - L1_x(1) = POS_e(1,j); - L1_y(1) = POS_e(2,j); - case 2 - L2_x(1) = POS_e(1,j); - L2_y(1) = POS_e(2,j); - case 3 - L1_x(2) = POS_e(1,j); - L1_y(2) = POS_e(2,j); - case 4 - L2_x(2) = POS_e(1,j); - L2_y(2) = POS_e(2,j); - end - end -end - -% Calculate distances -Dx12 = L1_x(1)-L1_x(2); -Dx34 = L2_x(1)-L2_x(2); -Dy12 = L1_y(1)-L1_y(2); -Dy34 = L2_y(1)-L2_y(2); -Dx24 = L1_x(2)-L2_x(2); -Dy24 = L1_y(2)-L2_y(2); -% Solve the equation -ts = [Dx12 -Dx34; Dy12 -Dy34] \ [-Dx24; -Dy24]; -% Take weighted combinations of points on the line -P = ts(1)*[L1_x(1); L1_y(1)] + (1-ts(1))*[L1_x(2); L1_y(2)]; - -% figure();hold on;grid on; -% plot(L1_x,L1_y); -% plot(L2_x,L2_y); -% plot(P(1),P(2),'ko'); -% plot(POS_e(1,:),POS_e(2,:),'bo'); -Base_Position(1,1) = P(1) + norm(surface_param.floor_level)*tan(inc*pi/180); -Base_Position(2,1) = P(2); -Base_Position(3,1) = base_height + surface_param.floor_level; -end - -function flag = Requirement_Check(SV,LP,POS_e,path_planning_param) -RequirementCheck_CNT = 0; -% Requirement Check of Supporting Leg flag -for k = 1:LP.num_limb - if path_planning_param.swing_number == k - pos_next = path_planning_param.POS_next(:,k); - else - pos_next = POS_e(:,k); - end - pos_next(3,1) = pos_next(3,1) - SV.R0(3); - diff1 = abs( LP.reachable_area.data_far(3,:) - pos_next(3,1) ); - diff2 = abs( LP.reachable_area.data_near(3,:) - pos_next(3,1) ); - [~,I_1] = min(diff1(:)); - [~,I_2] = min(diff2(:)); - % x1 = LP.reachable_area.data_far(2,I_1(1));% x2 = LP.reachable_area.data_near(2,I_2(1));% MaxRange = x1;% MinRange = x2;% MinRange_from_Joint = MinRange - LP.c0(1,1);% MaxRange_from_Joint = MaxRange - LP.c0(1,1); - MinRange_from_Joint = LP.reachable_area.data_near(2,I_2(1)) - LP.c0(1,1)*sqrt(2); - MaxRange_from_Joint = LP.reachable_area.data_far(2,I_1(1)) - LP.c0(1,1)*sqrt(2); - %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - p_check = pos_next + ( - ( path_planning_param.base_next - SV.R0 ) ); - p_from_CoM = p_check(:) - SV.R0; - % p_from_CoM = POS_e(:,k) - path_planning_param.base_next; - p_from_Joint = p_from_CoM - LP.c0(:, 3*k-2); - d = norm( p_from_Joint(1:2,1)); - % <<< Step 1 >>> - if d < MaxRange_from_Joint && d > MinRange_from_Joint - % <<< Step 2 >>> - theta = atan2(p_from_Joint(2), p_from_Joint(1)); - switch k %leg-number - case 1 - case 2 - if theta <= 0 - theta = theta + 2*pi; - end - case 3 - if theta <= 0 - theta = theta + 2*pi; - end - case 4 - theta = theta + 2*pi; - end - if theta > LP.Qi(3, (3*k-2)) - pi/3 && theta < LP.Qi(3, (3*k-2)) + pi/3 - RequirementCheck_CNT = RequirementCheck_CNT + 1; - else -% flag = 0; - break; - end - else -% flag = 0; - break; - end -end -if RequirementCheck_CNT == 4 - flag = true; -else - flag = false; -end -end \ No newline at end of file diff --git a/src/controller/path/upd_swing_number_crawl_fixed_stride.m b/src/controller/path/upd_swing_number_crawl_fixed_stride.m deleted file mode 100644 index 89e11ba..0000000 --- a/src/controller/path/upd_swing_number_crawl_fixed_stride.m +++ /dev/null @@ -1,47 +0,0 @@ -%%%%%% Update -%%%%%% upd_swing_number_crawl_fixed_stride -%%%%%% -%%%%%% Update number of swing leg for crawl gait -%%%%%% -%%%%%% Created 2020-04-13 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-13 -% -% -% Update number of swing leg for crawl gait with fixed stride -% -% Function variables: -% -% OUTPUT -% path_planning_param : Parameters for path planning (class) -% -% path_planning_param.swing_number : Number of the selected leg to be the next swing leg (scalar) -% INPUT -% path_planning_param : Parameters for path planning (class) -% gait_param : Parameters for gait (class) -% des_SV : Desired state values (SpaceDyn class) -% LP : Link parameters (SpaceDyn class) -% time : Simulation time [s] (scalar) - -function path_planning_param = upd_swing_number_crawl_fixed_stride(path_planning_param, gait_param, des_SV, LP, time) - - %%% Select new swing leg - % If all legs are in support phase - if sum(des_SV.sup) == LP.num_limb - % First step - if time == 0 - path_planning_param.swing_number = gait_param.sequence(1); - else - sw_num = path_planning_param.swing_number(1); - seq = gait_param.sequence; - % Find previous swing leg - previous_index = find(seq==sw_num); - if previous_index == size(gait_param.sequence,1) - path_planning_param.swing_number = gait_param.sequence(1); - else - path_planning_param.swing_number = gait_param.sequence(previous_index+1); - end - end - end - -end \ No newline at end of file diff --git a/src/controller/tools/get_i_kine_3dof.m b/src/controller/tools/get_i_kine_insect_config_3dof_limb.m similarity index 83% rename from src/controller/tools/get_i_kine_3dof.m rename to src/controller/tools/get_i_kine_insect_config_3dof_limb.m index ed2108a..c540d69 100644 --- a/src/controller/tools/get_i_kine_3dof.m +++ b/src/controller/tools/get_i_kine_insect_config_3dof_limb.m @@ -1,11 +1,13 @@ %%%%%% Calculate -%%%%%% get_pos_i_kine_3dof +%%%%%% get_i_kine_insect_config_3dof_limb %%%%%% -%%%%%% - Postion Inverse kinematics solver for a 3 DOF manipulator +%%%%%% - Position inverse kinematics solver for a 3 DOF manipulator that +%%%%%% has a insect joint configuration %%%%%% -%%%%%% Created 2020-06-25 +%%%%%% Created: 2020-06-25 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-01-19 %%%%%% Kentaro Uno -%%%%%% Last update: 2020-09-22 % % % Calculate joints angular position of a 3 degrees of freedom manipulator from the end-effector position @@ -21,7 +23,7 @@ % num_e : Number of the end-effector (scalar) -function q_sol = get_i_kine_3dof( LP, SV, POS_e, num_e ) +function q_sol = get_i_kine_insect_config_3dof_limb( LP, SV, POS_e, num_e ) q_sol = zeros(3,1); p0e = zeros(3,1); % Base (link 0) to endeffector (link "e") position vector, i.e. input of IK @@ -40,9 +42,9 @@ % p0e should be written seen from the base frame p0e = POS_e - SV.R0; -p0e = (rot_x(SV.Q0(1,1))*rot_y(SV.Q0(2,1))*rot_z(SV.Q0(3,1))*p0e); +p0e = SV.A0'*p0e; POS_eE = p0e; -% p0e = [0.15;0.10;-0.13]; + % load link parameter: LP file and substituting into the local constant p01(:,1) = LP.c0(:, joints(1)); p12(:,1) = LP.cc(:, joints(1) , joints(1)+1) - LP.cc(:,joints(1) , joints(1) ); @@ -61,6 +63,11 @@ b1 = p0e(2,1) - p01(2,1); c1 = p12(2,1) + p23(2,1) + p3e(2,1); +%Singurarity check +if isreal(sqrt(a1^2+b1^2-c1^2)) == false %&& imag(sqrt(a1^2+b1^2-c1^2)) > 3*10e-3 + return +end + q1 = atan2( a1 , b1 ) + atan2( sqrt(a1^2+b1^2-c1^2), c1 ) - alpha; %%% joint Coxa to Femur solver @@ -71,6 +78,11 @@ b2 = 2*(A*p23(1,1)+B*p23(3,1)); c2 = A^2 + B^2 + p23(1,1)^2 + p23(3,1)^2 - p3e(1,1)^2 - p3e(3,1)^2; +%Singurarity check +if isreal(sqrt(a2^2+b2^2-c2^2)) == false %&& imag(sqrt(a2^2+b2^2-c2^2)) > 3*10e-3 + return +end + q2 = - ( atan2( a2 , b2 ) + atan2( sqrt(a2^2+b2^2-c2^2), c2 ) ); %%% joint Femur to Tibia solver @@ -80,7 +92,7 @@ q_sol(2,1) = - q2; q_sol(3,1) = - ( q3 + pi()/2 ); -%%% adjust the solutions to be described radians from -2*pi to 2*pi() +%%% adjust the solutions to be described radians from -pi to pi() if q_sol(1,1) > pi() q_sol(1,1) = q_sol(1,1) - 2*pi(); elseif q_sol(1,1) < -pi() diff --git a/src/controller/tools/get_i_kine_mammal_config_3dof_limb.m b/src/controller/tools/get_i_kine_mammal_config_3dof_limb.m new file mode 100644 index 0000000..8c07c72 --- /dev/null +++ b/src/controller/tools/get_i_kine_mammal_config_3dof_limb.m @@ -0,0 +1,157 @@ +%%%%%% Calculate +%%%%%% get_i_kine_mammal_config_3dof_limb.m +%%%%%% +%%%%%% - Position inverse kinematics solver for a 3 DOF manipulator that +%%%%%% has a mammalian configuration +%%%%%% +%%%%%% Created: 2021-01-18 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-02-12 +%%%%%% Kentaro Uno +% +% +% Calculate joints angular position of a 3 degrees of freedom manipulator from the end-effector position +% +% Function variables: +% +% OUTPUT +% q_sol : Initial angular position for joints (1xn vector) +% INPUT +% LP : Link Parameters (SpaceDyn class) +% SV : State Variables (SpaceDyn class) +% POS_e : Position of the end-effector [m] (3x1 vector) +% num_e : Number of the end-effector (scalar) + + +function q_sol = get_i_kine_mammal_config_3dof_limb( LP, SV, POS_e, num_e ) + +%% variables declearation +q_sol = zeros(3,1); +p0e = zeros(3,1); % Base (link 0) to endeffector (link "e") position vector, i.e. input of IK + +%% link parameters substitiono into the local variables to solve the IK +% for reader of this code, note that the following frame orientation is +% different from the rule of spacedyn. basically no orientation between two +% of Hip, Thigh, and Shank link. +p01 = zeros(3,1); % Base (link 0) to Hip (link 1) position vector +p12 = zeros(3,1); % Hip (link 1) to Thigh (link 2) position vector +p23 = zeros(3,1); % Thigh (link 2) to Shank (link 3) position vector +p3e = zeros(3,1); % Shank (link 3) to endeffector (link "e") position vector + +joints = j_num( LP, num_e ); + +% p0e should be written seen from the base frame +p0e = POS_e - SV.R0; +p0e = SV.A0'*p0e; +POS_eE = p0e; + +% load link parameter: LP file and substituting into the local constant +p01(:,1) = LP.c0(:, joints(1)); +p12_tmp(:,1) = LP.cc(:, joints(1) , joints(1)+1) - LP.cc(:,joints(1) , joints(1) ); +p23_tmp(:,1) = LP.cc(:, joints(1)+1, joints(1)+2) - LP.cc(:,joints(1)+1, joints(1)+1); +p3e_tmp(:,1) = LP.ce(:, joints(1)+2) - LP.cc(:,joints(1)+2, joints(1)+2); + +% adjusting the frame for IK from frame of SpaceDyn +p12(1,1) = p12_tmp(3,1); p12(2,1) = p12_tmp(2,1); p12(3,1) = -p12_tmp(1,1); +p23(1,1) = p23_tmp(2,1); p23(2,1) = -p23_tmp(3,1); p23(3,1) = -p23_tmp(1,1); +p3e(1,1) = p3e_tmp(2,1); p3e(2,1) = -p3e_tmp(3,1); p3e(3,1) = -p3e_tmp(1,1); + +% extract the offset angle of the hip joint +if num_e == 1 || num_e == 3 + theta_1 = LP.theta_1; +else + theta_1 = - LP.theta_1; +end +if num_e == 1 || num_e == 4 + theta_2 = LP.theta_2; +else + theta_2 = - LP.theta_2; +end + +%% joint base to Hip solver +% a1, b1, c1: temporary variable to calculate q1 +a1 = cos(theta_1) * sin(theta_2) * (p0e(1,1)-p01(1,1))... + + sin(theta_1) * sin(theta_2) * (p0e(2,1)-p01(2,1))... + + cos(theta_2) * (p0e(3,1)-p01(3,1)); +b1 = - sin(theta_1)*(p0e(1,1)-p01(1,1))... + + cos(theta_1)*(p0e(2,1)-p01(2,1)); +c1 = p12(2,1) + p23(2,1) + p3e(2,1); + +q1 = atan2( a1 , b1 ) + atan2( sqrt(a1^2+b1^2-c1^2), c1 ); + +%% joint Hip to Thigh solver +% A, B, a2, b2, c2: temporary variable to calculate q2 +A = cos(theta_1) * cos(theta_2) * ( p0e(1,1) - p01(1,1) )... + + sin(theta_1) * cos(theta_2) * ( p0e(2,1) - p01(2,1) )... + - sin(theta_2) * ( p0e(3,1) - p01(3,1) )... + - p12(1,1); +B = ( sin(theta_1)*sin(q1) + cos(theta_1)*sin(theta_2)*cos(q1) ) * ( p0e(1,1) - p01(1,1) )... + + ( -cos(theta_1)*sin(q1) + sin(theta_1)*sin(theta_2)*cos(q1) ) * ( p0e(2,1) - p01(2,1) )... + + cos(theta_2) * cos(q1) * ( p0e(3,1) - p01(3,1) )... + - p12(3,1); +a2 = 2*(A*p23(3,1)-B*p23(1,1)); +b2 = 2*(A*p23(1,1)+B*p23(3,1)); +c2 = A^2 + B^2 + p23(1,1)^2 + p23(3,1)^2 - p3e(1,1)^2 - p3e(3,1)^2; + +% If we want xx config solution, the sign of the hip to thigh joint solution +% should be flipped between front legs and hind legs. + +switch LP.leg_config_type + case 'oo' + if num_e == 1 || num_e == 4 + q2 = atan2( a2 , b2 ) - atan2( sqrt(a2^2+b2^2-c2^2), c2 ); % oo config + elseif num_e == 2 || num_e == 3 + q2 = atan2( a2 , b2 ) + atan2( sqrt(a2^2+b2^2-c2^2), c2 ); % oo config + end + case 'xx' + if num_e == 1 || num_e == 4 + q2 = atan2( a2 , b2 ) + atan2( sqrt(a2^2+b2^2-c2^2), c2 ); % xx config + elseif num_e == 2 || num_e == 3 + q2 = atan2( a2 , b2 ) - atan2( sqrt(a2^2+b2^2-c2^2), c2 ); % xx config + end + case 'M' + if num_e == 1 || num_e == 4 + q2 = atan2( a2 , b2 ) + atan2( sqrt(a2^2+b2^2-c2^2), c2 ); + elseif num_e == 2 || num_e == 3 + q2 = atan2( a2 , b2 ) + atan2( sqrt(a2^2+b2^2-c2^2), c2 ); + end +end + +% % If theta2 is negative, it is forced to be "oo" config +% if LP.theta_2 < 0 +% if num_e == 1 || num_e == 4 +% q2 = atan2( a2 , b2 ) - atan2( sqrt(a2^2+b2^2-c2^2), c2 ); +% elseif num_e == 2 || num_e == 3 +% q2 = atan2( a2 , b2 ) + atan2( sqrt(a2^2+b2^2-c2^2), c2 ); +% end +% end + +%% joint Thigh to Shank solver +q3 = 2*pi() + atan2( A*cos(q2)-B*sin(q2)-p23(1,1) , A*sin(q2)+B*cos(q2)-p23(3,1) ) - atan2(p3e(1,1), p3e(3,1)); + +%% fit back the range of the joint into the spacedyn solver +q_sol(1,1) = q1; +q_sol(2,1) = - q2; +q_sol(3,1) = - q3; + +%% adjust the solutions to be described radians from -pi to pi() +if q_sol(1,1) > pi() + q_sol(1,1) = q_sol(1,1) - 2*pi(); +elseif q_sol(1,1) < -pi() + q_sol(1,1) = q_sol(1,1) + 2*pi(); +end + +if q_sol(2,1) > pi() + q_sol(2,1) = q_sol(2,1) - 2*pi(); +elseif q_sol(2,1) < -pi() + q_sol(2,1) = q_sol(2,1) + 2*pi(); +end + +if q_sol(3,1) > pi() + q_sol(3,1) = q_sol(3,1) - 2*pi(); +elseif q_sol(3,1) < -pi() + q_sol(3,1) = q_sol(3,1) + 2*pi(); +end + +end + diff --git a/src/controller/tools/get_map_pos_of_graspable_points.m b/src/controller/tools/get_map_pos_of_graspable_points.m new file mode 100644 index 0000000..5e0c45b --- /dev/null +++ b/src/controller/tools/get_map_pos_of_graspable_points.m @@ -0,0 +1,42 @@ +%%%%%% Calculate +%%%%%% get_map_pos_of_graspable_points +%%%%%% +%%%%%% Obtain the correspondent graspable points' positions (including height) for a x-y set +%%%%%% +%%%%%% Created 2021-03-09 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-03-09 +%&%%%% Keigo Haji +% +% +% For a given set of points, obtain the closest graspable points in the map +% +% Function variables: +% +% OUTPUT +% xm : x-coordinate position for the map (1xn, where n is the number of points to be checked) +% ym : y-coordinate position for the map (1xn) +% zm : z-coordinate position for the map (1xn) +% INPUT +% xp : Given x-coordinate position of the point(s) to be checked (1xn) +% yp : Given y-coordinate position of the point(s) to be checked (1xn) +% surface_param : Parameters for surface (class) +% surface_param.graspable_points : 3xN matrix contains position vectors of graspable points [m] (x;y;z) + +function [xm,ym,zm] = get_map_pos_of_graspable_points(xp,yp,surface_param) + + +% number of points to be checked +n = length(xp); + + for i = 1:n + % get the graspable point of the map nearest point + GPs = surface_param.graspable_points; + [~,id_GP] = min((GPs(1,:) - xp(i)).^2 + (GPs(2,:) - yp(i) ).^2); + % get the height corresponding to that point + xm(i) = GPs(1,id_GP); + ym(i) = GPs(2,id_GP); + zm(i) = GPs(3,id_GP); + end + +end diff --git a/src/controller/upd_grasp_detach.m b/src/controller/upd_grasp_detach.m index f65960f..e7fbd34 100644 --- a/src/controller/upd_grasp_detach.m +++ b/src/controller/upd_grasp_detach.m @@ -4,8 +4,9 @@ %%%%%% Update gripping and detachment state %%%%%% %%%%%% Created 2020-04-17 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-10-15 by Kentaro Uno +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2021-04-26 +%%%%%% by Warley Ribeiro % % % Update the real state of the gripper, if it is open or closed, based on the desired state and detachment conditions @@ -19,7 +20,7 @@ % SV : State variables (SpaceDyn class) % des_SV : Desired state variables (SpaceDyn class) -function SV = upd_grasp_detach(LP, SV, des_SV, F_grip, environment_param, equilibrium_eval_param, variables_saved) +function SV = upd_grasp_detach(LP, SV, des_SV, environment_param, equilibrium_eval_param, variables_saved) % Update gripper SV.sup = des_SV.sup; @@ -28,9 +29,9 @@ switch environment_param.detachment_detection_method case 'max_holding_force' - + Force = SV.Fe(:,LP.SE == 1); for i = 1:LP.num_limb - if variables_saved.(genvarname(['Fe_res' num2str(i)]))(end) > F_grip && SV.Fe(3,3*i) < 0 && des_SV.sup(i) == 1 + if norm(Force(:,i)) > LP.F_grip && SV.Fe(3,3*i) < 0 && des_SV.sup(i) == 1 SV.slip(i) = 1; end end diff --git a/src/controller/upd_grasp_slip.m b/src/controller/upd_grasp_slip.m deleted file mode 100644 index 01e1354..0000000 --- a/src/controller/upd_grasp_slip.m +++ /dev/null @@ -1,33 +0,0 @@ -%%%%%% Update -%%%%%% upd_grasp_slip -%%%%%% -%%%%%% Update gripping and slippage state -%%%%%% -%%%%%% Created 2020-04-17 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 -% -% -% Update the real state of the gripper, if it is open or closed, based on the desired state and slippage conditions -% -% Function variables: -% -% OUTPUT -% SV : State variables (SpaceDyn class) -% INPUT -% LP : Link parameter (SpaceDyn class) -% SV : State variables (SpaceDyn class) -% des_SV : Desired state variables (SpaceDyn class) - -function SV = upd_grasp_slip(LP, SV, des_SV) - -% Update gripper -SV.sup = des_SV.sup; -% Check Slippage -for i = 1:LP.num_limb - if SV.slip(i) == 1 - SV.sup(i) = 0; - end -end - -end \ No newline at end of file diff --git a/src/engine/upd_fwd_dynamics.m b/src/engine/upd_fwd_dynamics.m index 60404df..e098e4b 100644 --- a/src/engine/upd_fwd_dynamics.m +++ b/src/engine/upd_fwd_dynamics.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-04-08 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-15 +%%%%%% Last update: 2020-12-28 % % Use SpaceDyn functions for forward dynamics to obtain joints and links accelerations, velocities and positions from the % given torques and external forces @@ -24,22 +24,38 @@ global d_time; -if strcmp(environment_param.dynamics_flag,'on') - % Dynamics on - % Solve equation of motion - SV = f_dyn_rk2(LP,SV); - SV = f_dyn(LP,SV); -else - % Dynamics off - des_SV.v0 = (des_SV.R0 - SV.R0); - des_SV.qd = (des_SV.q - SV.q)/d_time; - SV = des_SV; -end +switch environment_param.dynamics_flag + + case 'on' + % Dynamics on + % Solve equation of motion + SV = f_dyn_rk2(LP,SV); + SV = f_dyn(LP,SV); + + % Calculate links orientations, positions, velocities and accelerations + SV = calc_aa(LP,SV); + SV = calc_pos(LP,SV); + SV = calc_vel(LP,SV); + SV = calc_acc(LP,SV); + case 'off' + % Dynamics off + des_SV.v0 = (des_SV.R0 - SV.R0)/d_time; + des_SV.qd = (des_SV.q - SV.q)/d_time; + SV = des_SV; -% Calculate links orientations, positions, velocities and accelerations -SV = calc_aa(LP,SV); -SV = calc_pos(LP,SV); -SV = calc_vel(LP,SV); -SV = calc_acc(LP,SV); + % Calculate links orientations, positions, velocities and accelerations + SV = calc_aa(LP,SV); + SV = calc_pos(LP,SV); + SV = calc_vel(LP,SV); + SV = calc_acc(LP,SV); + case 'no_acc' + % Dynamics off and zero acceleration + SV = des_SV; + + % Calculate links orientations, positions, velocities and accelerations + SV = calc_aa(LP,SV); + SV = calc_pos(LP,SV); + SV = calc_vel(LP,SV); +end end \ No newline at end of file diff --git a/src/environment/climbing_holds_map/climbing_holds_map_on_full_testfield.mat b/src/environment/climbing_holds_map/climbing_holds_map_on_full_testfield.mat new file mode 100644 index 0000000..b47fd8b Binary files /dev/null and b/src/environment/climbing_holds_map/climbing_holds_map_on_full_testfield.mat differ diff --git a/src/environment/climbing_holds_map/climbing_holds_map_on_testfield.mat b/src/environment/climbing_holds_map/climbing_holds_map_on_testfield.mat new file mode 100644 index 0000000..5e76d9d Binary files /dev/null and b/src/environment/climbing_holds_map/climbing_holds_map_on_testfield.mat differ diff --git a/src/environment/climbing_holds_map/graspable_points_of_full_testfield.mat b/src/environment/climbing_holds_map/graspable_points_of_full_testfield.mat new file mode 100644 index 0000000..5126da6 Binary files /dev/null and b/src/environment/climbing_holds_map/graspable_points_of_full_testfield.mat differ diff --git a/src/environment/climbing_holds_map/graspable_points_of_testfield_without_board.mat b/src/environment/climbing_holds_map/graspable_points_of_testfield_without_board.mat new file mode 100644 index 0000000..ec7479e Binary files /dev/null and b/src/environment/climbing_holds_map/graspable_points_of_testfield_without_board.mat differ diff --git a/src/environment/climbing_holds_map/graspable_points_on_test_field.mat b/src/environment/climbing_holds_map/graspable_points_on_test_field.mat new file mode 100644 index 0000000..a4c3f38 Binary files /dev/null and b/src/environment/climbing_holds_map/graspable_points_on_test_field.mat differ diff --git a/src/environment/climbing_holds_map/raw_PCD_data/leaning_bouldering_holds.pcd b/src/environment/climbing_holds_map/raw_PCD_data/leaning_bouldering_holds.pcd new file mode 100644 index 0000000..9fe7674 Binary files /dev/null and b/src/environment/climbing_holds_map/raw_PCD_data/leaning_bouldering_holds.pcd differ diff --git a/src/environment/climbing_holds_map/raw_PCD_data/testfield_Apr8.pcd b/src/environment/climbing_holds_map/raw_PCD_data/testfield_Apr8.pcd new file mode 100644 index 0000000..1c7568a Binary files /dev/null and b/src/environment/climbing_holds_map/raw_PCD_data/testfield_Apr8.pcd differ diff --git a/src/environment/climbing_holds_map/raw_PCD_data/vertical_bouldering_holds.pcd b/src/environment/climbing_holds_map/raw_PCD_data/vertical_bouldering_holds.pcd new file mode 100644 index 0000000..008215f Binary files /dev/null and b/src/environment/climbing_holds_map/raw_PCD_data/vertical_bouldering_holds.pcd differ diff --git a/src/environment/grid_map/flat_delta_003.mat b/src/environment/grid_map/flat_delta_003.mat index 5f9b2bd..fd86630 100644 Binary files a/src/environment/grid_map/flat_delta_003.mat and b/src/environment/grid_map/flat_delta_003.mat differ diff --git a/src/environment/grid_map/flat_delta_006.mat b/src/environment/grid_map/flat_delta_006.mat index 584f1b8..27d38e2 100644 Binary files a/src/environment/grid_map/flat_delta_006.mat and b/src/environment/grid_map/flat_delta_006.mat differ diff --git a/src/environment/grid_map/flat_delta_008.mat b/src/environment/grid_map/flat_delta_008.mat index 014ae6c..85c50a4 100644 Binary files a/src/environment/grid_map/flat_delta_008.mat and b/src/environment/grid_map/flat_delta_008.mat differ diff --git a/src/environment/grid_map/flat_delta_009.mat b/src/environment/grid_map/flat_delta_009.mat index 182c14d..2c9f684 100644 Binary files a/src/environment/grid_map/flat_delta_009.mat and b/src/environment/grid_map/flat_delta_009.mat differ diff --git a/src/environment/grid_map/flat_delta_010.mat b/src/environment/grid_map/flat_delta_010.mat index ff15209..f31a098 100644 Binary files a/src/environment/grid_map/flat_delta_010.mat and b/src/environment/grid_map/flat_delta_010.mat differ diff --git a/src/environment/grid_map/flat_delta_012.mat b/src/environment/grid_map/flat_delta_012.mat index 1af7a2c..67cae49 100644 Binary files a/src/environment/grid_map/flat_delta_012.mat and b/src/environment/grid_map/flat_delta_012.mat differ diff --git a/src/environment/grid_map/flat_delta_015.mat b/src/environment/grid_map/flat_delta_015.mat index 63e020e..c63d7b7 100644 Binary files a/src/environment/grid_map/flat_delta_015.mat and b/src/environment/grid_map/flat_delta_015.mat differ diff --git a/src/environment/grid_map/flat_test_for_camera.mat b/src/environment/grid_map/flat_test_for_camera.mat new file mode 100644 index 0000000..08f2152 Binary files /dev/null and b/src/environment/grid_map/flat_test_for_camera.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/flat_3mx3m_25mm_thin50_90.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/flat_3mx3m_25mm_thin50_90.mat new file mode 100644 index 0000000..c100a50 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/flat_3mx3m_25mm_thin50_90.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/known_test_grid_map_3mx3m_dx85mm_2void_areas_with_15cm_margin.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/known_test_grid_map_3mx3m_dx85mm_2void_areas_with_15cm_margin.mat new file mode 100644 index 0000000..a3d743e Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/known_test_grid_map_3mx3m_dx85mm_2void_areas_with_15cm_margin.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_35mm_1m_center_thinned_50.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_35mm_1m_center_thinned_50.mat new file mode 100644 index 0000000..dd21579 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_35mm_1m_center_thinned_50.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx70mm_thined_50_1void_area_40cm_at_center.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx70mm_thined_50_1void_area_40cm_at_center.mat new file mode 100644 index 0000000..8d13dc4 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx70mm_thined_50_1void_area_40cm_at_center.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm.mat new file mode 100644 index 0000000..e930461 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_thined_area_30percent_60cm_center.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_thined_area_30percent_60cm_center.mat new file mode 100644 index 0000000..ca445ba Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_thined_area_30percent_60cm_center.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_thinned_area_at_corner_70percent.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_thinned_area_at_corner_70percent.mat new file mode 100644 index 0000000..0fa82cb Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_thinned_area_at_corner_70percent.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_10cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_10cm.mat new file mode 100644 index 0000000..104a757 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_10cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_25cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_25cm.mat new file mode 100644 index 0000000..eaa43df Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_25cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_40cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_40cm.mat new file mode 100644 index 0000000..4b77643 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_40cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_60cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_60cm.mat new file mode 100644 index 0000000..3f0e83a Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_1_void_area_at_center_60cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_2void_areas.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_2void_areas.mat new file mode 100644 index 0000000..dd712f8 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_2void_areas.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm.mat new file mode 100644 index 0000000..81eef96 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm_ver2.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm_ver2.mat new file mode 100644 index 0000000..cbbe4a4 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm_ver2.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_20_1void_area_at_center_40cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_20_1void_area_at_center_40cm.mat new file mode 100644 index 0000000..7467731 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_20_1void_area_at_center_40cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_20_2void_areas.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_20_2void_areas.mat new file mode 100644 index 0000000..125ccc2 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_20_2void_areas.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_30_1void_area_at_center_40cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_30_1void_area_at_center_40cm.mat new file mode 100644 index 0000000..1e3d162 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_30_1void_area_at_center_40cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_30_2void_areas.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_30_2void_areas.mat new file mode 100644 index 0000000..be358f1 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_30_2void_areas.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm.mat new file mode 100644 index 0000000..32b5723 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver2.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver2.mat new file mode 100644 index 0000000..df44192 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver2.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver3.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver3.mat new file mode 100644 index 0000000..600c393 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver3.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50.mat new file mode 100644 index 0000000..53a92fb Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm.mat new file mode 100644 index 0000000..b90ab47 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm.mat differ diff --git a/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm_ver2.mat b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm_ver2.mat new file mode 100644 index 0000000..89442f2 Binary files /dev/null and b/src/environment/grid_map/grid_map_for_case_study_of_local_path_plan/test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm_ver2.mat differ diff --git a/src/environment/grid_map/main_grid_map_designer.m b/src/environment/grid_map/main_grid_map_designer.m new file mode 100644 index 0000000..d03fb6d --- /dev/null +++ b/src/environment/grid_map/main_grid_map_designer.m @@ -0,0 +1,190 @@ +%%%%%% Grid Map Designer +%%%%%% main_grid_map_designer.m +%%%%%% +%%%%%% Main File +%%%%%% +%%%%%% Create grid maps with void and thinned areas. +%%%%%% +%%%%%% Created 2021-01-26 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-06-11 +%%%%%% Kentaro Uno (made a new 3m x 3m map for the example demo 3) +% +% Create grid maps with void and thinned areas. +% +% [Note] +% This file is NOT used in main_sim.m. +% This file is used only before running climb_main to create a proper +% environment file as .mat file. +% You can design a grid-style environment with any stride width, any size, +% any thinning percentage, and any void area. +% In the current version, both the grid map and the internal blank and +% thinned regions are created as squares. +% +% Variables +% field: A structure for creating a field. +% thin: A stucture for creating thinned areas. +% void: A structure for creating void areas. +% map: A structure for drawing points in the designed grid map. +% x: 1*n vector +% y: 1*n vector +% z: n*n matrix +% +% OUTPUT +% Executing this file will save the three variables of x-y-z as a single +% .mat file in this folder with the specified file name. +% +close all; +clear; + +%%%%%%%%%%%%%%%%%%%%%%%%%% Parameter Setting %%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Set the file name as you want to save. +filename = 'test_grid_map_3mx3m_dx85mm_thined_20_2void_areas.mat'; %'xxx.mat' + +%Set the stride width +field.dx = 85; %[mm] +%Set the field size +field.size = 3; %[m] + + +%Set the switch for thinning +thin.setting = 'full'; %'on', 'off', or 'full', when "full" is selected, the entire map is targeted. +%Set the number of thinning areas +thin.num = 1; +%Set the center positions of thinning areas as a matrix. +%Each row indicates each center and columns indicate x and y positions. +% thin.center = [field.size/6 field.size/6; +% field.size/2 field.size/6; +% field.size/6*5 field.size/6; +% field.size/6 field.size/2; +% field.size/2 field.size/2; +% field.size/6*5 field.size/2; +% field.size/6 field.size/6*5; +% field.size/2 field.size/6*5; +% field.size/6*5 field.size/6*5 +% ]; %[m m] +thin.center = [3*field.size/4 field.size/4]; +%Set the thinning percentage as a vector. +% This number is the percentage of points to be thinned out, not the +% percentage to be left behind; 0% does nothing, 100% is theoretically +% void. +% thin.percentage = [50 50 50 50 90 50 50 50 50]; % [%] 0 - 100 +thin.percentage = 20; % [%] 0 - 100 +%Set the length of one side of thinning area +thin.length = 1.5; %[m] + +%Set the switch for void area +void.setting = 'on'; %'on' or 'off' +%Set the number of void areas +void.num = 2; +%Set the center positons of void squares as a matrix +%Each row indicates each center and columns indicate x and y positions. +% void.center =[field.size/6 field.size/6; +% field.size/2 field.size/6]; %[m m] +void.center =[ 0.95 0.9; + 2.05 2.15]; %[m m] +%Set the length of one side of void area +% void.length = 1.0; %[m] +void.length_xy = [0.3 1.4; + 0.3 1.4]; %[m] + +%%%%%%%%%%%%%%%%%%%%%%% Create the grid map %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%Convert units +field.dX = field.dx / 1000; %[m] + +%Make Grid area +[field.X, field.Y] = meshgrid(-field.size/2 : field.dX : field.size/2); +%Convert formats +x = field.X(1,:); +y = field.Y(:,1)'; +%Insert z positons +z = zeros(length(x)); +z(:,:) = 0; %previously -0.08 + + +%%%%%%%%%%%%%%%%% Thinned points %%%%%%%%%%%%%%%%% +if strcmp(thin.setting,'full') || strcmp(thin.setting,'on') + if strcmp(thin.setting,'full') + thin.id_xmax = length(x); + thin.id_xmin = 1; + thin.id_ymax = length(y); + thin.id_ymin = 1; + thin.num_of_thinning_points = thin.percentage/100 * numel(z(thin.id_xmin : thin.id_xmax, thin.id_ymin : thin.id_ymax)); + %Loop thinning until the specified percentage is reached + while sum(sum(isnan(z(thin.id_xmin : thin.id_xmax, thin.id_ymin : thin.id_ymax)))) < thin.num_of_thinning_points + thin.id_x = randi([thin.id_xmin, thin.id_xmax]); + thin.id_y = randi([thin.id_ymin, thin.id_ymax]); + z(thin.id_x, thin.id_y) = NaN; + end + end + if strcmp(thin.setting,'on') + %Convert units + thin.id_length = round(thin.length/field.dX); + for i = 1:thin.num + %Convert units + thin.id_center(i,:) = round(thin.center(i,:)/field.dX); + + %Set the void area in grid-format indeies + thin.id_xmax(i) = round(thin.id_center(i,1) + thin.id_length/2 + 1); + thin.id_xmin(i) = round(thin.id_center(i,1) - thin.id_length/2 + 1); + thin.id_ymax(i) = round(thin.id_center(i,2) + thin.id_length/2 + 1); + thin.id_ymin(i) = round(thin.id_center(i,2) - thin.id_length/2 + 1); + thin.num_of_thinning_points = thin.percentage(i)/100 * numel(z(thin.id_xmin(i) : thin.id_xmax(i), thin.id_ymin(i) : thin.id_ymax(i))); + %Loop thinning until the specified percentage is reached + while sum(sum(isnan(z(thin.id_xmin(i) : thin.id_xmax(i), thin.id_ymin(i) : thin.id_ymax(i))))) < thin.num_of_thinning_points + thin.id_x = randi([thin.id_xmin(i), thin.id_xmax(i)]); + thin.id_y = randi([thin.id_ymin(i), thin.id_ymax(i)]); + z(thin.id_x, thin.id_y) = NaN; + end + end + + end +end + +%%%%%%%%%%%%%%%%%%%%%% Void Area %%%%%%%%%%%%%%%%%%%%%% +if strcmp(void.setting, 'on') +% void.id_length = void.length/field.dX; + void.id_length_xy = void.length_xy/field.dX; +% %%% Create void area %%% +% for i = 1:void.num +% %Convert units +% void.id_center(i,:) = round(void.center(i,:)/field.dX); +% +% %Set the void area in grid-format indeies +% void.id_xmax(i) = void.id_center(i,1) + void.id_length/2 +1; +% void.id_xmin(i) = void.id_center(i,1) - void.id_length/2 +1; +% void.id_ymax(i) = void.id_center(i,2) + void.id_length/2 +1; +% void.id_ymin(i) = void.id_center(i,2) - void.id_length/2 +1; +% +% % Insert NaN in the void area +% z(void.id_xmin(i):void.id_xmax(i),void.id_ymin(i):void.id_ymax(i)) = NaN; +% end + %%% Create void area %%% + for i = 1:void.num + %Convert units + void.id_center(i,:) = round(void.center(i,:)/field.dX); + + %Set the void area in grid-format indeies + void.id_xmax(i) = void.id_center(i,1) + void.id_length_xy(i,1)/2 +1; + void.id_xmin(i) = void.id_center(i,1) - void.id_length_xy(i,1)/2 +1; + void.id_ymax(i) = void.id_center(i,2) + void.id_length_xy(i,2)/2 +1; + void.id_ymin(i) = void.id_center(i,2) - void.id_length_xy(i,2)/2 +1; + + % Insert NaN in the void area + z(void.id_xmin(i):void.id_xmax(i),void.id_ymin(i):void.id_ymax(i)) = NaN; + end +end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save the map %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +save(filename,'x','y','z') + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% View the map %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +[map.n,map.m] = size(z); +map_points = zeros(3,map.n*map.m); +map_points(1,:) = reshape(field.X,1,map.n*map.m); +map_points(2,:) = reshape(field.Y,1,map.n*map.m); +map_points(3,:) = reshape(z,1,map.n*map.m); +figure(1); +map_points = rmmissing(map_points,2); +plot3(map_points(1,:),map_points(2,:),map_points(3,:),'g.') + + diff --git a/src/environment/ini_graspable_points.m b/src/environment/ini_graspable_points.m index 8ebed3f..bcc8cc5 100644 --- a/src/environment/ini_graspable_points.m +++ b/src/environment/ini_graspable_points.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-05-12 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-10-01 +%%%%%% Last update: 2021-04 -16 %%%%%% Keigo Haji % % @@ -36,11 +36,13 @@ surface_param.graspable_points(2,:)=reshape(Y,1,n*m); surface_param.graspable_points(3,:)=reshape(z,1,n*m); - +% Deletes the columns that have NaN values. +surface_param.graspable_points = rmmissing(surface_param.graspable_points, 2); % set the floor level variable to have a z directional height in global -% frame. +% frame. surface_param.floor_level = mean(surface_param.graspable_points(3,:)); + switch environment_param.graspable_points_detection_type case 'all' ; @@ -48,6 +50,16 @@ surface_param.graspable_points = target_detection(surface_param.graspable_points, environment_param.graspable_points_detection_type, gripper_param, map_param, matching_settings); case 'peaks' surface_param.graspable_points = target_detection(surface_param.graspable_points, environment_param.graspable_points_detection_type, gripper_param, map_param, matching_settings); + case 'climbing_holds_map_on_testfield' + load('graspable_points_of_testfield_without_board.mat'); + surface_param.graspable_points = graspable_points_on_testfield_deleted_targets_on_board; + % load('graspable_points_on_test_field.mat'); + % surface_param.graspable_points = graspable_points_on_test_field; + case 'climbing_holds_map_on_full_testfield' + load('graspable_points_of_full_testfield.mat'); + surface_param.graspable_points = graspable_points_on_full_testfield; + % load('graspable_points_on_test_field.mat'); + % surface_param.graspable_points = graspable_points_on_test_field; otherwise if isnumeric(environment_param.graspable_points_detection_type) if environment_param.graspable_points_detection_type >=0 && environment_param.graspable_points_detection_type <=100 diff --git a/src/environment/ini_surface.m b/src/environment/ini_surface.m index a2846a1..5c9f0ba 100644 --- a/src/environment/ini_surface.m +++ b/src/environment/ini_surface.m @@ -5,8 +5,8 @@ %%%%%% %%%%%% Created 2020-04-06 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-10-16 -%%%%%% Kentaro Uno +%%%%%% Last update: 2021-06-28 +%%%%%% Keigo Haji % % % Load surface points from .mat file and set contact characteristics (stiffness and damping) @@ -19,24 +19,48 @@ % surface_param.D : Ground reaction force damping coefficient (scalar) % INPUT % environment_param : Parameters for environment (class) +% +% [NOTE] +% If you added new bouldering holds map taken by RealSense camera, you must +% include 'climbing_holds' in the .mat file name. This is because we +% switch how to plot the surface in vis_surface depending on the name of +% the map. function surface_param = ini_surface(environment_param) global x ; global y ; global z; surface_param.graspable_points = []; +surface_param.known_environment = []; switch environment_param.surface_type case 'flat_HR' load('map_flat_HR.mat'); + +case 'flat_HR_5m_x_5m' + load('map_flat_HR_5m_x_5m.mat'); + +case 'flat_zero-height_5m_x_5m' + load('map_flat_zero-height_5m_x_5m.mat'); case 'rough' load('map_uneven.mat'); + +case 'rough_3m_x_3m' + load('map_uneven_3m_x_3m.mat'); + +case 'climbing_holds_map_on_testfield' + load('climbing_holds_map_on_testfield.mat'); + % see also ini_graspable_points.m + +case 'climbing_holds_map_on_full_testfield' + load('climbing_holds_map_on_full_testfield.mat'); + % see also ini_graspable_points.m case 'climbing_holds_1m_x_1m' load('climbing_holds_1m_x_1m.mat'); % see also ini_graspable_points.m - + case 'flat_003' load('flat_delta_003.mat'); @@ -51,13 +75,96 @@ case 'flat_012' load('flat_delta_012.mat'); + +case 'grid_3mx3m_dx100mm_thined_30' + load('map_grid_3mx3m_dx100mm_thined_30.mat'); + +case 'grid_3mx3m_dx100mm_thined_40' + load('map_grid_3mx3m_dx100mm_thined_40.mat'); + +case 'grid_3mx3m_dx100mm_thined_50' + load('map_grid_3mx3m_dx100mm_thined_50.mat'); + +case 'grid_3mx3m_dx100mm_thined_50_ver2' + load('map_grid_3mx3m_dx100mm_thined_50_ver2.mat'); + +case 'test_grid_map_3mx3m_dx85mm' + load('test_grid_map_3mx3m_dx85mm.mat'); + surface_param.known_environment = z; + +case 'test_grid_map_3mx3m_dx85mm_1_void_area_at_center_25cm' + load('test_grid_map_3mx3m_dx85mm_1_void_area_at_center_25cm.mat'); + +case 'test_grid_map_3mx3m_dx85mm_1_void_area_at_center_40cm' + load('test_grid_map_3mx3m_dx85mm_1_void_area_at_center_40cm.mat'); +case 'test_grid_map_3mx3m_dx85mm_thined_20_1void_area_at_center_40cm' + load('test_grid_map_3mx3m_dx85mm_thined_20_1void_area_at_center_40cm.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm' + load('test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm_ver2' + load('test_grid_map_3mx3m_dx85mm_thined_15_1void_area_at_center_40cm_ver2.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_30_1void_area_at_center_40cm' + load('test_grid_map_3mx3m_dx85mm_thined_30_1void_area_at_center_40cm.mat'); + +case 'test_grid_map_3mx3m_dx70mm_thined_50_1void_area_40cm_at_center' + load('test_grid_map_3mx3m_dx70mm_thined_50_1void_area_40cm_at_center.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm' + load('test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver2' + load('test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver2.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver3' + load('test_grid_map_3mx3m_dx85mm_thined_40_1void_area_at_center_40cm_ver3.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_50' + load('test_grid_map_3mx3m_dx85mm_thined_50.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm' + load('test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm_ver2' + load('test_grid_map_3mx3m_dx85mm_thined_50_1void_area_at_center_40cm_ver2.mat'); + +case 'test_grid_map_3mx3m_dx85mm_2void_areas' + %%% Get the Obstacles infromation + load('known_test_grid_map_3mx3m_dx85mm_2void_areas_with_15cm_margin.mat'); + surface_param.known_environment = z; + load('test_grid_map_3mx3m_dx85mm_2void_areas.mat') + +case 'test_grid_map_3mx3m_dx85mm_thined_20_2void_areas' +% %%% Get the Obstacles infromation +% surface_param.known_environment = load('test_grid_map_3mx3m_dx85mm_2void_areas.mat'); +% surface_param.known_environment.x = x; +% surface_param.known_environment.y = y; +% surface_param.known_environment.z = z; + load('test_grid_map_3mx3m_dx85mm_thined_20_2void_areas.mat'); + +case 'test_grid_map_3mx3m_dx85mm_thined_30_2void_areas' + %%% Get the Obstacles infromation + load('test_grid_map_3mx3m_dx85mm_thined_30_2void_areas.mat'); + +case 'test_grid_map_3mx3m_dx50mm_thined_70_2void_areas' + load('test_grid_map_3mx3m_dx50mm_thined_70_2void_areas.mat'); + +case 'test_grid_map_3mx3m_dx50mm_thined_70' + load('test_grid_map_3mx3m_dx50mm_thined_70.mat'); + +case 'test_grid_map_3mx3m_dx10mm_thined_0988' + load('test_grid_map_3mx3m_dx10mm_thined_0988.mat'); + otherwise disp('invalid surface'); end surface_param.min = min(min(z)); +surface_param.max = max(max(z)); % Contact parameters surface_param.K = environment_param.surface_K; % Floor reaction force stiffness coefficient diff --git a/src/environment/map_flat_HR.mat b/src/environment/map_flat_HR.mat index 6ea7d17..3946493 100644 Binary files a/src/environment/map_flat_HR.mat and b/src/environment/map_flat_HR.mat differ diff --git a/src/environment/map_flat_HR_5m_x_5m.mat b/src/environment/map_flat_HR_5m_x_5m.mat new file mode 100644 index 0000000..e661491 Binary files /dev/null and b/src/environment/map_flat_HR_5m_x_5m.mat differ diff --git a/src/environment/map_format_conversion.m b/src/environment/map_format_conversion.m index eb07a7b..49d537d 100644 --- a/src/environment/map_format_conversion.m +++ b/src/environment/map_format_conversion.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-10-16 %%%%%% Keigo Haji -%%%%%% Last update: 2020-10-16 +%%%%%% Last update: 2021-04-19 %%%%%% Keigo Haji %------------------------------------------------------------------------- % map_format_conversion.m changes the point cloud data of the terrain @@ -25,13 +25,18 @@ % interpolation_method : "linear" or "natural" % % [Note] -% This function is not executed in climb_main.m. +% This function is not executed in main_sim.m. % This function is used to convert a new PCD file acquired by a RealSense % camera from a 3*N matrix to the grid format used by ClimbLab. % After execution, check the results to see if the terrain data is not % upside down. % If there is no problem, pack the three variables of xyz together and save % them as a .mat file. +% [Note 2] +% When you added new bouldering holds map taken by RealSense camera in +% ClimbLab, you must include 'climbing_holds' in the .mat file name. This +% is because we switch how to plot the surface in vis_surface depending on +% the name of the map. %------------------------------------------------------------------------- function [x,y,z] = map_format_conversion(pcd,interpolation_method) diff --git a/src/environment/map_grid_3mx3m_dx100mm.mat b/src/environment/map_grid_3mx3m_dx100mm.mat new file mode 100644 index 0000000..d934fd1 Binary files /dev/null and b/src/environment/map_grid_3mx3m_dx100mm.mat differ diff --git a/src/environment/map_grid_3mx3m_dx100mm_thined_30.mat b/src/environment/map_grid_3mx3m_dx100mm_thined_30.mat new file mode 100644 index 0000000..e7402b5 Binary files /dev/null and b/src/environment/map_grid_3mx3m_dx100mm_thined_30.mat differ diff --git a/src/environment/map_grid_3mx3m_dx100mm_thined_40.mat b/src/environment/map_grid_3mx3m_dx100mm_thined_40.mat new file mode 100644 index 0000000..11d65ef Binary files /dev/null and b/src/environment/map_grid_3mx3m_dx100mm_thined_40.mat differ diff --git a/src/environment/map_grid_3mx3m_dx100mm_thined_50.mat b/src/environment/map_grid_3mx3m_dx100mm_thined_50.mat new file mode 100644 index 0000000..e42f9d0 Binary files /dev/null and b/src/environment/map_grid_3mx3m_dx100mm_thined_50.mat differ diff --git a/src/environment/map_grid_3mx3m_dx100mm_thined_50_ver2.mat b/src/environment/map_grid_3mx3m_dx100mm_thined_50_ver2.mat new file mode 100644 index 0000000..52620e0 Binary files /dev/null and b/src/environment/map_grid_3mx3m_dx100mm_thined_50_ver2.mat differ diff --git a/src/environment/map_uneven.mat b/src/environment/map_uneven.mat index c778132..5845f5e 100644 Binary files a/src/environment/map_uneven.mat and b/src/environment/map_uneven.mat differ diff --git a/src/environment/map_uneven_3m_x_3m.mat b/src/environment/map_uneven_3m_x_3m.mat new file mode 100644 index 0000000..240ae68 Binary files /dev/null and b/src/environment/map_uneven_3m_x_3m.mat differ diff --git a/src/main_sim.m b/src/main_sim.m new file mode 100644 index 0000000..54eca00 --- /dev/null +++ b/src/main_sim.m @@ -0,0 +1,149 @@ +%%%%%% MAIN +%%%%%% main_sim +%%%%%% +%%%%%% Main file for climbing/walking simulation +%%%%%% +%%%%%% Created: 2020-02-20 +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2021-08-23 +%%%%%% by Kentaro Uno + +clc; clear; close all; +tic; ini_path(); variables_saved = []; evaluation_param = []; trajectories = []; + +%%% Select configuration: +%%% - 'default', +%%% - 'USER', +%%% - 'example_demo_1', +%%% - 'example_demo_2', +%%% - 'example_demo_3', +%%% - 'crawl_gait_for_discrete_footholds', +%%% - 'nonperiodic_gait_for_discrete_footholds', +%%% - 'gia_static', +%%% - 'iSAIRAS_2020_demo' +%%% - 'clawar_2021_dynamic_climbing_demo' +%%% - 'base_pos_opt_quasistatic' % this is now under development +%%% - 'hubrobo_testfield_exp' +config = 'default'; + +%%% Load all initial parameters from configuration files stored in config/ +[robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_simulation(config); + +%%% Define a code for current set of simulations +run_cod = 'test'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize environment %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +global d_time; global Gravity; global Ez; global contact_f; global x; global y; global z; + +ini_environment(environment_param); +surface_param = ini_surface(environment_param); +surface_param = ini_graspable_points(environment_param, surface_param, gripper_param, map_param, matching_settings); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +[LP, SV, des_SV, POS_e, ORI_e, cont_POS, ani_settings] = ini_robot(robot_param, gait_planning_param, surface_param, ani_settings); +shape_robot = vis_create_robot_model(ani_settings, LP); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize sensing %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +surface_param = ini_sensed_graspable_points(SV, surface_param, sensing_camera_param); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize gait %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +[gait_planning_param, motion_planning_param] = ini_gait(gait_planning_param, SV); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize files %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +[run_id, run_date] = ini_id(robot_param, environment_param, gait_planning_param, control_param); +video = ini_video_file(ani_settings, run_cod, run_id, run_date); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Simulation loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +for time = 0:d_time:environment_param.time_max + % Display time + disp(time) + +% % @TODO: this will also be implemented as a optional switch and +% % simulation stopper function +% if norm(SV.R0(1:2) - gait_planning_param.goal(1:2,gait_planning_param.goal_num)) < 0.05 +% break +% end + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Sensing %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + surface_param = upd_sensed_graspable_points(SV, surface_param, sensing_camera_param); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Planning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + gait_planning_param = upd_gait_planning(gait_planning_param, surface_param, des_SV, SV, LP, POS_e, ... + environment_param, robot_param, sensing_camera_param,time); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Motion Planning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + [motion_planning_param, des_SV] = upd_motion_planning(motion_planning_param, gait_planning_param, ... + LP, SV, des_SV, cont_POS, POS_e, time); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Evaluation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + equilibrium_eval_param = upd_equilibrium_eval(equilibrium_eval_param, surface_param, ani_settings, LP, SV, POS_e); + + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot controller %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + SV = upd_control(SV, des_SV, control_param); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Kinematics and dynamics %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + SV = upd_grasp_detach(LP, SV, des_SV, environment_param, equilibrium_eval_param, variables_saved); + cont_POS = upd_collision_endeffector(LP, SV, POS_e, cont_POS); + SV = upd_ground_reaction_forces_spring_damper(LP, SV, surface_param, POS_e, cont_POS); + SV = upd_fwd_dynamics(environment_param, LP, SV, des_SV); + [POS_e, Qe_deg, Q0_deg, SV] = upd_fwd_kinematics(LP, SV); + + evaluation_param = upd_manipulability(LP,SV,evaluation_param); + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save selected variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + variables_saved = upd_variables_saved(variables_saved, save_settings, time, LP, SV, POS_e, environment_param, equilibrium_eval_param, evaluation_param, gait_planning_param); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Climbing animation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + % Visualization + if rem(time,1/ani_settings.frame_rate) == 0 && strcmp(ani_settings.display,'on') + inc = environment_param.inc; + figure(1); clf; hold on; + vis_robot(LP, SV, POS_e, shape_robot, inc, ani_settings); + ani_settings = vis_animation_range(motion_planning_param,surface_param,ani_settings,inc); + vis_surface(inc, ani_settings, environment_param, surface_param); + vis_graspable_points(surface_param,inc,ani_settings); + vis_graspable_points_in_reachable_area(gait_planning_param, inc, ani_settings); + vis_reachable_area(SV, LP, gait_planning_param, inc, ani_settings, surface_param); + vis_support_triangle(SV,LP,POS_e,inc,ani_settings); + gait_planning_param = vis_com_projection(LP, SV, inc, ani_settings, gait_planning_param); + vis_next_desired_position(gait_planning_param, inc, ani_settings); + vis_stability_polyhedron(inc, equilibrium_eval_param, ani_settings); + vis_vectors(inc,ani_settings,equilibrium_eval_param,LP,SV,POS_e,gait_planning_param,sensing_camera_param); + vis_animation_settings(ani_settings,surface_param,time); + vis_com_proj_trajectory(gait_planning_param, ani_settings, inc); + vis_trajectory_4legged(environment_param, motion_planning_param, ani_settings); + vis_sensing_camera_fov(SV, inc, sensing_camera_param, ani_settings, surface_param); + vis_sensed_graspable_points(surface_param, inc, ani_settings); + vis_goal(gait_planning_param, inc, ani_settings); + writeVideo(video,getframe(1)); +% %%% if you want to save the snapshots at the specific time period, you can uncomment the following code. --> @TODO: this should be selectable. +% % @TODO: this will also be implemented as a optional switch +% if rem(time, 10) == 0 +% % Create directory +% if time == 0 +% dir_name = ['dat/' run_cod '/' run_id]; +% mkdir(dir_name); +% end +% saveas(gcf, [ 'dat/' run_cod '/' run_id '/' num2str(time) 'sec.png'], 'png') +% saveas(gcf, [ 'dat/' run_cod '/' run_id '/' num2str(time) 'sec.fig'], 'fig') +% end + end + + %%% Simulation break + if upd_stop_sim(environment_param, gait_planning_param, SV, des_SV, LP, time) == false + break + end + + %%% Update Goal Position + gait_planning_param = upd_goal_position(gait_planning_param, environment_param, LP, SV); +end +%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Saving variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +close(video); +data = sav_data_file(variables_saved, save_settings, run_cod, run_id, run_date, config); +% Plot graphs +vis_plot_graph(data, plot_settings, LP, inc, surface_param, gait_planning_param); + +toc +% EOF \ No newline at end of file diff --git a/src/misc/ini_id.m b/src/misc/ini_id.m index 80b1558..551e409 100644 --- a/src/misc/ini_id.m +++ b/src/misc/ini_id.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-10 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 +%%%%%% Last updated: 2021-03-31 +%%%%%% Keigo Haji % % % Create identification for file naming @@ -13,18 +14,19 @@ % 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_param : Parameter for robot (class) -% environment_param : Parameter for environment (class) -% gait_param : Parameter for gait (class) -% control_param : Parameter for control (class) +% robot_param : Parameter for robot (class) +% environment_param : Parameter for environment (class) +% gait_planning_param : Parameter for gait planning (class) +% control_param : Parameter for control (class) -function [run_id, run_date] = ini_id(robot_param, environment_param, gait_param, control_param) +function [run_id, run_date] = ini_id(robot_param, environment_param, gait_planning_param, control_param) run_date = [datestr(now,'yymmdd_HHMMSS')]; % Date and time -run_id = [run_date '-' robot_param.robot_type '-' gait_param.type '-' control_param.type '-' environment_param.surface_type ... - '-' num2str(environment_param.inc) '[deg]-' num2str(environment_param.grav) '[G]']; +% run_id = [run_date '-' robot_param.robot_type '-' gait_planning_param.type '-' control_param.type '-' environment_param.surface_type ... +% '-' num2str(environment_param.inc) '[deg]-' num2str(environment_param.grav) '[G]']; +run_id = [run_date]; end \ No newline at end of file diff --git a/src/misc/sav_data_file.m b/src/misc/sav_data_file.m index bb89b94..0eb6fe0 100644 --- a/src/misc/sav_data_file.m +++ b/src/misc/sav_data_file.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-05-13 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-05-13 +%%%%%% Last update: 2021-03-31 +%%%%%% Keigo Haji % % % Create .csv file to save simulation data @@ -23,7 +24,7 @@ % run_date : Run identification date (string) -function data = sav_data_file(variables_saved, save_settings, run_cod, run_id, run_date) +function data = sav_data_file(variables_saved, save_settings, run_cod, run_id, run_date, config) %%% Definition of files for data file %%% if strcmp(save_settings.csv_file,'on') @@ -33,6 +34,19 @@ % Convert to table and save csv data = struct2table(variables_saved); writetable(data,[dir_name '/' run_date '_data.csv']); + %%% Save loaded config file in dat folder + if strcmp(save_settings.config_file, 'on') + if strcmp(config, 'default') + elseif strcmp(config, 'USER') + config_file_name = ['config_' config '_param.m']; + config_file_name = append('./config/USER/', config_file_name); + copyfile(config_file_name, dir_name); + else + config_file_name = ['config_' config '_param.m']; + config_file_name = append('./config/preset/', config_file_name); + copyfile(config_file_name, dir_name); + end + end else data = []; end diff --git a/src/misc/upd_goal_position.m b/src/misc/upd_goal_position.m new file mode 100644 index 0000000..979ed26 --- /dev/null +++ b/src/misc/upd_goal_position.m @@ -0,0 +1,65 @@ +%%%%%% Update +%%%%%% upd_goal_position +%%%%%% +%%%%%% Update goal position +%%%%%% +%%%%%% Created 2021-05-03 +%%%%%% Keigo Haji +%%%%%% Updated 2021-07-09 +%%%%%% Keigo Haji +% +% Update goal position to pass some positions on the map +% +% Function variables: +% +% OUTPUT +% gait_planning_param : Parameters for gait (class) +% INPUT +% environment_param :Parameters for environment (class) +% gait_planning_param : Parameters for gait (class) +% SV : State Variables (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) + +function gait_planning_param = upd_goal_position(gait_planning_param, environment_param, LP, SV) + +% Execute when multiple goals are set +if size(gait_planning_param.goal, 2) > 1 + i = gait_planning_param.goal_num; + % Calculate the distance between the robot base position and the + % current goal position + distance_goal = norm(SV.R0(1:2) - gait_planning_param.goal(1:2,i)); + + % If the robot is closer to the goal than the threshold, it is judged + % to have reached it. +% if distance_goal < environment_param.sim_stop_goal_threshold && sum(SV.sup) == LP.num_limb +% if distance_goal < environment_param.sim_stop_goal_threshold + + % Update the sub-goal position when the robot passes the sub-goal. + % This is because the moving field of local path planning doesn't work + % well if the goal position is updated before the robot passes the + % goal. + if gait_planning_param.goal_num < size(gait_planning_param.goal,2) + % Set a vector from the robot base to the current sub-gaol + vec_current_goal_to_base = SV.R0 - gait_planning_param.goal(:,gait_planning_param.goal_num); + unit_vec_current_goal_to_base = vec_current_goal_to_base/norm(vec_current_goal_to_base); + % Set a vector from the current sub-goal to the next sub-goal + vec_current_subgoal_to_next_subgoal = gait_planning_param.goal(:,gait_planning_param.goal_num + 1) - gait_planning_param.goal(:,gait_planning_param.goal_num); + unit_vec_current_subgoal_to_next_subgoal = vec_current_subgoal_to_next_subgoal/norm(vec_current_subgoal_to_next_subgoal); + % Calculate theta between two vectors + dot_tmp = dot(unit_vec_current_subgoal_to_next_subgoal(1:2),unit_vec_current_goal_to_base(1:2)); + theta_tmp = acos(dot_tmp); % 0 to pi + % if distance_goal < environment_param.sim_stop_goal_threshold && theta_tmp <= pi/2 + if theta_tmp < pi/2 + % Only in the case of crawl gait, Update the goal judgment for each sequence + if strfind(gait_planning_param.type, 'crawl') > 0 + if gait_planning_param.swing_number == gait_planning_param.sequence(end) + gait_planning_param.goal_num = i + 1; + end + else + % Set the goal as next one + gait_planning_param.goal_num = i +1; + end + end + end +end + diff --git a/src/misc/upd_manipulability.m b/src/misc/upd_manipulability.m new file mode 100644 index 0000000..a047d7b --- /dev/null +++ b/src/misc/upd_manipulability.m @@ -0,0 +1,32 @@ +%%%%%% Update +%%%%%% upd_manipulability +%%%%%% +%%%%%% calculate manipulability +%%%%%% +%%%%%% Created 2020-04-10 +%%%%%% Koizumi Yusuke +%%%%%% Last update: 2020-10-30 +% +% +% Calculate manipulability of each limb +% +% Function variables: +% +% OUTPUT +% evaluation_param : Parameters for evaluation (struct) +% INPUT +% LP : Link Parameters +% SV : State Variables +% evaluation_param : Parameters for evaluation (struct) + + +function [ evaluation_param ] = upd_manipulability(LP,SV,evaluation_param) +mani = zeros(1,LP.num_limb); +joints = zeros(LP.num_limb,3); +for i = 1:LP.num_limb + joints(i,:) = j_num(LP, i); + je = calc_je(LP,SV,joints(i,:)); + mani(i) = sqrt(det(je(1:3,3*i-2:3*i)*je(1:3,3*i-2:3*i)')); +end +evaluation_param.manipulability = mani; +end \ No newline at end of file diff --git a/src/misc/upd_stop_goal_any.m b/src/misc/upd_stop_goal_any.m new file mode 100644 index 0000000..0c10989 --- /dev/null +++ b/src/misc/upd_stop_goal_any.m @@ -0,0 +1,33 @@ +%%%%%% Update +%%%%%% upd_stop_goal_any +%%%%%% +%%%%%% Simulation stopper goal +%%%%%% +%%%%%% Created 2021-04-13 +%%%%%% Warley Ribeiro +%%%%%% Last updated 2021-05-04 +%%%%%% Keigo Haji + +% +% +% Stop simulation if the distance to Goal falls below a certain threshold +% +% Function variables: +% +% OUTPUT +% simu_flag : Simulation stop if flag is false (logical) +% INPUT +% environment_param :Parameters for environment (class) +% gait_planning_param : Parameters for gait (class) +% SV : State Variables (SpaceDyn class) + +function simu_flag = upd_stop_goal_any(simu_flag, environment_param, gait_planning_param, SV) + +if strcmp(environment_param.sim_stop_goal_any,'on') + % Find the distance to the last goal, when there are multiple goals, + distance_goal = norm(SV.R0(1:2) - gait_planning_param.goal(1:2,end)); + if distance_goal < environment_param.sim_stop_goal_threshold + simu_flag = false; + disp("STOP: Robot reached goal position") + end +end \ No newline at end of file diff --git a/src/misc/upd_stop_goal_grasping.m b/src/misc/upd_stop_goal_grasping.m new file mode 100644 index 0000000..417476d --- /dev/null +++ b/src/misc/upd_stop_goal_grasping.m @@ -0,0 +1,31 @@ +%%%%%% Update +%%%%%% upd_stop_goal_grasping +%%%%%% +%%%%%% Simulation stopper goal +%%%%%% +%%%%%% Created 2021-04-13 +%%%%%% Warley Ribeiro + +% +% +% Stop simulation if the distance to Goal falls below a certain threshold and all legs are grasping +% +% Function variables: +% +% OUTPUT +% simu_flag : Simulation stop if flag is false (logical) +% INPUT +% environment_param :Parameters for environment (class) +% gait_planning_param : Parameters for gait (class) +% SV : State Variables (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) + +function simu_flag = upd_stop_goal_grasping(simu_flag, environment_param, gait_planning_param, LP, SV) + +if strcmp(environment_param.sim_stop_goal_grasping,'on') + distance_goal = norm(SV.R0(1:2) - gait_planning_param.goal(1:2,end)); + if distance_goal < environment_param.sim_stop_goal_threshold && sum(SV.sup) == LP.num_limb + simu_flag = false; + disp("STOP: Robot reached goal position") + end +end \ No newline at end of file diff --git a/src/misc/upd_stop_joint_limit.m b/src/misc/upd_stop_joint_limit.m new file mode 100644 index 0000000..86bfbd0 --- /dev/null +++ b/src/misc/upd_stop_joint_limit.m @@ -0,0 +1,34 @@ +%%%%%% Update +%%%%%% upd_stop_joint_limit +%%%%%% +%%%%%% Simulation stopper joint limit +%%%%%% +%%%%%% Created 2021-04-13 +%%%%%% Warley Ribeiro + +% +% +% Stop simulation if the joint position is outside joint limits +% +% Function variables: +% +% OUTPUT +% simu_flag : Simulation stop if flag is false (logical) +% INPUT +% environment_param : Parameters for environment (class) +% SV : State Variables (SpaceDyn class) + +function simu_flag = upd_stop_joint_limit(simu_flag, environment_param, LP, SV) + +if strcmp(environment_param.sim_stop_joint_limit,'on') + for i = 1:LP.num_limb + joint_limit(3*i-2:3*i-0) = [deg2rad(LP.joint_limit(1,1)) < SV.q(3*i-2) && SV.q(3*i-2) < deg2rad(LP.joint_limit(1,2)); + deg2rad(LP.joint_limit(2,1)) < SV.q(3*i-1) && SV.q(3*i-2) < deg2rad(LP.joint_limit(2,2)); + deg2rad(LP.joint_limit(3,1)) < SV.q(3*i-0) && SV.q(3*i-2) < deg2rad(LP.joint_limit(2,2))]; + end + if sum(joint_limit) < LP.num_q + simu_flag = false; + disp("STOP: Configuration is outside joint limit"); + end + +end \ No newline at end of file diff --git a/src/misc/upd_stop_sim.m b/src/misc/upd_stop_sim.m new file mode 100644 index 0000000..8c76c51 --- /dev/null +++ b/src/misc/upd_stop_sim.m @@ -0,0 +1,44 @@ +%%%%%% Update +%%%%%% upd_stop_sim +%%%%%% +%%%%%% Simulation stopper +%%%%%% +%%%%%% Created 2020-07-23 +%%%%%% Koki Kurihara +%%%%%% Last update: 2020-12-18 +%%%%%% Warley Ribeiro + +% +% +% Stop simulation based on selected configuration parameters +% +% Function variables: +% +% OUTPUT +% simu_flag : Simulation stop if flag is false (logical) +% INPUT +% environment_param :Parameters for environment (class) +% gait_planning_param : Parameters for gait (class) +% SV : State Variables (SpaceDyn class) +% des_SV : State Variables (SpaceDyn class) +% LP : Link parameters (SpaceDyn class) +% time : Simulation time [s] (scalar) + +function simu_flag = upd_stop_sim(environment_param, gait_planning_param, SV, des_SV, LP, time) + +% initialize flag +simu_flag = true; + + % Stop simulation when robot reaches the goal + simu_flag = upd_stop_goal_grasping(simu_flag, environment_param, gait_planning_param, LP, SV); + simu_flag = upd_stop_goal_any(simu_flag, environment_param, gait_planning_param, SV); + % Stop simulation when robot is selecting the same grasping point + simu_flag = upd_stop_stuck(simu_flag, environment_param, gait_planning_param, LP,des_SV); + % Stop simulation for singular configuration + simu_flag = upd_stop_singularity(simu_flag, environment_param, SV); + % Stop simulation for joint limit + simu_flag = upd_stop_joint_limit(simu_flag, environment_param, LP, SV); + % Stop simulation for time limit + simu_flag = upd_stop_time_max(simu_flag, environment_param, time); + +end \ No newline at end of file diff --git a/src/misc/upd_stop_singularity.m b/src/misc/upd_stop_singularity.m new file mode 100644 index 0000000..6815f0a --- /dev/null +++ b/src/misc/upd_stop_singularity.m @@ -0,0 +1,28 @@ +%%%%%% Update +%%%%%% upd_stop_singularity +%%%%%% +%%%%%% Simulation stopper singularity +%%%%%% +%%%%%% Created 2021-04-13 +%%%%%% Warley Ribeiro + +% +% +% Stop simulation if the robot is in a singular configuration +% +% Function variables: +% +% OUTPUT +% simu_flag : Simulation stop if flag is false (logical) +% INPUT +% environment_param : Parameters for environment (class) +% SV : State Variables (SpaceDyn class) + +function simu_flag = upd_stop_singularity(simu_flag, environment_param, SV) + +if strcmp(environment_param.sim_stop_singular,'on') + if isreal(SV.q) == false + simu_flag = false; + disp("STOP: Singularity"); + end +end \ No newline at end of file diff --git a/src/misc/upd_stop_stuck.m b/src/misc/upd_stop_stuck.m new file mode 100644 index 0000000..6d294d5 --- /dev/null +++ b/src/misc/upd_stop_stuck.m @@ -0,0 +1,72 @@ +%%%%%% Update +%%%%%% upd_stop_stuck +%%%%%% +%%%%%% Simulation stopper stuck +%%%%%% +%%%%%% Created 2021-04-13 +%%%%%% Warley Ribeiro +%%%%%% Lase updated: 2021-04-20 +%%%%%% Keigo Haji +% +% +% Stop simulation if the robot selects the same position for grasping point +% +% Function variables: +% +% OUTPUT +% simu_flag : Simulation stop if flag is false (logical) +% INPUT +% environment_param :Parameters for environment (class) +% gait_planning_param : Parameters for gait (class) +% LP : Link parameters (SpaceDyn class) +% des_SV : Desired state values (SpaceDyn class) + +function simu_flag = upd_stop_stuck(simu_flag, environment_param, gait_planning_param, LP,des_SV) + +if strcmp(environment_param.sim_stop_stuck,'on') + + % If all legs are in support phase + if sum(des_SV.sup) == LP.num_limb + + % Initialize next positions of limbs + des_pos_of_limbs = zeros(3,4); + + % Insert current counts of footholds + count = gait_planning_param.footholds_count_limb; + + % Get the number of how many steps robot walked in total + m = length(gait_planning_param.swing_number_history); + + % Get the desired positions of limbs from footholds_history_limb + for i = 1:LP.num_limb + des_pos_of_limbs(:,i) = gait_planning_param.footholds_history_limb(:,gait_planning_param.footholds_count_limb(i), i); + end + + % Prepare the matrix representing previous limbs' positions + previous_pos_of_limbs = des_pos_of_limbs; + + % Check previous phases + for j = 1 : environment_param.sim_stop_stuck_serach_range_threshold + % Check if the phase can be traced back + if m - j >= 0 + % Get the previous swing limb number + swing_limb_num = gait_planning_param.swing_number_history(m-j+1); + % Update the previous positon of swing limb + previous_pos_of_limbs(:,swing_limb_num) = gait_planning_param.footholds_history_limb(:,count(swing_limb_num)-1,swing_limb_num); + % If the positions of the limbs are too close, we jduged + % the robot gets stuck. + % [Note] norm(X,1) means max(sum(abs(X))) + if norm(des_pos_of_limbs - previous_pos_of_limbs, 1) < environment_param.sim_stop_stuck_threshold + simu_flag = false; + disp("STOP: Robot is stuck") + end + % Update count to trace back + count(swing_limb_num) = count(swing_limb_num) - 1; + end + end + end + +end + + + diff --git a/src/misc/upd_stop_time_max.m b/src/misc/upd_stop_time_max.m new file mode 100644 index 0000000..f19646f --- /dev/null +++ b/src/misc/upd_stop_time_max.m @@ -0,0 +1,28 @@ +%%%%%% Update +%%%%%% upd_stop_time_max +%%%%%% +%%%%%% Simulation stopper time +%%%%%% +%%%%%% Created 2021-04-13 +%%%%%% Warley Ribeiro + +% +% +% Stop simulation if the simulation time reaches maximum value +% +% Function variables: +% +% OUTPUT +% simu_flag : Simulation stop if flag is false (logical) +% INPUT +% environment_param : Parameters for environment (class) +% time : Simulation time [s] (scalar) + +function simu_flag = upd_stop_time_max(simu_flag, environment_param, time) + +if strcmp(environment_param.sim_stop_time,'on') + if time >= environment_param.time_max + simu_flag = false; + disp("STOP: Maximum time") + end +end \ No newline at end of file diff --git a/src/misc/upd_variables_saved.m b/src/misc/upd_variables_saved.m index c314f69..ecc0263 100644 --- a/src/misc/upd_variables_saved.m +++ b/src/misc/upd_variables_saved.m @@ -5,10 +5,11 @@ %%%%%% %%%%%% Created 2020-05-12 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-09-08 +%%%%%% Last update: 2021-07-09 +%%%%%% Kentaro Uno % % -% Create identification for file naming +% Save history of selected variables % % Function variables: % @@ -21,16 +22,37 @@ % LP : Link Parameters (SpaceDyn class) % SV : State Variables (SpaceDyn class) % POS_e : Position of the end-effector [m] (3xnum_limb matrix) +% environment_param.time_step : time step of the simulation % equilibrium_eval_param : Parameters for equilibrium evaluation (struct) +% evaluation_param : Parameters for evaluation (struct) -function variables_saved = upd_variables_saved(variables_saved, save_settings, time, LP, SV, POS_e, equilibrium_eval_param) +function variables_saved = upd_variables_saved(variables_saved, save_settings, time, LP, SV, POS_e, environment_param, equilibrium_eval_param, evaluation_param, gait_planning_param) if strcmp(save_settings.csv_file,'on') if time == 0 cnt = 1; - else + variables_saved = save_variables(cnt, variables_saved, save_settings, time, LP, SV, POS_e, equilibrium_eval_param, evaluation_param, gait_planning_param); + + % With default settings, all variables are saved at each calc cycle + elseif save_settings.variable_saving_time_interval == environment_param.time_step cnt = size(variables_saved.time,1) + 1; + variables_saved = save_variables(cnt, variables_saved, save_settings, time, LP, SV, POS_e, equilibrium_eval_param, evaluation_param, gait_planning_param); + + % In case save_settings.variable_saving_time_interval > time_step, all + % variables are only logged every when the saving time interval is passed. + elseif save_settings.variable_saving_time_interval > environment_param.time_step + if round(rem(time, save_settings.variable_saving_time_interval),6) == environment_param.time_step % just rem is not enough to adjust the accuracy of the output + cnt = size(variables_saved.time,1) + 1; + variables_saved = save_variables(cnt, variables_saved, save_settings, time, LP, SV, POS_e, equilibrium_eval_param, evaluation_param, gait_planning_param); + end + else + error("In upd_variables_saved(): save_settings.variable_saving_time_interval should be larger than nvironment_param.time_step !!!"); end +end + +end + +function variables_saved = save_variables(cnt, variables_saved, save_settings, time, LP, SV, POS_e, equilibrium_eval_param, evaluation_param, gait_planning_param) % Save time variables_saved.time(cnt,1) = time; % Save Base position @@ -45,7 +67,7 @@ variables_saved.vd0(cnt,:) = SV.vd0'; % Save Base angular acceleration variables_saved.wd0(cnt,:) = SV.wd0'; - + % Save Joints angular position variables_saved.q(cnt,:) = SV.q'; % Save Joints angular velocity @@ -54,13 +76,23 @@ variables_saved.qdd(cnt,:) = SV.qdd'; % Save Joints torque variables_saved.tau(cnt,:) = SV.tau'; - + + % Save Maximum of absolute torque of all joint + if strcmp(save_settings.joint_max_torque,'on') + variables_saved.tau_max(cnt,1) = max( abs(SV.tau') ); + end + + % Save Root Mean Square (RMS) of torque of all joint (*need Signal Processing Toolbox) + if strcmp(save_settings.joint_rms_torque,'on') + variables_saved.tau_rms(cnt,1) = rms(SV.tau'); + end + % Save end-effector position for i = 1:size(POS_e,2) % Generate name for variables pos_e1, pos_e2, ... variables_saved.(genvarname(['pos_e' num2str(i)]))(cnt,:) = POS_e(:,i)'; end - + % Save end-effector reaction force Force = SV.Fe(:,LP.SE == 1); for i = 1:size(POS_e,2) @@ -68,17 +100,33 @@ variables_saved.(genvarname(['Fe' num2str(i)]))(cnt,:) = Force(:,i)'; variables_saved.(genvarname(['Fe_res' num2str(i)]))(cnt,:) = norm(Force(:,i)'); end - + % Save TSM if strcmp(save_settings.tsm,'on') variables_saved.tsm(cnt,1) = equilibrium_eval_param.tsm; end - + % Save GIA if strcmp(save_settings.gia,'on') variables_saved.gia(cnt,:) = equilibrium_eval_param.gia'; - variables_saved.inclination_margin(cnt,:) = equilibrium_eval_param.inclination_margin; - variables_saved.acc_margin(cnt,:) = equilibrium_eval_param.acc_margin'; + variables_saved.gia_inclination_margin(cnt,:) = equilibrium_eval_param.gia_inclination_margin; + variables_saved.gia_margin(cnt,:) = equilibrium_eval_param.gia_margin'; + end + + % Save Manipulability Measure + if strcmp(save_settings.manipulability,'on') + for i = 1:size(evaluation_param.manipulability,2) + variables_saved.(genvarname(['manipulability' num2str(i)]))(cnt,1) = evaluation_param.manipulability(i); + end + end + % Save the mean value of the Manipulability Measure + if strcmp(save_settings.mean_manipulability,'on') + variables_saved.mean_manipulability(cnt,1) = mean( evaluation_param.manipulability ); + end + + % Save the minimum value of the Manipulability Measure + if strcmp(save_settings.min_manipulability,'on') + variables_saved.min_manipulability(cnt,1) = min( evaluation_param.manipulability ); end end \ No newline at end of file diff --git a/src/robot/LP_analysis/README.md b/src/robot/LP_analysis/README.md index 0a4fa6c..c0dda40 100644 --- a/src/robot/LP_analysis/README.md +++ b/src/robot/LP_analysis/README.md @@ -22,14 +22,10 @@ All code is confirmed to work on hubrobo_description |--README.md |--docs/ -|--2D_reachable_region_vis/ -| |--sample_main.m -| |--SV -|--3D_reachable_region_vis/ -| |--sample_main.m -| |--SV |--tools/ |--LP_to_design/ +|--main_LP_2D_analysis.m +|--main_LP_3D_analysis.m ``` ## Build and Usage diff --git a/src/robot/LP_analysis/sample_2D_main.m b/src/robot/LP_analysis/main_LP_2D_analysis.m similarity index 65% rename from src/robot/LP_analysis/sample_2D_main.m rename to src/robot/LP_analysis/main_LP_2D_analysis.m index d7ec5c4..fc4e2fa 100644 --- a/src/robot/LP_analysis/sample_2D_main.m +++ b/src/robot/LP_analysis/main_LP_2D_analysis.m @@ -1,130 +1,155 @@ -%%%%%%--------------------------------------------------------------------- -%%%%%% sample_main.m -%%%%%%--------------------------------------------------------------------- -%%%%%% @@ -%%%%%% @@@@@@ -%%%%%% @```@@@@ -%%%%%% @` `@@@@@@ -%%%%%% @@` `@@@@@@@@ -%%%%%% @@` `@@@@@@@@@ Tohoku University -%%%%%% @` ` `@@@@@@@@@ SPACE ROBOTICS LABORATORY -%%%%%% @`` ## `@@@@@@@@@ http://www.astro.mech.tohoku.ac.jp/ -%%%%%% @` #..#`@@@@@@@@@ -%%%%%% @` #..#`@@@@@@@@@ -%%%%%% @` ### `@@@@@@@@@ Professor Kazuya Yoshida -%%%%%% @` ###``@@@@@@@@@ Assistant Professor Kenji Nagaoka -%%%%%% @### ``@@@@@@@@ -%%%%%% ###` `@@@@@@@ -%%%%%% ### @`.`@@@@@ Creation Date: -%%%%%% ### @@@@@ 09/10/2019 -%%%%%% /-\ @@ -%%%%%% | | %% Authors: -%%%%%% \-/## %%%%% Kentaro Uno -%%%%%% #### %%% unoken@astro.mech.tohoku.ac.jp -%%%%%% ###%% * -%%%%%% ##%% ***** -%%%%%% #%% *** -%%%%%% %% * * -%%%%%% %%% -%%%%%% %%%%% -%%%%%% %% -%%%%%%--------------------------------------------------------------------- -%%%%%% -%%%%%% Two Dimensional Reachable Area Visualization Simulation Code for Leg -%%%%%% Designing -%%%%%% -%%%%%%--------------------------------------------------------------------- -%%%%%% -%%%%%% Created 2019-09-10 by Kentaro Uno -%%%%%% Last updated 2020-06-21 by Kentaro Uno -%%%%%% -%%%%%%--------------------------------------------------------------------- - -clc; clear; close all; -tic - -% figure number -fig_num = 1; -addpath(genpath('../..')); -addpath(genpath('../LP_design')); -addpath(genpath('../tools')); - -%% -%%%%%%%%%%%%%%%%%%%%%%%% -%%% CLiMBot testbed %%% -%%%%%%%%%%%%%%%%%%%%%%%% -robot_name = "CLiMBot"; -%%% Link parameters loading %%% -LP = LP_to_design_CLiMBot_v3(); -%%% initialize state variables %%% -SV = ini_12DOF_SV( LP ); - -%%% rotate CCW the joint 1 to get along with y axis of the base %%% -%rotAngleForJoint1ForCLiMBot = atan(0.5); % unit: radian -rotAngleForJoint1ForCLiMBot = 45*pi/180; % unit: radian -joint1 = rotAngleForJoint1ForCLiMBot; -%%% set the limitation of motion of joint2 and 3 %%% -joint2.max = 0; % unit: degree -joint2.min = -90; % unit: degree -joint3.max = 135; % unit: degree -joint3.min = 0; % unit: degree -%%% set the visualization color %%% -color = 'r'; - -%%% Reachable area parameter %%% -drawReachableArea( LP, SV, joint1 , joint2 , joint3 , color , fig_num, robot_name ); -view(0,0) % view from z axis direction i.e. 2D visualization - -%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% HubRobo with gripper %%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%% -robot_name = "HubRobo with gripper"; -%%% Link parameters loading %%% -LP = ini_HubRobo_grip_LP(); -%%% initialize state variables %%% -SV = ini_12DOF_SV( LP ); - -%%% rotate CCW the joint 1 to get along with y axis of the base %%% -rotAngleForJoint1ForOldTesbed = -45*pi/180; % unit: radian -joint1 = rotAngleForJoint1ForOldTesbed; -%%% set the limitation of motion of joint2 and 3 %%% -joint2.max = 90; % unit: degree -joint2.min = 0; % unit: degree -joint3.max = 0; % unit: degree -joint3.min = -135; % unit: degree -%%% set the visualization color %%% -color = 'b'; - -%%% Reachable area parameter %%% -drawReachableArea( LP, SV, joint1 , joint2 , joint3 , color , fig_num, robot_name ); -view(0,0) % view from z axis direction i.e. 2D visualization - - -%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% HubRobo with link extention %%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -robot_name = "HubRobo new design"; -%%% Link parameters loading %%% -LP = LP_to_design_HubRobo(); -%%% initialize state variables %%% -SV = ini_12DOF_SV( LP ); - -%%% rotate CCW the joint 1 to get along with y axis of the base %%% -rotAngleForJoint1ForOldTesbed = -45*pi/180; % unit: radian -joint1 = rotAngleForJoint1ForOldTesbed; -%%% set the limitation of motion of joint2 and 3 %%% -joint2.max = 90; % unit: degree -joint2.min = 0; % unit: degree -joint3.max = 0; % unit: degree -joint3.min = -135; % unit: degree -%%% set the visualization color %%% -color = 'g'; - -%%% Reachable area parameter %%% -drawReachableArea( LP, SV, joint1 , joint2 , joint3 , color , fig_num, robot_name ); -view(0,0) % view from z axis direction i.e. 2D visualization - - +%%%%%%--------------------------------------------------------------------- +%%%%%% main_LP_2D_analysis.m +%%%%%%--------------------------------------------------------------------- +%%%%%% @@ +%%%%%% @@@@@@ +%%%%%% @```@@@@ +%%%%%% @` `@@@@@@ +%%%%%% @@` `@@@@@@@@ +%%%%%% @@` `@@@@@@@@@ Tohoku University +%%%%%% @` ` `@@@@@@@@@ SPACE ROBOTICS LABORATORY +%%%%%% @`` ## `@@@@@@@@@ http://www.astro.mech.tohoku.ac.jp/ +%%%%%% @` #..#`@@@@@@@@@ +%%%%%% @` #..#`@@@@@@@@@ +%%%%%% @` ### `@@@@@@@@@ Professor Kazuya Yoshida +%%%%%% @` ###``@@@@@@@@@ Assistant Professor Kenji Nagaoka +%%%%%% @### ``@@@@@@@@ +%%%%%% ###` `@@@@@@@ +%%%%%% ### @`.`@@@@@ Creation Date: +%%%%%% ### @@@@@ 09/10/2019 +%%%%%% /-\ @@ +%%%%%% | | %% Authors: +%%%%%% \-/## %%%%% Kentaro Uno +%%%%%% #### %%% unoken@astro.mech.tohoku.ac.jp +%%%%%% ###%% * +%%%%%% ##%% ***** +%%%%%% #%% *** +%%%%%% %% * * +%%%%%% %%% +%%%%%% %%%%% +%%%%%% %% +%%%%%%--------------------------------------------------------------------- +%%%%%% +%%%%%% Two Dimensional Reachable Area Visualization Simulation Code for Leg +%%%%%% Designing +%%%%%% +%%%%%%--------------------------------------------------------------------- +%%%%%% +%%%%%% Created 2019-09-10 +%%%%%% Kentaro Uno +%%%%%% Last updated: 2021-04-11 +%%%%%% Kentaro Uno +%%%%%% +%%%%%%--------------------------------------------------------------------- + +clc; clear; close all; +tic + +% figure number +fig_num = 1; +addpath(genpath('../../lib')); +addpath(genpath('../LP_to_design')); +addpath(genpath('../tools')); + +ini_path(); +% %% +% %%%%%%%%%%%%%%%%%%%%%%%% +% %%% CLiMBot testbed %%% +% %%%%%%%%%%%%%%%%%%%%%%%% +% robot_name = "CLiMBot"; +% %%% Link parameters loading %%% +% LP = LP_to_design_CLiMBot_v3(); +% %%% initialize state variables %%% +% SV = ini_12DOF_SV( LP ); +% +% %%% rotate CCW the joint 1 to get along with y axis of the base %%% +% %rotAngleForJoint1ForCLiMBot = atan(0.5); % unit: radian +% rotAngleForJoint1ForCLiMBot = 45*pi/180; % unit: radian +% joint1 = rotAngleForJoint1ForCLiMBot; +% %%% set the limitation of motion of joint2 and 3 %%% +% joint2.max = 0; % unit: degree +% joint2.min = -90; % unit: degree +% joint3.max = 135; % unit: degree +% joint3.min = 0; % unit: degree +% %%% set the visualization color %%% +% color = 'r'; +% +% %%% Reachable area parameter %%% +% drawReachableArea( LP, SV, joint1 , joint2 , joint3 , color , fig_num, robot_name ); +% view(0,0) % view from z axis direction i.e. 2D visualization + +%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% HubRobo with gripper %%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%% +robot_name = "HubRobo V3"; +%%% Link parameters loading %%% +LP = ini_HubRobo_v3_2_grip_to_palm_LP(); +%%% initialize state variables %%% +SV = ini_12DOF_SV( LP ); + +%%% rotate CCW the joint 1 to get along with y axis of the base %%% +rotAngleForJoint1ForOldTesbed = -45*pi/180; % unit: radian +joint1 = rotAngleForJoint1ForOldTesbed; +%%% set the limitation of motion of joint2 and 3 %%% +joint2.max = 90; % unit: degree +joint2.min = -60; % unit: degree +joint3.max = 0; % unit: degree +joint3.min = -160; % unit: degree +%%% set the visualization color %%% +color = 'b'; + +%%% Reachable area parameter %%% +drawReachableArea( LP, SV, joint1 , joint2 , joint3 , color , fig_num, robot_name ); +view(0,0) % view from z axis direction i.e. 2D visualization + + +%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% HubRobo with link extention %%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +robot_name = "HubRobo V2"; +%%% Link parameters loading %%% +LP = ini_HubRobo_v2_2_grip_to_palm_LP(); +%%% initialize state variables %%% +SV = ini_12DOF_SV( LP ); + +%%% rotate CCW the joint 1 to get along with y axis of the base %%% +rotAngleForJoint1ForOldTesbed = -45*pi/180; % unit: radian +joint1 = rotAngleForJoint1ForOldTesbed; +%%% set the limitation of motion of joint2 and 3 %%% +joint2.max = 90; % unit: degree +joint2.min = 0; % unit: degree +joint3.max = 0; % unit: degree +joint3.min = -120; % unit: degree +%%% set the visualization color %%% +color = 'r'; + +%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Alphled with link extention %%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +robot_name = "Alphred"; +%%% Link parameters loading %%% +LP = ini_ALPHRED_LP(); +%%% initialize state variables %%% +SV = ini_12DOF_SV( LP ); + +%%% rotate CCW the joint 1 to get along with y axis of the base %%% +rotAngleForJoint1ForOldTesbed = -45*pi/180; % unit: radian +joint1 = rotAngleForJoint1ForOldTesbed; +%%% set the limitation of motion of joint2 and 3 %%% +joint2.max = 135; % unit: degree +joint2.min = -90; % unit: degree +joint3.max = 165; % unit: degree +joint3.min = -165; % unit: degree +%%% set the visualization color %%% +color = 'r'; + + +%%% Reachable area parameter %%% +drawReachableArea( LP, SV, joint1 , joint2 , joint3 , color , fig_num, robot_name ); +view(0,0) % view from z axis direction i.e. 2D visualization + + %%% EOF %%% \ No newline at end of file diff --git a/src/robot/LP_analysis/sample_3D_main.m b/src/robot/LP_analysis/main_LP_3D_analysis.m similarity index 94% rename from src/robot/LP_analysis/sample_3D_main.m rename to src/robot/LP_analysis/main_LP_3D_analysis.m index 06c0af6..0d0c79f 100644 --- a/src/robot/LP_analysis/sample_3D_main.m +++ b/src/robot/LP_analysis/main_LP_3D_analysis.m @@ -1,166 +1,166 @@ -%%%%%%--------------------------------------------------------------------- -%%%%%% sample_3D_main.m -%%%%%%--------------------------------------------------------------------- -%%%%%% @@ -%%%%%% @@@@@@ -%%%%%% @```@@@@ -%%%%%% @` `@@@@@@ -%%%%%% @@` `@@@@@@@@ -%%%%%% @@` `@@@@@@@@@ Tohoku University -%%%%%% @` ` `@@@@@@@@@ SPACE ROBOTICS LABORATORY -%%%%%% @`` ## `@@@@@@@@@ http://www.astro.mech.tohoku.ac.jp/ -%%%%%% @` #..#`@@@@@@@@@ -%%%%%% @` #..#`@@@@@@@@@ -%%%%%% @` ### `@@@@@@@@@ Professor Kazuya Yoshida -%%%%%% @` ###``@@@@@@@@@ Assistant Professor Kenji Nagaoka -%%%%%% @### ``@@@@@@@@ -%%%%%% ###` `@@@@@@@ -%%%%%% ### @`.`@@@@@ Creation Date: -%%%%%% ### @@@@@ 10/02/2019 -%%%%%% /-\ @@ -%%%%%% | | %% Authors: -%%%%%% \-/## %%%%% Kentaro Uno, Naomasa Takada -%%%%%% #### %%% unoken@astro.mech.tohoku.ac.jp -%%%%%% ###%% * -%%%%%% ##%% ***** -%%%%%% #%% *** -%%%%%% %% * * -%%%%%% %%% -%%%%%% %%%%% -%%%%%% %% -%%%%%%--------------------------------------------------------------------- -%%%%%% -%%%%%% Three Dimensional Reachable Area Visualization Simulation Code for -%%%%%% Leg Designing -%%%%%% -%%%%%%--------------------------------------------------------------------- -%%%%%% -%%%%%% Created 2019-09-10 by Kentaro Uno -%%%%%% Last updated 2020-06-21 by Kentaro Uno -%%%%%% -%%%%%%--------------------------------------------------------------------- - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% 1.The clc function clears all inputs and outputs from the command window -% display. -% 2.The clear function deletes items from the workspace and releases system -% memory. -% 3.The close all function deletes all Figures whose handles are not -% hidden. -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -clc -clear -close all - -floor = 0; % height from the floor to the robot base [m] -inc = 0; % inclination angle of the ground plane [deg/rad?] - -%%%%%%%% Defining link parameters and initializing state variables %%%%%%%% -% 1.Acquire the defined link parameters with LP definition file such as LP_climbot_v3(). -% 2.Initialize state variables with SV(). -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -addpath(genpath('../..')); -addpath(genpath('../LP_design')); - -%% - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%% Example of old CLiMBot %%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% % @TODO: change the LP wrt the new joint allocation -% robot_name = "CLiMBot"; -% LP = LP_to_design_CLiMBot_v3(); -% LP.num_limb = 4; -% %%% rotate CCW the joint 1 to get along with y axis of the base %%% -% rotAngleForJoint1ForOldTesbed = 45*pi/180; % unit: radian -% joint1 = rotAngleForJoint1ForOldTesbed; -% %%% set the limitation of motion of joint2 and 3 %%% -% joint2.max = 0; % unit: degree -% joint2.min = -90; % unit: degree -% joint3.max = 135; % unit: degree -% joint3.min = 0; % unit: degree -% %%% set the visualization color %%% -% color = 'r' -% -% %%% initialize state variables %%% -% SV = ini_12DOF_SV( LP ); - -%% - -% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -% %%%%%%%%%%%%%%%%%%%%%%% Example of HubRobo %%%%%%%%%%%%%%%%%%%%%%%%%% -% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -robot_name = "HubRobo with gripper"; -% LP = ini_HubRobo_grip_LP(); -LP = LP_to_design_HubRobo(); -LP.num_limb = 4; -%%% rotate CCW the joint 1 to get along with y axis of the base %%% -rotAngleForJoint1ForOldTesbed = 45*pi/180; % unit: radian -joint1 = rotAngleForJoint1ForOldTesbed; -%%% set the limitation of motion of joint2 and 3 %%% -joint2.max = 90; % unit: degree -joint2.min = 0; % unit: degree -joint3.max = 0; % unit: degree -joint3.min = -135; % unit: degree -%%% set the visualization color %%% -color = 'r' - -%%% initialize state variables %%% -SV = ini_12DOF_SV( LP ); - -%% - -%%%%%%%%%%%%%%%%%%%%%%%% To use defined function %%%%%%%%%%%%%%%%%%%%%%%%%% - -% SV = SV( LP ); -SV.q = zeros(12,1); - -SV.q = [ 0.0; 0.0; -1.5708; - 0.0; 0.0; -1.5708; - 0.0; 0.0; -1.5708; - 0.0; 0.0; -1.5708 ]; - -SV.q(1) = joint1; -%%% Acquisition of SV.AA -SV = calc_aa( LP, SV ); -%%% Acquisition of SV.RR -SV = calc_pos( LP, SV ); - -%%% Calculate of hand positions (we dont care orientations, which is not -%%% stored in the following scripts ) -[ POS_e(:,1), ORI_e ] = f_kin_e( LP, SV, 3); -[ POS_e(:,2), ORI_e ] = f_kin_e( LP, SV, 6); -[ POS_e(:,3), ORI_e ] = f_kin_e( LP, SV, 9); -[ POS_e(:,4), ORI_e ] = f_kin_e( LP, SV, 12); -%%% Calculate of joints position and posture -[ POS_j, ORI_j ] = f_kin_j( LP, SV, [1 2 3 4 5 6 7 8 9 10 11 12 ] ); -floor = POS_e(3,1); - -%% - -%%% set the number of the figure -fig_num = 1; - -%%% Use of the defined function -drawRobotWireFrame( LP, SV, floor, POS_e, POS_j, inc, fig_num ); - -%%% visualize the reachable region -addpath(genpath('../tools')); -drawReachableArea( LP, SV, joint1, joint2, joint3, color, fig_num, robot_name ); - -%%% matlab figure setting -title('Reachable region (origin: CoM of robot)'); -xlabel('x [m]'); -ylabel('y [m]'); -zlabel('z [m]'); -xlim([-0.4 0.4]); -ylim([-0.4 0.4]); -zlim([-0.20 0.2]); -daspect([1 1 1]) % set grid is standardized -grid on; -hold on; -legend("off"); - - +%%%%%%--------------------------------------------------------------------- +%%%%%% main_LP_3D_analysis.m +%%%%%%--------------------------------------------------------------------- +%%%%%% @@ +%%%%%% @@@@@@ +%%%%%% @```@@@@ +%%%%%% @` `@@@@@@ +%%%%%% @@` `@@@@@@@@ +%%%%%% @@` `@@@@@@@@@ Tohoku University +%%%%%% @` ` `@@@@@@@@@ SPACE ROBOTICS LABORATORY +%%%%%% @`` ## `@@@@@@@@@ http://www.astro.mech.tohoku.ac.jp/ +%%%%%% @` #..#`@@@@@@@@@ +%%%%%% @` #..#`@@@@@@@@@ +%%%%%% @` ### `@@@@@@@@@ Professor Kazuya Yoshida +%%%%%% @` ###``@@@@@@@@@ Assistant Professor Kenji Nagaoka +%%%%%% @### ``@@@@@@@@ +%%%%%% ###` `@@@@@@@ +%%%%%% ### @`.`@@@@@ Creation Date: +%%%%%% ### @@@@@ 10/02/2019 +%%%%%% /-\ @@ +%%%%%% | | %% Authors: +%%%%%% \-/## %%%%% Kentaro Uno, Naomasa Takada +%%%%%% #### %%% unoken@astro.mech.tohoku.ac.jp +%%%%%% ###%% * +%%%%%% ##%% ***** +%%%%%% #%% *** +%%%%%% %% * * +%%%%%% %%% +%%%%%% %%%%% +%%%%%% %% +%%%%%%--------------------------------------------------------------------- +%%%%%% +%%%%%% Three Dimensional Reachable Area Visualization Simulation Code for +%%%%%% Leg Designing +%%%%%% +%%%%%%--------------------------------------------------------------------- +%%%%%% +%%%%%% Created 2019-09-10 by Kentaro Uno +%%%%%% Last updated 2021-02-02 by Keigo Haji +%%%%%% +%%%%%%--------------------------------------------------------------------- + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 1.The clc function clears all inputs and outputs from the command window +% display. +% 2.The clear function deletes items from the workspace and releases system +% memory. +% 3.The close all function deletes all Figures whose handles are not +% hidden. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +clc +clear +close all + +floor = 0; % height from the floor to the robot base [m] +inc = 0; % inclination angle of the ground plane [deg/rad?] + +%%%%%%%% Defining link parameters and initializing state variables %%%%%%%% +% 1.Acquire the defined link parameters with LP definition file such as LP_climbot_v3(). +% 2.Initialize state variables with SV(). +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +addpath(genpath('../..')); +addpath(genpath('../LP_to_design')); + +%% + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%% Example of old CLiMBot %%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% % @TODO: change the LP wrt the new joint allocation +% robot_name = "CLiMBot"; +% LP = LP_to_design_CLiMBot_v3(); +% LP.num_limb = 4; +% %%% rotate CCW the joint 1 to get along with y axis of the base %%% +% rotAngleForJoint1ForOldTesbed = 45*pi/180; % unit: radian +% joint1 = rotAngleForJoint1ForOldTesbed; +% %%% set the limitation of motion of joint2 and 3 %%% +% joint2.max = 0; % unit: degree +% joint2.min = -90; % unit: degree +% joint3.max = 135; % unit: degree +% joint3.min = 0; % unit: degree +% %%% set the visualization color %%% +% color = 'r' +% +% %%% initialize state variables %%% +% SV = ini_12DOF_SV( LP ); + +%% + +% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% %%%%%%%%%%%%%%%%%%%%%%% Example of HubRobo %%%%%%%%%%%%%%%%%%%%%%%%%% +% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +robot_name = "HubRobo with gripper"; +% LP = ini_HubRobo_grip_LP(); +LP = LP_to_design_HubRobo(); +LP.num_limb = 4; +%%% rotate CCW the joint 1 to get along with y axis of the base %%% +rotAngleForJoint1ForOldTesbed = 45*pi/180; % unit: radian +joint1 = rotAngleForJoint1ForOldTesbed; +%%% set the limitation of motion of joint2 and 3 %%% +joint2.max = 90; % unit: degree +joint2.min = 0; % unit: degree +joint3.max = 0; % unit: degree +joint3.min = -135; % unit: degree +%%% set the visualization color %%% +color = 'r' + +%%% initialize state variables %%% +SV = ini_12DOF_SV( LP ); + +%% + +%%%%%%%%%%%%%%%%%%%%%%%% To use defined function %%%%%%%%%%%%%%%%%%%%%%%%%% + +% SV = SV( LP ); +SV.q = zeros(12,1); + +SV.q = [ 0.0; 0.0; -1.5708; + 0.0; 0.0; -1.5708; + 0.0; 0.0; -1.5708; + 0.0; 0.0; -1.5708 ]; + +SV.q(1) = joint1; +%%% Acquisition of SV.AA +SV = calc_aa( LP, SV ); +%%% Acquisition of SV.RR +SV = calc_pos( LP, SV ); + +%%% Calculate of hand positions (we dont care orientations, which is not +%%% stored in the following scripts ) +[ POS_e(:,1), ORI_e ] = f_kin_e( LP, SV, 3); +[ POS_e(:,2), ORI_e ] = f_kin_e( LP, SV, 6); +[ POS_e(:,3), ORI_e ] = f_kin_e( LP, SV, 9); +[ POS_e(:,4), ORI_e ] = f_kin_e( LP, SV, 12); +%%% Calculate of joints position and posture +[ POS_j, ORI_j ] = f_kin_j( LP, SV, [ 1 2 3 4 5 6 7 8 9 10 11 12 ] ); +floor = POS_e(3,1); + +%% + +%%% set the number of the figure +fig_num = 1; + +%%% Use of the defined function +drawRobotWireFrame( LP, SV, floor, POS_e, POS_j, inc, fig_num ); + +%%% visualize the reachable region +addpath(genpath('../tools')); +drawReachableArea( LP, SV, joint1, joint2, joint3, color, fig_num, robot_name ); + +%%% matlab figure setting +title('Reachable region (origin: CoM of robot)'); +xlabel('x [m]'); +ylabel('y [m]'); +zlabel('z [m]'); +xlim([-0.4 0.4]); +ylim([-0.4 0.4]); +zlim([-0.20 0.2]); +daspect([1 1 1]) % set grid is standardized +grid on; +hold on; +legend("off"); + + %%% EOF %%% \ No newline at end of file diff --git a/src/robot/LP_analysis/tools/drawReachableArea.m b/src/robot/LP_analysis/tools/drawReachableArea.m index 9ddf260..fc52736 100644 --- a/src/robot/LP_analysis/tools/drawReachableArea.m +++ b/src/robot/LP_analysis/tools/drawReachableArea.m @@ -118,17 +118,17 @@ hold on; % visualize the reachable area on the z-xy plane -plot3(POS_e1_tmp1(:,1),POS_e1_tmp1(:,2),POS_e1_tmp1(:,3), color, ... - POS_e1_tmp2(:,1),POS_e1_tmp2(:,2),POS_e1_tmp2(:,3), color, ... - POS_e1_tmp3(:,1),POS_e1_tmp3(:,2),POS_e1_tmp3(:,3), color, ... - POS_e1_tmp4(:,1),POS_e1_tmp4(:,2),POS_e1_tmp4(:,3), color, 'DisplayName', robot_name) +plot3(POS_e1_tmp1(:,1)-LP.c0(1,1),POS_e1_tmp1(:,2),POS_e1_tmp1(:,3), color, ... + POS_e1_tmp2(:,1)-LP.c0(1,1),POS_e1_tmp2(:,2),POS_e1_tmp2(:,3), color, ... + POS_e1_tmp3(:,1)-LP.c0(1,1),POS_e1_tmp3(:,2),POS_e1_tmp3(:,3), color, ... + POS_e1_tmp4(:,1)-LP.c0(1,1),POS_e1_tmp4(:,2),POS_e1_tmp4(:,3), color, 'DisplayName', robot_name) % matlab figure settings grid on; daspect([1 1 1]) % set grid is standardized xlabel('x (origin: CoM of robot)') zlabel('z (origin: CoM of robot)') -xlim([ 0 , 0.5]); -ylim([-0.15 , 0.25]); +xlim([ -0.05 , 0.35]); +ylim([-0.3 , 0.3]); legend hold on; diff --git a/src/robot/ini_ALPHRED_LP.m b/src/robot/ini_ALPHRED_LP.m new file mode 100644 index 0000000..6cf60f5 --- /dev/null +++ b/src/robot/ini_ALPHRED_LP.m @@ -0,0 +1,168 @@ +%%%%%% Initialize +%%%%%% ini_ALPHRED_LP +%%%%%% +%%%%%% Mass/inertia parameters and link connections for ALPHRED +%%%%%% +%%%%%% NOTE: Parameters are based on the following publications: +%%%%%% [1] ALPHRED: A Multi-Modal Operations Quadruped Robot for Package Delivery Applications +%%%%%% https://ieeexplore.ieee.org/document/9134727 +%%%%%% [2] Implementation of a Versatile 3D ZMP Trajectory Optimization Algorithm on a Multi-Modal Legged Robotic Platform +%%%%%% https://ieeexplore.ieee.org/document/8593968 +%%%%%% +%%%%%% Created on 2021-03-09 by Keigo Haji +%%%%%% Last updated on 2021-03-09 by Keigo Haji + +function LP = ini_ALPHRED_LP() + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force +LP.F_grip = 0.0; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Define link connection relationscoxa +LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; + +% Number of links/joints +LP.num_q = length(LP.BB); + +LP.SS = [ -1 1 0 0 0 0 0 0 0 0 0 0; + 0 -1 1 0 0 0 0 0 0 0 0 0; + 0 0 -1 0 0 0 0 0 0 0 0 0; + 0 0 0 -1 1 0 0 0 0 0 0 0; + 0 0 0 0 -1 1 0 0 0 0 0 0; + 0 0 0 0 0 -1 0 0 0 0 0 0; + 0 0 0 0 0 0 -1 1 0 0 0 0; + 0 0 0 0 0 0 0 -1 1 0 0 0; + 0 0 0 0 0 0 0 0 -1 0 0 0; + 0 0 0 0 0 0 0 0 0 -1 1 0; + 0 0 0 0 0 0 0 0 0 0 -1 1; + 0 0 0 0 0 0 0 0 0 0 0 -1]; +% Links connected to base (link 0) +LP.S0 = [ 1 0 0 1 0 0 1 0 0 1 0 0]; +% End link (leg) +LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; +% Type of joint +LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint <- based on [1] +LP.joint_limit=[-90 90; -90 135; -165 165]; %[deg] + +% Distance from base CoM to Coxa joint <- based on [2] +baseCoM2Coxa = 0.08599; %[m] +% Position vector from base CoM to i-th joint +LP.c0 = [ baseCoM2Coxa/sqrt(2) 0 0 -baseCoM2Coxa/sqrt(2) 0 0 -baseCoM2Coxa/sqrt(2) 0 0 baseCoM2Coxa/sqrt(2) 0 0; + baseCoM2Coxa/sqrt(2) 0 0 baseCoM2Coxa/sqrt(2) 0 0 -baseCoM2Coxa/sqrt(2) 0 0 -baseCoM2Coxa/sqrt(2) 0 0; + 0.000 0 0 0.000 0 0 0.000 0 0 0.000 0 0]; + +% Mass of the base (link 0) <- based on [1] +LP.m0 = 13.58; % [kg] + +% Moment of inertia of base <- based on [1] +LP.inertia0 = [ 0.288 0.000 0.000 ; % [kg*m^2] + 0.000 0.474 0.000 ; + 0.000 0.000 0.287 ]; + +% Total mass <- based on [1] +LP.mass = 17.55; % [kg] + +% Mass & Length & Radius of each link +% For radiuses, there is no spec. in [1][2], but we need the radiuses to +% calculate the inertia tensor of each limb. So, we set the radiuses of +% each link approximately. +FemurLinkMass = 0.463; %[kg] <- based on [1] +FemurLinkLength = 0.350; %[m] <- based on [1] +FemurLinkRadius = 0.029; %[m] <- approximately + +TibiaLinkMass = 0.495; %[kg] <- based on [1] +TibiaLinkLength = 0.350; %[m] <- based on [1] +TibiaLinkRadius = 0.01167; %[m] <- approximately + +CoxaLinkMass = (LP.mass - ((FemurLinkMass + TibiaLinkMass)*4 + LP.m0))/4; %<- approximately +CoxaLinkLength = 0.09778; %[m] <- based on [2] +CoxaLinkRadius = 0.70; %[m] <- approximately + +% Mass of each link +LP.m = [ CoxaLinkMass FemurLinkMass TibiaLinkMass ... + CoxaLinkMass FemurLinkMass TibiaLinkMass ... + CoxaLinkMass FemurLinkMass TibiaLinkMass ... + CoxaLinkMass FemurLinkMass TibiaLinkMass ]; % [kg] + +% note that z axis is always rotation axis, and x axis is always towards +% the EE in ClimbLab. So, for the links, the axial direction is x, and +% Radial directions are y and z. +% For LF(Left Front) limb, +inertiaTensorForLinks.LF.coxa.Ixx = 1/2 * CoxaLinkMass * CoxaLinkRadius^2; +inertiaTensorForLinks.LF.coxa.Iyy = 1/4 * CoxaLinkMass * CoxaLinkRadius^2 + 1/12 * CoxaLinkMass * CoxaLinkLength^2; +inertiaTensorForLinks.LF.coxa.Izz = inertiaTensorForLinks.LF.coxa.Iyy; +inertiaTensorForLinks.LF.femur.Ixx = 1/2 * FemurLinkMass * FemurLinkRadius^2; +inertiaTensorForLinks.LF.femur.Iyy = 1/4 * FemurLinkLength * FemurLinkRadius^2 + 1/12 * FemurLinkLength * FemurLinkLength^2; +inertiaTensorForLinks.LF.femur.Izz = inertiaTensorForLinks.LF.coxa.Iyy; +inertiaTensorForLinks.LF.tibia.Ixx = 1/2 * TibiaLinkMass * TibiaLinkRadius^2; +inertiaTensorForLinks.LF.tibia.Iyy = 1/4 * TibiaLinkMass * TibiaLinkRadius^2 + 1/12 * TibiaLinkMass * TibiaLinkLength^2; +inertiaTensorForLinks.LF.tibia.Izz = inertiaTensorForLinks.LF.coxa.Iyy; +% all limb has the same inertial spec. +inertiaTensorForLinks.LH = inertiaTensorForLinks.LF; +inertiaTensorForLinks.RF = inertiaTensorForLinks.LF; +inertiaTensorForLinks.RH = inertiaTensorForLinks.LF; + +% Moment of inertia of each link % [kg*m^2] +LP.inertia = [ inertiaTensorForLinks.LF.coxa.Ixx 0 0 inertiaTensorForLinks.LF.femur.Ixx 0 0 inertiaTensorForLinks.LF.tibia.Ixx 0 0 inertiaTensorForLinks.LH.coxa.Ixx 0 0 inertiaTensorForLinks.LH.femur.Ixx 0 0 inertiaTensorForLinks.LH.tibia.Ixx 0 0 inertiaTensorForLinks.RH.coxa.Ixx 0 0 inertiaTensorForLinks.RH.femur.Ixx 0 0 inertiaTensorForLinks.RH.tibia.Ixx 0 0 inertiaTensorForLinks.RF.coxa.Ixx 0 0 inertiaTensorForLinks.RF.femur.Ixx 0 0 inertiaTensorForLinks.RF.tibia.Ixx 0 0 ; + 0 inertiaTensorForLinks.LF.coxa.Iyy 0 0 inertiaTensorForLinks.LF.femur.Iyy 0 0 inertiaTensorForLinks.LF.tibia.Iyy 0 0 inertiaTensorForLinks.LH.coxa.Iyy 0 0 inertiaTensorForLinks.LH.femur.Iyy 0 0 inertiaTensorForLinks.LH.tibia.Iyy 0 0 inertiaTensorForLinks.RH.coxa.Iyy 0 0 inertiaTensorForLinks.RH.femur.Iyy 0 0 inertiaTensorForLinks.RH.tibia.Iyy 0 0 inertiaTensorForLinks.RF.coxa.Iyy 0 0 inertiaTensorForLinks.RF.femur.Iyy 0 0 inertiaTensorForLinks.RF.tibia.Iyy 0 ; + 0 0 inertiaTensorForLinks.LF.coxa.Izz 0 0 inertiaTensorForLinks.LF.femur.Izz 0 0 inertiaTensorForLinks.LF.tibia.Izz 0 0 inertiaTensorForLinks.LH.coxa.Izz 0 0 inertiaTensorForLinks.LH.femur.Izz 0 0 inertiaTensorForLinks.LH.tibia.Izz 0 0 inertiaTensorForLinks.RH.coxa.Izz 0 0 inertiaTensorForLinks.RH.femur.Izz 0 0 inertiaTensorForLinks.RH.tibia.Izz 0 0 inertiaTensorForLinks.RF.coxa.Izz 0 0 inertiaTensorForLinks.RF.femur.Izz 0 0 inertiaTensorForLinks.RF.tibia.Izz ]; + +% Rotational relationscoxa of links frames +LP.Qi = [ 0 pi/2 0 0 pi/2 0 0 pi/2 0 0 pi/2 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + pi/4 0 0 pi*3/4 0 0 pi*5/4 0 0 pi*7/4 0 0]; + + +% Position vector from each link CoM to i-th joint +LP.cc = zeros( 3,12,12 ); + +% Postion vector from link 1 CoM to joint 1 +LP.cc(:,1,1) = [ -CoxaLinkLength/2 0 0]'; +% Postion vector from link 1 CoM to joint 2 +LP.cc(:,1,2) = [ CoxaLinkLength/2 0 0 ]'; +LP.cc(:,2,2) = [ -FemurLinkLength/2 0 0 ]'; +LP.cc(:,2,3) = [ FemurLinkLength/2 0 0 ]'; +LP.cc(:,3,3) = [ -TibiaLinkLength/2 0 0 ]'; + +LP.cc(:,4,4) = [ -CoxaLinkLength/2 0 0]'; +LP.cc(:,4,5) = [ CoxaLinkLength/2 0 0 ]'; +LP.cc(:,5,5) = [ -FemurLinkLength/2 0 0 ]'; +LP.cc(:,5,6) = [ FemurLinkLength/2 0 0 ]'; +LP.cc(:,6,6) = [ -TibiaLinkLength/2 0 0 ]'; + +LP.cc(:,7,7) = [ -CoxaLinkLength/2 0 0]'; +LP.cc(:,7,8) = [ CoxaLinkLength/2 0 0 ]'; +LP.cc(:,8,8) = [ -FemurLinkLength/2 0 0 ]'; +LP.cc(:,8,9) = [ FemurLinkLength/2 0 0 ]'; +LP.cc(:,9,9) = [ -TibiaLinkLength/2 0 0 ]'; + +LP.cc(:,10,10) = [ -CoxaLinkLength/2 0 0]'; +LP.cc(:,10,11) = [ CoxaLinkLength/2 0 0 ]'; +LP.cc(:,11,11) = [ -FemurLinkLength/2 0 0 ]'; +LP.cc(:,11,12) = [ FemurLinkLength/2 0 0 ]'; +LP.cc(:,12,12) = [ -TibiaLinkLength/2 0 0 ]'; + + +% Position vector from end link CoM to end point +footLength = 0.02; %[m] <- approximately +TibiaCoM2EE = TibiaLinkLength/2 + footLength; +LP.ce = [ 0 0 TibiaCoM2EE 0 0 TibiaCoM2EE 0 0 TibiaCoM2EE 0 0 TibiaCoM2EE ; + 0 0 0 0 0 0 0 0 0 0 0 0 ; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; +% Rotational relationscoxa of end link frames +LP.Qe = [ 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; + +% EOF \ No newline at end of file diff --git a/src/robot/ini_ANYmal_B_LP.m b/src/robot/ini_ANYmal_B_LP.m new file mode 100644 index 0000000..ab9b262 --- /dev/null +++ b/src/robot/ini_ANYmal_B_LP.m @@ -0,0 +1,147 @@ +%%%%%% Initialize +%%%%%% ini_ANYmal_B_LP +%%%%%% +%%%%%% Mass/inertia parameters and link connections for ANYmal B +%%%%%% +%%%%%% NOTE: all parameters are based on the following URDF file +%%%%%% https://github.com/ANYbotics/anymal_b_simple_description +%%%%%% +%%%%%% Created: 2021-01-18 +%%%%%% Kentaro Uno +%%%%%% Last update: 2021-03-03 +%%%%%% Kentaro Uno + +function LP = ini_ANYmal_B_LP() + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solver is switched +LP.joint_allocation_type = 'mammal'; +LP.leg_config_type = 'xx'; + +% Max. endurable gripping force +LP.F_grip = 200.0; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Define link connection relationship +LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; + +% Number of links/joints +LP.num_q = length(LP.BB); + +LP.SS = [ -1 1 0 0 0 0 0 0 0 0 0 0; + 0 -1 1 0 0 0 0 0 0 0 0 0; + 0 0 -1 0 0 0 0 0 0 0 0 0; + 0 0 0 -1 1 0 0 0 0 0 0 0; + 0 0 0 0 -1 1 0 0 0 0 0 0; + 0 0 0 0 0 -1 0 0 0 0 0 0; + 0 0 0 0 0 0 -1 1 0 0 0 0; + 0 0 0 0 0 0 0 -1 1 0 0 0; + 0 0 0 0 0 0 0 0 -1 0 0 0; + 0 0 0 0 0 0 0 0 0 -1 1 0; + 0 0 0 0 0 0 0 0 0 0 -1 1; + 0 0 0 0 0 0 0 0 0 0 0 -1]; +% Links connected to base (link 0) +LP.S0 = [ 1 0 0 1 0 0 1 0 0 1 0 0]; +% End link (leg) +LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; +% Type of joint +LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-540 540; -540 540; -540 540]; %[deg] + +% Position vector from base CoM to i-th joint. Order is LF_HAA, LF_HFE, +% LF_KFE, LH_HAA, LH_HFE, LH_KFE, RH_HAA, RH_HFE, RH_KFE, RF_HAA, RF_HFE, and RF_KFE. +LP.c0 = [ 0.277 0 0 -0.277 0 0 -0.277 0 0 0.277 0 0; + 0.116 0 0 0.116 0 0 -0.116 0 0 -0.116 0 0; + 0.000 0 0 0.000 0 0 0.000 0 0 0.000 0 0]; +% Mass of the base (link 0) <- copied from URDF file +LP.m0 = 16.793507758; % [kg] +% Moment of inertia of base <- copied from URDF file +LP.inertia0 = [ 0.217391101503 -0.00132873239126 -0.00228200226173 ; % [kg*m^2] + -0.00132873239126 0.639432546734 -0.00138078263145 ; + -0.00228200226173 -0.00138078263145 0.62414077654 ]; + +% Mass of each link <- based on manual measurement +LP.m = [ 1.42462064 1.634976467 0.207204302 + 0.140170767 ... + 1.42462064 1.634976467 0.207204302 + 0.140170767 ... + 1.42462064 1.634976467 0.207204302 + 0.140170767 ... + 1.42462064 1.634976467 0.207204302 + 0.140170767 ]; % [kg] +% Total mass +LP.mass = sum(LP.m) + LP.m0; % [kg] + +% Rotational relationship of links frames +% local variables to define the offset angle theta_1 and theta_2 +LP.theta_1 = 0.0; % [rad] +LP.theta_2 = 0.0; % [rad] + +% so, with the rotation about theta_1 around the z axis, then rotation +% about theta_2+pi/2 around the y' axis, the hip joint frame is obtained. +% However, for the SpaceDyn function, we should describe this in not z-y-x +% euler angle, but in x-y-z euler angle!! +LF_euler_angle_ZYX = [LP.theta_1 pi/2+LP.theta_2 0]; LH_euler_angle_ZYX = [-LP.theta_1 pi/2-LP.theta_2 0]; +RH_euler_angle_ZYX = [LP.theta_1 pi/2-LP.theta_2 0]; RF_euler_angle_ZYX = [-LP.theta_1 pi/2+LP.theta_2 0]; +LF_rotation_matrix = eul2rotm(LF_euler_angle_ZYX,'ZYX'); +LH_rotation_matrix = eul2rotm(LH_euler_angle_ZYX,'ZYX'); +RH_rotation_matrix = eul2rotm(RH_euler_angle_ZYX,'ZYX'); +RF_rotation_matrix = eul2rotm(RF_euler_angle_ZYX,'ZYX'); + +LF_euler_angle_XYZ = rotm2eul(LF_rotation_matrix,'XYZ'); +LH_euler_angle_XYZ = rotm2eul(LH_rotation_matrix,'XYZ'); +RH_euler_angle_XYZ = rotm2eul(RH_rotation_matrix,'XYZ'); +RF_euler_angle_XYZ = rotm2eul(RF_rotation_matrix,'XYZ'); + +LP.Qi = [ LF_euler_angle_XYZ(1) pi/2 0 LH_euler_angle_XYZ(1) pi/2 0 RH_euler_angle_XYZ(1) pi/2 0 RF_euler_angle_XYZ(1) pi/2 0; + LF_euler_angle_XYZ(2) 0 0 LH_euler_angle_XYZ(2) 0 0 RH_euler_angle_XYZ(2) 0 0 RF_euler_angle_XYZ(2) 0 0; + LF_euler_angle_XYZ(3) 0 0 LH_euler_angle_XYZ(3) 0 0 RH_euler_angle_XYZ(3) 0 0 RF_euler_angle_XYZ(3) 0 0]; + +% Moment of inertia of each link <- copied from hubrobo_v3_2_parameters.dat @TODO: current each last link inertial matrixes are replaced with the ones of Tibia links, and the coodinates are based on the URDF ones. These should be adjusted for SpaceDyn one.% [kg*m^2] +% @TODO: current value is just copied from HubRobo +LP.inertia = [ 4.17250591050379E-05 -3.280237461361E-07 -3.28017174197894E-07 1.53134966250451E-05 1.56836205467462E-14 3.77019535347995E-15 2.20258150957412E-05 -3.2869671194301E-07 -3.31326402019975E-07 4.1725E-05 3.3003E-07 -3.3004E-07 1.53134966258197E-05 1.55870791195499E-14 4.18826057628801E-14 2.20258136176618E-05 -3.28696725826472E-07 -3.31325909357267E-07 4.17250600316099E-05 -3.28023694988862E-07 -3.28016569372976E-07 1.53134966250456E-05 1.56835951357578E-14 3.7701969258325E-15 2.20258152259854E-05 -3.28696694818991E-07 -3.31326354131817E-07 4.1725054229097E-05 3.30034360628209E-07 -3.30041495547628E-07 1.531349662582E-05 1.5587029991639E-14 4.18826171023906E-14 4.18826171023906E-14 -1.5341346770838E-14 3.67324197174481E-05; + -3.280237461361E-07 2.75916233673388E-05 6.28753118367121E-07 1.56836205467462E-14 2.40171123385755E-05 6.33438629632886E-16 -3.2869671194301E-07 1.76456846672338E-05 -3.14370002187497E-07 3.3003E-07 2.7592E-05 -6.1624E-07 1.55870791195499E-14 2.40171123385758E-05 -1.53413473909909E-14 -3.28696725826472E-07 1.76456831216546E-05 -3.14370065697366E-07 -3.28023694988862E-07 2.75916244818459E-05 6.28753097529483E-07 1.56835951357578E-14 2.40171123385754E-05 6.33432116769803E-16 -3.28696694818991E-07 1.76456848327505E-05 -3.14370044226523E-07 3.30034360628209E-07 2.75916215197804E-05 -6.16243771991015E-07 1.5587029991639E-14 2.40171123385757E-05 -1.5341346770838E-14 -3.2869672582661E-07 1.76456831216548E-05 -3.14370065697396E-07; + -3.28017174197894E-07 6.28753118367121E-07 2.75915908175676E-05 3.77019535347995E-15 6.33438629632886E-16 3.67324197182656E-05 -3.31326402019975E-07 -3.14370002187497E-07 1.19815307988945E-05 -3.3004E-07 -6.1624E-07 2.7592E-05 4.18826057628801E-14 -1.53413473909909E-14 3.67324197174477E-05 -3.31325909357267E-07 -3.14370065697366E-07 1.19815307394935E-05 -3.28016569372976E-07 6.28753097529483E-07 2.75915909936714E-05 3.7701969258325E-15 6.33432116769803E-16 3.67324197182661E-05 -3.31326354131817E-07 -3.1437E-07 1.19815308159254E-05 -3.30041495547628E-07 -6.16243771991015E-07 2.759158797365E-05 4.18826171023906E-14 -1.5341346770838E-14 3.67324197174481E-05 -3.31325909357463E-07 -3.14370065697396E-07 1.19815307394936E-05]; + +% Position vector from each link CoM to i-th joint +LP.cc = zeros( 3,12,12 ); + +% Postion vector from link 1 CoM to joint 1 seen from the joint 1 frame +LP.cc(:,1,1) = [ 0 -0.041/2 -0.0635/2 ]'; +% Postion vector from link 1 CoM to joint 2 +LP.cc(:,1,2) = [ 0 0.041/2 0.0635/2 ]'; +LP.cc(:,2,2) = [ -0.25/2 0 0.109/2 ]'; +LP.cc(:,2,3) = [ 0.25/2 0 -0.109/2 ]'; +LP.cc(:,3,3) = [ -0.32125/2 -0.1/2 0 ]'; + +LP.cc(:,4,4) = [ 0 -0.041/2 0.0635/2 ]'; +LP.cc(:,4,5) = [ 0 0.041/2 -0.0635/2 ]'; +LP.cc(:,5,5) = [ -0.25/2 0 0.109/2 ]'; +LP.cc(:,5,6) = [ 0.25/2 0 -0.109/2 ]'; +LP.cc(:,6,6) = [ -0.32125/2 0.1/2 0 ]'; + +LP.cc(:,7,7) = [ 0 0.041/2 0.0635/2 ]'; +LP.cc(:,7,8) = [ 0 -0.041/2 -0.0635/2 ]'; +LP.cc(:,8,8) = [ -0.25/2 0 -0.109/2 ]'; +LP.cc(:,8,9) = [ 0.25/2 0 0.109/2 ]'; +LP.cc(:,9,9) = [ -0.32125/2 0.1/2 0 ]'; + +LP.cc(:,10,10) = [ 0 0.041/2 -0.0635/2 ]'; +LP.cc(:,10,11) = [ 0 -0.041/2 0.0635/2 ]'; +LP.cc(:,11,11) = [ -0.25/2 0 -0.109/2 ]'; +LP.cc(:,11,12) = [ 0.25/2 0 0.109/2 ]'; +LP.cc(:,12,12) = [ -0.32125/2 -0.1/2 0 ]'; + +% Position vector from end link CoM to end point +LP.ce = [ 0 0 0.32125/2 0 0 0.32125/2 0 0 0.32125/2 0 0 0.32125/2 ; + 0 0 0.1/2 0 0 -0.1/2 0 0 -0.1/2 0 0 0.1/2 ; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; +% Rotational relationship of end link frames +LP.Qe = [ 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; + +% EOF \ No newline at end of file diff --git a/src/robot/ini_Climbing_ANYmal_LP.m b/src/robot/ini_Climbing_ANYmal_LP.m new file mode 100644 index 0000000..4f27edb --- /dev/null +++ b/src/robot/ini_Climbing_ANYmal_LP.m @@ -0,0 +1,352 @@ +%%%%%% Initialize +%%%%%% ini_Climbing_ANYmal_LP +%%%%%% +%%%%%% Mass/inertia parameters and link connections for possible climbing +%%%%%% ANYmal robot model +%%%%%% +%%%%%% -------------------------------------------------------------------- +%%%%%% This configration file can reproduce the simulation result used in +%%%%%% CLAWAR 2021 proceedings paper by K. Uno and G. Valsecchi et al. +%%%%%% Proceedings Paper URL: +%%%%%% https://******** (to be added) +%%%%%% -------------------------------------------------------------------- +%%%%%% +%%%%%% +%%%%%% Created: 2021-02-02 +%%%%%% by Kentaro Uno +%%%%%% Last update: 2021-08-24 +%%%%%% by Kentaro Uno + +function [LP, ani_settings] = ini_Climbing_ANYmal_LP(robot_param, ani_settings) + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Copied from the optimized values %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%% My robot +%%% Add my robot properties for climbing and concurrent design %%% +robot.myRobot.mass.total = 22.8; % only used to calculated cost of transport +robot.myRobot.legCount = 4; + +% Density of each link +% kg/m^3. Density values calculated to give correct link mass when link +% approximated as solid cylinder. +robot.myRobot.legDensity.hip(1) = 9728.3; robot.myRobot.legDensity.hip(2) = 9728.3; +robot.myRobot.legDensity.thigh(1) = 5826.3; robot.myRobot.legDensity.thigh(2) = 5826.3; +robot.myRobot.legDensity.shank(1) = 888.2668; robot.myRobot.legDensity.shank(2) = 888.2668; +robot.myRobot.legDensity.foot(1) = 800; robot.myRobot.legDensity.foot(2) = 800; +robot.myRobot.legDensity.phalanges(1) = 800; robot.myRobot.legDensity.phalanges(2) = 800; + +% End effector mass +robot.myRobot.EE(1).mass = 0.1402; +robot.myRobot.EE(2).mass = 0.1402; + +% Offset from nominal CoM position to base hip attachment for each leg. +robot.myRobot.xNom(1) = 0.05; +robot.myRobot.xNom(2) = 0.05; +robot.myRobot.yNom(1) = 0.14; +robot.myRobot.yNom(2) = 0.11; +robot.myRobot.zNom = 0.01; % offset from CoM to HAA in z direction. Positive value means HAA above CoM. + +robot.myRobot.nomHipPos.LF = [ robot.myRobot.xNom(1), robot.myRobot.yNom(1), robot.myRobot.zNom]; +robot.myRobot.nomHipPos.LH = [-robot.myRobot.xNom(2), robot.myRobot.yNom(2), robot.myRobot.zNom]; +robot.myRobot.nomHipPos.RF = [ robot.myRobot.xNom(1), -robot.myRobot.yNom(1), robot.myRobot.zNom]; +robot.myRobot.nomHipPos.RH = [-robot.myRobot.xNom(2), -robot.myRobot.yNom(2), robot.myRobot.zNom]; + +% redefine the Offset from nominal CoM position to base hip attachment for each leg based on the CAD. +robot.myRobot.nomHipPos.LF = [ 0.15, 0.15, 0.0]; +robot.myRobot.nomHipPos.LH = [-0.15, 0.15, 0.0]; +robot.myRobot.nomHipPos.RF = [ 0.15, -0.15, 0.0]; +robot.myRobot.nomHipPos.RH = [-0.15, -0.15, 0.0]; + +% Link lengths [m] +robot.myRobot.hip(1).length = 0.100; % updated with the optimized value. +robot.myRobot.hip(2).length = 0.100; % updated with the optimized value. +robot.myRobot.thigh(1).length = 0.434; % updated with the optimized value. +robot.myRobot.thigh(2).length = 0.434; % updated with the optimized value. +robot.myRobot.shank(1).length = 0.418; % updated with the optimized value. +robot.myRobot.shank(2).length = 0.418; % updated with the optimized value. +robot.myRobot.foot(1).length = 0.16; % for now, not used +robot.myRobot.foot(2).length = 0.16; % for now, not used +robot.myRobot.phalanges(1).length = 0.1; % for now, not used +robot.myRobot.phalanges(2).length = 0.1; % for now, not used + +% Link radius [m] (only used to calculate link mass as solid cylinder) +robot.myRobot.hip(1).radius = 0.015; +robot.myRobot.hip(2).radius = 0.015; +robot.myRobot.thigh(1).radius = 0.015; +robot.myRobot.thigh(2).radius = 0.015; +robot.myRobot.shank(1).radius = 0.015; +robot.myRobot.shank(2).radius = 0.015; +robot.myRobot.foot(1).radius = 0.015; +robot.myRobot.foot(2).radius = 0.015; +robot.myRobot.phalanges(1).radius = 0.015; +robot.myRobot.phalanges(2).radius = 0.015; + +% Transmission Ratio +% For remote and directly actuated joints +% GearRatio = input speed/ output speed +% Front legs Hind legs +robot.myRobot.transmissionGearRatio.HAA(1) = 5.6; robot.myRobot.transmissionGearRatio.HAA(2) = 5.6; +robot.myRobot.transmissionGearRatio.HFE(1) = 5.6; robot.myRobot.transmissionGearRatio.HFE(2) = 5.6; +robot.myRobot.transmissionGearRatio.KFE(1) = 5.6; robot.myRobot.transmissionGearRatio.KFE(2) = 5.6; +robot.myRobot.transmissionGearRatio.AFE(1) = 5.6; robot.myRobot.transmissionGearRatio.AFE(2) = 5.6; +robot.myRobot.transmissionGearRatio.DFE(1) = 5.6; robot.myRobot.transmissionGearRatio.DFE(2) = 5.6; + +% Joint angle limits (used only in reachable positions plot) +% q1 HAA, q2 HFE, q3 KFE, q4 AFE +robot.myRobot.q1.minAngle = -pi; +robot.myRobot.q1.maxAngle = pi; +robot.myRobot.q2.minAngle = -2*pi; +robot.myRobot.q2.maxAngle = 2*pi; +robot.myRobot.q3.minAngle = -2*pi; +robot.myRobot.q3.maxAngle = 2*pi; +robot.myRobot.q4.minAngle = -pi; +robot.myRobot.q4.maxAngle = pi; +robot.myRobot.q5.minAngle = -pi; +robot.myRobot.q5.maxAngle = pi; + +% Base dimensions used for visualization - visualized as a box +robot.myRobot.baseLength = 0.3; +robot.myRobot.baseWidth = 0.3; +robot.myRobot.baseHeight = 0.15; + + %% newly added parameters necessary for climbing/concurrent design + + % Nominal EE positions, translation from base to EE +% robot.myRobot.nomBaseHeight = 0.42; +% robot.myRobot.nomEEPos.LF = [ 0.34, 0.19, -robot.myRobot.nomBaseHeight]; +% robot.myRobot.nomEEPos.RF = [ 0.34, -0.19, -robot.myRobot.nomBaseHeight]; +% robot.myRobot.nomEEPos.LH = [ -0.34, 0.19, -robot.myRobot.nomBaseHeight]; +% robot.myRobot.nomEEPos.RH = [ -0.34, -0.19, -robot.myRobot.nomBaseHeight]; + + % maximum deviation from nominal EE position (edge length of foot workspace is equal to 2*maxDeviationEE = 2*b) + robot.myRobot.maxDeviationEEInitial.LF = [0.15; 0.1; 0.1]; + robot.myRobot.maxDeviationEEInitial.RF = [0.15; 0.1; 0.1]; + robot.myRobot.maxDeviationEEInitial.LH = [0.15; 0.1; 0.1]; + robot.myRobot.maxDeviationEEInitial.RH = [0.15; 0.1; 0.1]; + + % maximum EE gripping force + robot.myRobot.grippingForce = 200; + + % dynamic model robot, according to TOWR for ANYmal +% Ixx = 0.946438; +% Iyy = 1.94478; +% Izz = 2.01835; +% Ixy = 0.000938112; +% Ixz = -0.00595386; +% Iyz = -0.00146328; + + % dynamic model robot + Ixx = 0.05618; + Iyy = 0.06743; + Izz = 0.05055; + Ixy = 0.014655; + Ixz = 0.0945; + Iyz = 0.0945; + + % merging tensor (no user taks) + robot.myRobot.inertiaTensor = [ Ixx, -Ixy, -Ixz; + -Ixy, Iyy, -Iyz; + -Ixz, -Iyz, Izz]; % inertia Tensor of base + + %% added parameters necessary for ClimbLab + % note that z axis is always rotation axis, and x axis is always towards the EE in ClimbLab + % LF hip link inertial tensor calculation + robot.myRobot.inertiaTensorForLinks.LF.hip.mass = robot.myRobot.legDensity.hip(1) * pi ... + * robot.myRobot.hip(1).length * robot.myRobot.hip(1).radius^2; + robot.myRobot.inertiaTensorForLinks.LF.hip.Ixx = 1/2 * robot.myRobot.inertiaTensorForLinks.LF.hip.mass ... + * robot.myRobot.hip(1).radius^2; + robot.myRobot.inertiaTensorForLinks.LF.hip.Iyy = 1/4 * robot.myRobot.inertiaTensorForLinks.LF.hip.mass ... + * robot.myRobot.hip(1).radius^2 ... + + 1/12 * robot.myRobot.inertiaTensorForLinks.LF.hip.mass ... + * robot.myRobot.hip(1).length^2; + robot.myRobot.inertiaTensorForLinks.LF.hip.Izz = robot.myRobot.inertiaTensorForLinks.LF.hip.Iyy; + % LF thigh link inertial tensor calculation + robot.myRobot.inertiaTensorForLinks.LF.thigh.mass = robot.myRobot.legDensity.thigh(1) * pi ... + * robot.myRobot.thigh(1).length * robot.myRobot.thigh(1).radius^2; + robot.myRobot.inertiaTensorForLinks.LF.thigh.Ixx = 1/2 * robot.myRobot.inertiaTensorForLinks.LF.thigh.mass ... + * robot.myRobot.thigh(1).radius^2; + robot.myRobot.inertiaTensorForLinks.LF.thigh.Iyy = 1/4 * robot.myRobot.inertiaTensorForLinks.LF.thigh.mass ... + * robot.myRobot.thigh(1).radius^2 ... + + 1/12 * robot.myRobot.inertiaTensorForLinks.LF.thigh.mass ... + * robot.myRobot.thigh(1).length^2; + robot.myRobot.inertiaTensorForLinks.LF.thigh.Izz = robot.myRobot.inertiaTensorForLinks.LF.thigh.Iyy; + % LF shank link inertial tensor calculation + robot.myRobot.inertiaTensorForLinks.LF.shank.mass = robot.myRobot.legDensity.shank(1) * pi ... + * robot.myRobot.shank(1).length * robot.myRobot.shank(1).radius^2; + robot.myRobot.inertiaTensorForLinks.LF.shank.Ixx = 1/2 * robot.myRobot.inertiaTensorForLinks.LF.shank.mass ... + * robot.myRobot.shank(1).radius^2; + robot.myRobot.inertiaTensorForLinks.LF.shank.Iyy = 1/4 * robot.myRobot.inertiaTensorForLinks.LF.shank.mass ... + * robot.myRobot.shank(1).radius^2 ... + + 1/12 * robot.myRobot.inertiaTensorForLinks.LF.shank.mass ... + * robot.myRobot.shank(1).length^2; + robot.myRobot.inertiaTensorForLinks.LF.shank.Izz = robot.myRobot.inertiaTensorForLinks.LF.shank.Iyy; + + % all limb has the same inertial spec. + robot.myRobot.inertiaTensorForLinks.LH = robot.myRobot.inertiaTensorForLinks.LF; + robot.myRobot.inertiaTensorForLinks.RH = robot.myRobot.inertiaTensorForLinks.LF; + robot.myRobot.inertiaTensorForLinks.RF = robot.myRobot.inertiaTensorForLinks.LF; + + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Visualization settings are changed here based on the above values %%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Link radius [m] +ani_settings.link_radius = 2*robot.myRobot.hip(1).radius; % drawn radius is bigger than phisic model. same radius for all links +%%% Base thickness [m] +ani_settings.base_upper_thickness = 2/3 * robot.myRobot.baseHeight; +ani_settings.base_lower_thickness = 1/3 * robot.myRobot.baseHeight; +%%% Base horisontal scaling factor +ani_settings.base_xy_scale_factor = 1.0; + +%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% From here, the LP used in ClimbLab are substitued by with the abone values. +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +%%% nomally, the LP should be defined here. +LP.joint_allocation_type = 'mammal'; % state the type of the joint configuration -> IK analytical solver is switched +% LP.leg_config_type = 'oo'; % if the joint alloc. is mammalian, you need to decide the leg configuration as well + +% local variables to define the offset angle theta_1 and theta_2 +LP.theta_1 = 0.1440; % [rad] +LP.theta_2 = -0.1955; % [rad] + +if isnan(robot_param.theta_1) == 0 % this if sentence is only applied when you set the value in config file + LP.theta_1 = robot_param.theta_1; +end +if isnan(robot_param.theta_2) == 0 % this if sentence is only applied when you set the value in config file + LP.theta_2 = robot_param.theta_2; +end +% this if sentence is only for the special alias to define the theta1 and 2 in config file +if strcmp(robot_param.joint_allocation_type, 'insect') + % The following offset angle setting enables the robot model to be spider + % configuration + LP.theta_1 = pi/4; % [rad] + LP.theta_2 = -pi/2; % [rad] +end + +if robot_param.leg_config_type ~= "" % this if sentence is only for the special switch in config file + LP.leg_config_type = robot_param.leg_config_type; +end + + +% Max. endurable gripping force +LP.F_grip = robot.myRobot.grippingForce; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Define link connection relationship +LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; + +% Number of links/joints +LP.num_q = length(LP.BB); + +LP.SS = [ -1 1 0 0 0 0 0 0 0 0 0 0; + 0 -1 1 0 0 0 0 0 0 0 0 0; + 0 0 -1 0 0 0 0 0 0 0 0 0; + 0 0 0 -1 1 0 0 0 0 0 0 0; + 0 0 0 0 -1 1 0 0 0 0 0 0; + 0 0 0 0 0 -1 0 0 0 0 0 0; + 0 0 0 0 0 0 -1 1 0 0 0 0; + 0 0 0 0 0 0 0 -1 1 0 0 0; + 0 0 0 0 0 0 0 0 -1 0 0 0; + 0 0 0 0 0 0 0 0 0 -1 1 0; + 0 0 0 0 0 0 0 0 0 0 -1 1; + 0 0 0 0 0 0 0 0 0 0 0 -1]; +% Links connected to base (link 0) +LP.S0 = [ 1 0 0 1 0 0 1 0 0 1 0 0]; +% End link (leg) +LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; +% Type of joint +LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint [deg] +LP.joint_limit=[rad2deg(robot.myRobot.q1.minAngle) rad2deg(robot.myRobot.q1.maxAngle); % HAA + rad2deg(robot.myRobot.q2.minAngle) rad2deg(robot.myRobot.q2.maxAngle); % HFE + rad2deg(robot.myRobot.q3.minAngle) rad2deg(robot.myRobot.q3.maxAngle)]; % KFE + +% Position vector from base CoM to i-th joint. Order is LF_HAA, LF_HFE, +% LF_KFE, LH_HAA, LH_HFE, LH_KFE, RH_HAA, RH_HFE, RH_KFE, RF_HAA, RF_HFE, and RF_KFE. +LP.c0 = [ robot.myRobot.nomHipPos.LF(1) 0 0 robot.myRobot.nomHipPos.LH(1) 0 0 robot.myRobot.nomHipPos.RH(1) 0 0 robot.myRobot.nomHipPos.RF(1) 0 0; + robot.myRobot.nomHipPos.LF(2) 0 0 robot.myRobot.nomHipPos.LH(2) 0 0 robot.myRobot.nomHipPos.RH(2) 0 0 robot.myRobot.nomHipPos.RF(2) 0 0; + robot.myRobot.nomHipPos.LF(3) 0 0 robot.myRobot.nomHipPos.LH(3) 0 0 robot.myRobot.nomHipPos.RH(3) 0 0 robot.myRobot.nomHipPos.RF(3) 0 0]; +% Mass of the base (link 0) <- copied from URDF file +LP.m0 = 16.793507758; % [kg] +% Moment of inertia of base <- copied from URDF file +LP.inertia0 = robot.myRobot.inertiaTensor; + +% Mass of each link <- based on manual measurement +LP.m = [ robot.myRobot.inertiaTensorForLinks.LF.hip.mass robot.myRobot.inertiaTensorForLinks.LF.thigh.mass robot.myRobot.inertiaTensorForLinks.LF.shank.mass + robot.myRobot.EE(1).mass... + robot.myRobot.inertiaTensorForLinks.LH.hip.mass robot.myRobot.inertiaTensorForLinks.LH.thigh.mass robot.myRobot.inertiaTensorForLinks.LH.shank.mass + robot.myRobot.EE(1).mass... + robot.myRobot.inertiaTensorForLinks.RH.hip.mass robot.myRobot.inertiaTensorForLinks.RH.thigh.mass robot.myRobot.inertiaTensorForLinks.RH.shank.mass + robot.myRobot.EE(1).mass... + robot.myRobot.inertiaTensorForLinks.RF.hip.mass robot.myRobot.inertiaTensorForLinks.RF.thigh.mass robot.myRobot.inertiaTensorForLinks.RF.shank.mass + robot.myRobot.EE(1).mass ]; % [kg] +% Total mass +LP.mass = sum(LP.m) + LP.m0; % [kg] + +%%% Rotational relationship of links frames + +% so, with the rotation about theta_1 around the z axis, then rotation +% about theta_2+pi/2 around the y' axis, the hip joint frame is obtained. +% However, for the SpaceDyn function, we should describe this in not z-y-x +% euler angle, but in x-y-z euler angle!! +LF_euler_angle_ZYX = [LP.theta_1 pi/2+LP.theta_2 0]; LH_euler_angle_ZYX = [-LP.theta_1 pi/2-LP.theta_2 0]; +RH_euler_angle_ZYX = [LP.theta_1 pi/2-LP.theta_2 0]; RF_euler_angle_ZYX = [-LP.theta_1 pi/2+LP.theta_2 0]; +LF_rotation_matrix = eul2rotm(LF_euler_angle_ZYX,'ZYX'); +LH_rotation_matrix = eul2rotm(LH_euler_angle_ZYX,'ZYX'); +RH_rotation_matrix = eul2rotm(RH_euler_angle_ZYX,'ZYX'); +RF_rotation_matrix = eul2rotm(RF_euler_angle_ZYX,'ZYX'); + +LF_euler_angle_XYZ = rotm2eul(LF_rotation_matrix,'XYZ'); +LH_euler_angle_XYZ = rotm2eul(LH_rotation_matrix,'XYZ'); +RH_euler_angle_XYZ = rotm2eul(RH_rotation_matrix,'XYZ'); +RF_euler_angle_XYZ = rotm2eul(RF_rotation_matrix,'XYZ'); + +LP.Qi = [ LF_euler_angle_XYZ(1) pi/2 0 LH_euler_angle_XYZ(1) pi/2 0 RH_euler_angle_XYZ(1) pi/2 0 RF_euler_angle_XYZ(1) pi/2 0; + LF_euler_angle_XYZ(2) 0 0 LH_euler_angle_XYZ(2) 0 0 RH_euler_angle_XYZ(2) 0 0 RF_euler_angle_XYZ(2) 0 0; + LF_euler_angle_XYZ(3) 0 0 LH_euler_angle_XYZ(3) 0 0 RH_euler_angle_XYZ(3) 0 0 RF_euler_angle_XYZ(3) 0 0]; + +% Inertia tensor of each link +LP.inertia = [ robot.myRobot.inertiaTensorForLinks.LF.hip.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.LF.thigh.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.LF.shank.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.LH.hip.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.LH.thigh.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.LH.shank.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.RH.hip.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.RH.thigh.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.RH.shank.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.RF.hip.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.RF.thigh.Ixx 0 0 robot.myRobot.inertiaTensorForLinks.RF.shank.Ixx 0 0 ; + 0 robot.myRobot.inertiaTensorForLinks.LF.hip.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.LF.thigh.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.LF.shank.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.LH.hip.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.LH.thigh.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.LH.shank.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.RH.hip.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.RH.thigh.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.RH.shank.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.RF.hip.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.RF.thigh.Iyy 0 0 robot.myRobot.inertiaTensorForLinks.RF.shank.Iyy 0 ; + 0 0 robot.myRobot.inertiaTensorForLinks.LF.hip.Izz 0 0 robot.myRobot.inertiaTensorForLinks.LF.thigh.Izz 0 0 robot.myRobot.inertiaTensorForLinks.LF.shank.Izz 0 0 robot.myRobot.inertiaTensorForLinks.LH.hip.Izz 0 0 robot.myRobot.inertiaTensorForLinks.LH.thigh.Izz 0 0 robot.myRobot.inertiaTensorForLinks.LH.shank.Izz 0 0 robot.myRobot.inertiaTensorForLinks.RH.hip.Izz 0 0 robot.myRobot.inertiaTensorForLinks.RH.thigh.Izz 0 0 robot.myRobot.inertiaTensorForLinks.RH.shank.Izz 0 0 robot.myRobot.inertiaTensorForLinks.RF.hip.Izz 0 0 robot.myRobot.inertiaTensorForLinks.RF.thigh.Izz 0 0 robot.myRobot.inertiaTensorForLinks.RF.shank.Izz ]; + +% Position vector from each link CoM to i-th joint +LP.cc = zeros( 3,12,12 ); + +% Postion vector from link 1 CoM to joint 1 seen from the joint 1 frame +LP.cc(:,1,1) = [ 0 -0 -robot.myRobot.hip(1).length/2 ]'; +% Postion vector from link 1 CoM to joint 2 +LP.cc(:,1,2) = [ 0 0 robot.myRobot.hip(1).length/2 ]'; +LP.cc(:,2,2) = [ -robot.myRobot.thigh(1).length/2 0 0 ]'; +LP.cc(:,2,3) = [ robot.myRobot.thigh(1).length/2 0 -0 ]'; +LP.cc(:,3,3) = [ -robot.myRobot.shank(1).length/2 0 0 ]'; + +LP.cc(:,4,4) = [ 0 -0 robot.myRobot.hip(1).length/2 ]'; +LP.cc(:,4,5) = [ 0 0 -robot.myRobot.hip(1).length/2 ]'; +LP.cc(:,5,5) = [ -robot.myRobot.thigh(1).length/2 0 0 ]'; +LP.cc(:,5,6) = [ robot.myRobot.thigh(1).length/2 0 -0 ]'; +LP.cc(:,6,6) = [ -robot.myRobot.shank(1).length/2 0 0 ]'; + +LP.cc(:,7,7) = [ 0 0 robot.myRobot.hip(1).length/2 ]'; +LP.cc(:,7,8) = [ 0 -0 -robot.myRobot.hip(1).length/2 ]'; +LP.cc(:,8,8) = [ -robot.myRobot.thigh(1).length/2 0 -0 ]'; +LP.cc(:,8,9) = [ robot.myRobot.thigh(1).length/2 0 0 ]'; +LP.cc(:,9,9) = [ -robot.myRobot.shank(1).length/2 0 0 ]'; + +LP.cc(:,10,10) = [ 0 0 -robot.myRobot.hip(1).length/2 ]'; +LP.cc(:,10,11) = [ 0 -0 robot.myRobot.hip(1).length/2 ]'; +LP.cc(:,11,11) = [ -robot.myRobot.thigh(1).length/2 0 -0 ]'; +LP.cc(:,11,12) = [ robot.myRobot.thigh(1).length/2 0 0 ]'; +LP.cc(:,12,12) = [ -robot.myRobot.shank(1).length/2 0 0 ]'; + +% Position vector from end link CoM to end point +LP.ce = [ 0 0 0.32125/2 0 0 0.32125/2 0 0 0.32125/2 0 0 0.32125/2 ; + 0 0 0 0 0 -0 0 0 -0 0 0 0 ; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; +% Rotational relationship of end link frames +LP.Qe = [ 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; + +% EOF \ No newline at end of file diff --git a/src/robot/ini_HubRobo_grip_to_spine_LP.m b/src/robot/ini_HubRobo_grip_to_spine_LP.m deleted file mode 100644 index ba38e90..0000000 --- a/src/robot/ini_HubRobo_grip_to_spine_LP.m +++ /dev/null @@ -1,105 +0,0 @@ -%%%%%% Initialize -%%%%%% ini_HubRobo_grip_to_spine_LP -%%%%%% -%%%%%% Mass/inertia parameters and link connections for HubRobo with grippers -%%%%%% -%%%%%% Created 2018-02-23 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-06-24 by Kentaro Uno (changed the file name) - -function LP = ini_HubRobo_grip_to_spine_LP() - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Define link connection relationship -LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; - -% Number of links/joints -LP.num_q = length(LP.BB); - -LP.SS = [ -1 1 0 0 0 0 0 0 0 0 0 0; - 0 -1 1 0 0 0 0 0 0 0 0 0; - 0 0 -1 0 0 0 0 0 0 0 0 0; - 0 0 0 -1 1 0 0 0 0 0 0 0; - 0 0 0 0 -1 1 0 0 0 0 0 0; - 0 0 0 0 0 -1 0 0 0 0 0 0; - 0 0 0 0 0 0 -1 1 0 0 0 0; - 0 0 0 0 0 0 0 -1 1 0 0 0; - 0 0 0 0 0 0 0 0 -1 0 0 0; - 0 0 0 0 0 0 0 0 0 -1 1 0; - 0 0 0 0 0 0 0 0 0 0 -1 1; - 0 0 0 0 0 0 0 0 0 0 0 -1]; -% Links connected to base (link 0) -LP.S0 = [ 1 0 0 1 0 0 1 0 0 1 0 0]; -% End link (leg) -LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; -% Type of joint -LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; - -% Position vector from base CoM to i-th joint -LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; - 0.054 0 0 0.054 0 0 -0.054 0 0 -0.054 0 0; - 0.000 0 0 0.000 0 0 0.000 0 0 0.000 0 0]; -% Mass of the base (link 0) -% LP.m0 = 0.6762; -LP.m0 = 0.8502; % New measurement with upboard -% Moment of inertia of base -LP.inertia0 = [ 62404194*10^-11 0 0 ; - 0 62524799*10^-11 0 ; - 0 0 116269155*10^-11]; - -% Mass of each link -LP.m = [ 0.0945 0.1104 0.0088 0.0945 0.1104 0.0088 0.0945 0.1104 0.0088 ... - 0.0945 0.1104 0.0088 ]; -% Total mass -LP.mass = sum(LP.m) + LP.m0; -% Rotational relationship of links frames -LP.Qi = [ 0 pi/2 0 0 pi/2 0 0 pi/2 0 0 pi/2 0; - 0 0 0 0 0 0 0 0 0 0 0 0; - pi/4 0 0 pi*3/4 0 0 pi*5/4 0 0 pi*7/4 0 0]; -% Moment of inertia of each link -LP.inertia = [ 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 ; - 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 ; - 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 ]; - -% Position vector from each link CoM to i-th joint -LP.cc = zeros( 3,12,12 ); - -% Postion vector from link 1 CoM to joint 1 -LP.cc(:,1,1) = [ -0.0145 0 -0.0085 ]'; -% Postion vector from link 1 CoM to joint 2 -LP.cc(:,1,2) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,2,2) = [ -0.0290 0 0 ]'; -LP.cc(:,2,3) = [ 0.0290 0 0 ]'; -LP.cc(:,3,3) = [ -0.0810 0 0 ]'; - -LP.cc(:,4,4) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,4,5) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,5,5) = [ -0.0290 0 0 ]'; -LP.cc(:,5,6) = [ 0.0290 0 0 ]'; -LP.cc(:,6,6) = [ -0.0810 0 0 ]'; - -LP.cc(:,7,7) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,7,8) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,8,8) = [ -0.0290 0 0 ]'; -LP.cc(:,8,9) = [ 0.0290 0 0 ]'; -LP.cc(:,9,9) = [ -0.0810 0 0 ]'; - -LP.cc(:,10,10) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,10,11) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,11,11) = [ -0.0290 0 0 ]'; -LP.cc(:,11,12) = [ 0.0290 0 0 ]'; -LP.cc(:,12,12) = [ -0.0810 0 0 ]'; - -% Position vector from end link CoM to end point -LP.ce = [ 0 0 0.0810 0 0 0.0810 0 0 0.0810 0 0 0.0810 ; - 0 0 0 0 0 0 0 0 0 0 0 0 ; - 0 0 0 0 0 0 0 0 0 0 0 0 ]; -% Rotational relationship of end link frames -LP.Qe = [ 0 0 0 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 0 0 0]; - -% EOF \ No newline at end of file diff --git a/src/robot/ini_HubRobo_grip_to_spine_LP_old.m b/src/robot/ini_HubRobo_grip_to_spine_LP_old.m index 9323372..012556f 100644 --- a/src/robot/ini_HubRobo_grip_to_spine_LP_old.m +++ b/src/robot/ini_HubRobo_grip_to_spine_LP_old.m @@ -10,6 +10,16 @@ function LP = ini_HubRobo_grip_to_spine_LP_old() +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force +LP.F_grip = 3.2; + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -38,6 +48,8 @@ LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; % Type of joint LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-60 60; 0 90; -120 0]; %[deg] % Position vector from base CoM to i-th joint LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; diff --git a/src/robot/ini_HubRobo_no_grip_LP.m b/src/robot/ini_HubRobo_no_grip_LP.m deleted file mode 100644 index c45bbb9..0000000 --- a/src/robot/ini_HubRobo_no_grip_LP.m +++ /dev/null @@ -1,100 +0,0 @@ -%%%%%% Initialize -%%%%%% ini_HubRobo_no_grip_LP -%%%%%% -%%%%%% Mass/inertia parameters and link connections for HubRobo -%%%%%% -%%%%%% Created on 2020-06-24 by Kentaro Uno -%%%%%% Last updated on 2020-06-24 by Kentaro Uno - -function LP = ini_HubRobo_no_grip_LP() - -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -% Define link connection relationship -LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; - -% Number of links/joints -LP.num_q = length(LP.BB); - -LP.SS = [ -1 1 0 0 0 0 0 0 0 0 0 0; - 0 -1 1 0 0 0 0 0 0 0 0 0; - 0 0 -1 0 0 0 0 0 0 0 0 0; - 0 0 0 -1 1 0 0 0 0 0 0 0; - 0 0 0 0 -1 1 0 0 0 0 0 0; - 0 0 0 0 0 -1 0 0 0 0 0 0; - 0 0 0 0 0 0 -1 1 0 0 0 0; - 0 0 0 0 0 0 0 -1 1 0 0 0; - 0 0 0 0 0 0 0 0 -1 0 0 0; - 0 0 0 0 0 0 0 0 0 -1 1 0; - 0 0 0 0 0 0 0 0 0 0 -1 1; - 0 0 0 0 0 0 0 0 0 0 0 -1]; -% Links connected to base (link 0) -LP.S0 = [ 1 0 0 1 0 0 1 0 0 1 0 0]; -% End link (leg) -LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; -% Type of joint -LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; - -% Position vector from base CoM to i-th joint -LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; - 0.054 0 0 0.054 0 0 -0.054 0 0 -0.054 0 0; - 0.000 0 0 0.000 0 0 0.000 0 0 0.000 0 0]; -% Mass of the base (link 0) -% LP.m0 = 0.6762; -LP.m0 = 0.8502; % New measurement with upboard -% Moment of inertia of base -LP.inertia0 = [ 62404194*10^-11 0 0 ; - 0 62524799*10^-11 0 ; - 0 0 116269155*10^-11]; - -% Mass of each link -LP.m = [ 0.0945 0.1104 0.0088 0.0945 0.1104 0.0088 0.0945 0.1104 0.0088 ... - 0.0945 0.1104 0.0088 ]; -% Total mass -LP.mass = sum(LP.m) + LP.m0; -% Rotational relationship of links frames -LP.Qi = [ 0 pi/2 0 0 pi/2 0 0 pi/2 0 0 pi/2 0; - 0 0 0 0 0 0 0 0 0 0 0 0; - pi/4 0 0 pi*3/4 0 0 pi*5/4 0 0 pi*7/4 0 0]; -% Moment of inertia of each link -LP.inertia = [ 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 ; - 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 0 1.686642046585767e-06 0 0 4.271415082850776e-05 0 0 4.372178464980286e-06 0 ; - 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 0 0 4.287324265048666e-07 0 0 2.162286114968218e-05 0 0 1.610569299605712e-07 ]; - -% Postion vector from link 1 CoM to joint 1 -LP.cc(:,1,1) = [ -0.0145 0 -0.0085 ]'; -% Postion vector from link 1 CoM to joint 2 -LP.cc(:,1,2) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,2,2) = [ -0.0290 0 0 ]'; -LP.cc(:,2,3) = [ 0.0290 0 0 ]'; -LP.cc(:,3,3) = [ -0.0610 0 0 ]'; - -LP.cc(:,4,4) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,4,5) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,5,5) = [ -0.0290 0 0 ]'; -LP.cc(:,5,6) = [ 0.0290 0 0 ]'; -LP.cc(:,6,6) = [ -0.0610 0 0 ]'; - -LP.cc(:,7,7) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,7,8) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,8,8) = [ -0.0290 0 0 ]'; -LP.cc(:,8,9) = [ 0.0290 0 0 ]'; -LP.cc(:,9,9) = [ -0.0610 0 0 ]'; - -LP.cc(:,10,10) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,10,11) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,11,11) = [ -0.0290 0 0 ]'; -LP.cc(:,11,12) = [ 0.0290 0 0 ]'; -LP.cc(:,12,12) = [ -0.0610 0 0 ]'; - -% Position vector from end link CoM to end point -LP.ce = [ 0 0 0.0610 0 0 0.0610 0 0 0.0610 0 0 0.0610 ; - 0 0 0 0 0 0 0 0 0 0 0 0 ; - 0 0 0 0 0 0 0 0 0 0 0 0 ]; -% Rotational relationship of end link frames -LP.Qe = [ 0 0 0 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 0 0 0; - 0 0 0 0 0 0 0 0 0 0 0 0]; -% EOF \ No newline at end of file diff --git a/src/robot/ini_HubRobo_v2_2_grip_to_palm_LP.m b/src/robot/ini_HubRobo_v2_2_grip_to_palm_LP.m index aa91c74..49bae2e 100644 --- a/src/robot/ini_HubRobo_v2_2_grip_to_palm_LP.m +++ b/src/robot/ini_HubRobo_v2_2_grip_to_palm_LP.m @@ -10,6 +10,16 @@ function LP = ini_HubRobo_v2_2_grip_to_palm_LP() +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force +LP.F_grip = 3.2; + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -38,6 +48,8 @@ LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; % Type of joint LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-60 60; -15 90; -135 0]; %[deg] % Position vector from base CoM to i-th joint LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; diff --git a/src/robot/ini_HubRobo_grip_to_palm_LP.m b/src/robot/ini_HubRobo_v2_2_grip_to_palm_LP_gazebo.m similarity index 82% rename from src/robot/ini_HubRobo_grip_to_palm_LP.m rename to src/robot/ini_HubRobo_v2_2_grip_to_palm_LP_gazebo.m index 1b8bc0e..4662984 100644 --- a/src/robot/ini_HubRobo_grip_to_palm_LP.m +++ b/src/robot/ini_HubRobo_v2_2_grip_to_palm_LP_gazebo.m @@ -1,5 +1,5 @@ %%%%%% Initialize -%%%%%% ini_HubRobo_grip_to_palm_LP +%%%%%% ini_HubRobo_v2_2_grip_to_palm_LP %%%%%% %%%%%% Mass/inertia parameters and link connections for HubRobo with grippers %%%%%% Last link length is defined as the distance from @@ -8,12 +8,15 @@ %%%%%% Created on 2018-02-23 by Warley Ribeiro %%%%%% Last updated on 2020-06-21 by Kentaro Uno -function LP = ini_HubRobo_grip_to_palm_LP() +function LP = ini_HubRobo_v2_2_grip_to_palm_LP() %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% state the type of the joint configuration -> IK analytical solution is switched +LP.leg_config_type = 'insect'; + % Define link connection relationship LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; @@ -69,33 +72,33 @@ LP.cc = zeros( 3,12,12 ); % Postion vector from link 1 CoM to joint 1 -LP.cc(:,1,1) = [ -0.0145 0 -0.0085 ]'; +LP.cc(:,1,1) = [ -0.014224 -0.0001013 -0.0085025 ]'; % Postion vector from link 1 CoM to joint 2 -LP.cc(:,1,2) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,2,2) = [ -0.0290 0 0 ]'; -LP.cc(:,2,3) = [ 0.0290 0 0 ]'; -LP.cc(:,3,3) = [ -0.0760 0 0 ]'; +LP.cc(:,1,2) = [ 0.014224 0.0001013 0.0085025 ]'; +LP.cc(:,2,2) = [ -0.0285685 0 0 ]'; +LP.cc(:,2,3) = [ 0.0285685 0 0 ]'; +LP.cc(:,3,3) = [ -0.0738 0 0 ]'; -LP.cc(:,4,4) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,4,5) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,5,5) = [ -0.0290 0 0 ]'; -LP.cc(:,5,6) = [ 0.0290 0 0 ]'; -LP.cc(:,6,6) = [ -0.0760 0 0 ]'; +LP.cc(:,4,4) = [ -0.014224 -0.0001013 -0.0085025 ]'; +LP.cc(:,4,5) = [ 0.014224 0.0001013 0.0085025 ]'; +LP.cc(:,5,5) = [ -0.0285685 0 0 ]'; +LP.cc(:,5,6) = [ 0.0285685 0 0 ]'; +LP.cc(:,6,6) = [ -0.0738 0 0 ]'; -LP.cc(:,7,7) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,7,8) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,8,8) = [ -0.0290 0 0 ]'; -LP.cc(:,8,9) = [ 0.0290 0 0 ]'; -LP.cc(:,9,9) = [ -0.0760 0 0 ]'; +LP.cc(:,7,7) = [ -0.014224 -0.0001013 -0.0085025 ]'; +LP.cc(:,7,8) = [ 0.014224 0.0001013 0.0085025 ]'; +LP.cc(:,8,8) = [ -0.0285685 0 0 ]'; +LP.cc(:,8,9) = [ 0.0285685 0 0 ]'; +LP.cc(:,9,9) = [ -0.0738 0 0 ]'; -LP.cc(:,10,10) = [ -0.0145 0 -0.0085 ]'; -LP.cc(:,10,11) = [ 0.0145 0 0.0085 ]'; -LP.cc(:,11,11) = [ -0.0290 0 0 ]'; -LP.cc(:,11,12) = [ 0.0290 0 0 ]'; -LP.cc(:,12,12) = [ -0.0760 0 0 ]'; +LP.cc(:,10,10) = [ -0.014224 -0.0001013 -0.0085025 ]'; +LP.cc(:,10,11) = [ 0.014224 0.0001013 0.0085025 ]'; +LP.cc(:,11,11) = [ -0.0285685 0 0 ]'; +LP.cc(:,11,12) = [ 0.0285685 0 0 ]'; +LP.cc(:,12,12) = [ -0.0738 0 0 ]'; % Position vector from end link CoM to end point -LP.ce = [ 0 0 0.0760 0 0 0.0760 0 0 0.0760 0 0 0.0760 ; +LP.ce = [ 0 0 0.0738 0 0 0.0738 0 0 0.0738 0 0 0.0738 ; 0 0 0 0 0 0 0 0 0 0 0 0 ; 0 0 0 0 0 0 0 0 0 0 0 0 ]; % Rotational relationship of end link frames diff --git a/src/robot/ini_HubRobo_v2_2_grip_to_spine_LP.m b/src/robot/ini_HubRobo_v2_2_grip_to_spine_LP.m index 512ac92..9a658fa 100644 --- a/src/robot/ini_HubRobo_v2_2_grip_to_spine_LP.m +++ b/src/robot/ini_HubRobo_v2_2_grip_to_spine_LP.m @@ -9,6 +9,16 @@ function LP = ini_HubRobo_v2_2_grip_to_spine_LP() +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force +LP.F_grip = 3.2; + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -37,6 +47,8 @@ LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; % Type of joint LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-60 60; -15 90; -135 0]; %[deg] % Position vector from base CoM to i-th joint LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; diff --git a/src/robot/ini_HubRobo_v2_2_no_grip_LP.m b/src/robot/ini_HubRobo_v2_2_no_grip_LP.m index 8798fff..d4081f7 100644 --- a/src/robot/ini_HubRobo_v2_2_no_grip_LP.m +++ b/src/robot/ini_HubRobo_v2_2_no_grip_LP.m @@ -8,6 +8,16 @@ function LP = ini_HubRobo_v2_2_no_grip_LP() +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force +LP.F_grip = 0.0; + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -36,6 +46,8 @@ LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; % Type of joint LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-60 60; -15 90; -135 0]; %[deg] % Position vector from base CoM to i-th joint LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; diff --git a/src/robot/ini_HubRobo_v3_1_grip_to_palm_LP.m b/src/robot/ini_HubRobo_v3_1_grip_to_palm_LP.m index 53153ed..fe483a6 100644 --- a/src/robot/ini_HubRobo_v3_1_grip_to_palm_LP.m +++ b/src/robot/ini_HubRobo_v3_1_grip_to_palm_LP.m @@ -10,6 +10,16 @@ function LP = ini_HubRobo_v3_1_grip_to_palm_LP() +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force +LP.F_grip = 9.0; + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -38,6 +48,9 @@ LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; % Type of joint LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-60 60; -15 90; -125 0]; %[deg] + % Position vector from base CoM to i-th joint LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; diff --git a/src/robot/ini_HubRobo_v3_2_grip_to_palm_LP.m b/src/robot/ini_HubRobo_v3_2_grip_to_palm_LP.m new file mode 100644 index 0000000..3e3028d --- /dev/null +++ b/src/robot/ini_HubRobo_v3_2_grip_to_palm_LP.m @@ -0,0 +1,121 @@ +%%%%%% Initialize +%%%%%% ini_HubRobo_v3_2_grip_to_palm_LP +%%%%%% +%%%%%% Mass/inertia parameters and link connections for HubRobo with grippers +%%%%%% Last link length is defined as the distance from +%%%%%% joint 3 (joint_Femur_to_Tibia) to the center of palm of gripper +%%%%%% +%%%%%% NOTE: all parameters are based on the HubRobo full CAD version 3.2 +%%%%%% +%%%%%% Created on 2020-11-09 by Kentaro Uno +%%%%%% Last updated on 2020-12-21 by Kentaro Uno + +function LP = ini_HubRobo_v3_2_grip_to_palm_LP() + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force +LP.F_grip = 9.0; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Define link connection relationship +LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; + +% Number of links/joints +LP.num_q = length(LP.BB); + +LP.SS = [ -1 1 0 0 0 0 0 0 0 0 0 0; + 0 -1 1 0 0 0 0 0 0 0 0 0; + 0 0 -1 0 0 0 0 0 0 0 0 0; + 0 0 0 -1 1 0 0 0 0 0 0 0; + 0 0 0 0 -1 1 0 0 0 0 0 0; + 0 0 0 0 0 -1 0 0 0 0 0 0; + 0 0 0 0 0 0 -1 1 0 0 0 0; + 0 0 0 0 0 0 0 -1 1 0 0 0; + 0 0 0 0 0 0 0 0 -1 0 0 0; + 0 0 0 0 0 0 0 0 0 -1 1 0; + 0 0 0 0 0 0 0 0 0 0 -1 1; + 0 0 0 0 0 0 0 0 0 0 0 -1]; +% Links connected to base (link 0) +LP.S0 = [ 1 0 0 1 0 0 1 0 0 1 0 0]; +% End link (leg) +LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; +% Type of joint +LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-60 60; -60 90; -160 0]; %[deg] + +% Position vector from base CoM to i-th joint +LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; + 0.054 0 0 0.054 0 0 -0.054 0 0 -0.054 0 0; + 0.000 0 0 0.000 0 0 0.000 0 0 0.000 0 0]; +% Mass of the base (link 0) <- copied from CAD file +LP.m0 = 0.4417225774; % [kg] +% Moment of inertia of base <- copied from CAD file +LP.inertia0 = [ 0.0009254132 0.000051036 0.0001792838 ; % [kg*m^2] + 0.000051036 0.0014081514 -0.0000140803 ; + 0.0001792838 -0.0000140803 0.0012652109 ]; + +% Mass of each link <- based on manual measurement +LP.m = [ 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ... + 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ... + 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ... + 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ]; % [kg] +% Total mass +LP.mass = sum(LP.m) + LP.m0; % [kg] +% Rotational relationship of links frames +LP.Qi = [ 0 pi/2 0 0 pi/2 0 0 pi/2 0 0 pi/2 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + pi/4 0 0 pi*3/4 0 0 pi*5/4 0 0 pi*7/4 0 0]; +% Moment of inertia of each link <- copied from hubrobo_v3_2_parameters.dat @TODO: current each last link inertial matrixes are replaced with the ones of Tibia links, and the coodinates are based on the URDF ones. These should be adjusted for SpaceDyn one.% [kg*m^2] +LP.inertia = [ 4.17250591050379E-05 -3.280237461361E-07 -3.28017174197894E-07 1.53134966250451E-05 1.56836205467462E-14 3.77019535347995E-15 2.20258150957412E-05 -3.2869671194301E-07 -3.31326402019975E-07 4.1725E-05 3.3003E-07 -3.3004E-07 1.53134966258197E-05 1.55870791195499E-14 4.18826057628801E-14 2.20258136176618E-05 -3.28696725826472E-07 -3.31325909357267E-07 4.17250600316099E-05 -3.28023694988862E-07 -3.28016569372976E-07 1.53134966250456E-05 1.56835951357578E-14 3.7701969258325E-15 2.20258152259854E-05 -3.28696694818991E-07 -3.31326354131817E-07 4.1725054229097E-05 3.30034360628209E-07 -3.30041495547628E-07 1.531349662582E-05 1.5587029991639E-14 4.18826171023906E-14 4.18826171023906E-14 -1.5341346770838E-14 3.67324197174481E-05; + -3.280237461361E-07 2.75916233673388E-05 6.28753118367121E-07 1.56836205467462E-14 2.40171123385755E-05 6.33438629632886E-16 -3.2869671194301E-07 1.76456846672338E-05 -3.14370002187497E-07 3.3003E-07 2.7592E-05 -6.1624E-07 1.55870791195499E-14 2.40171123385758E-05 -1.53413473909909E-14 -3.28696725826472E-07 1.76456831216546E-05 -3.14370065697366E-07 -3.28023694988862E-07 2.75916244818459E-05 6.28753097529483E-07 1.56835951357578E-14 2.40171123385754E-05 6.33432116769803E-16 -3.28696694818991E-07 1.76456848327505E-05 -3.14370044226523E-07 3.30034360628209E-07 2.75916215197804E-05 -6.16243771991015E-07 1.5587029991639E-14 2.40171123385757E-05 -1.5341346770838E-14 -3.2869672582661E-07 1.76456831216548E-05 -3.14370065697396E-07; + -3.28017174197894E-07 6.28753118367121E-07 2.75915908175676E-05 3.77019535347995E-15 6.33438629632886E-16 3.67324197182656E-05 -3.31326402019975E-07 -3.14370002187497E-07 1.19815307988945E-05 -3.3004E-07 -6.1624E-07 2.7592E-05 4.18826057628801E-14 -1.53413473909909E-14 3.67324197174477E-05 -3.31325909357267E-07 -3.14370065697366E-07 1.19815307394935E-05 -3.28016569372976E-07 6.28753097529483E-07 2.75915909936714E-05 3.7701969258325E-15 6.33432116769803E-16 3.67324197182661E-05 -3.31326354131817E-07 -3.1437E-07 1.19815308159254E-05 -3.30041495547628E-07 -6.16243771991015E-07 2.759158797365E-05 4.18826171023906E-14 -1.5341346770838E-14 3.67324197174481E-05 -3.31325909357463E-07 -3.14370065697396E-07 1.19815307394936E-05]; + +% Position vector from each link CoM to i-th joint +LP.cc = zeros( 3,12,12 ); + +% Postion vector from link 1 CoM to joint 1 +LP.cc(:,1,1) = [ -0.014224 0.00045844232 -0.0087645 ]'; +% Postion vector from link 1 CoM to joint 2 +LP.cc(:,1,2) = [ 0.014224 -0.00045844232 0.0087645 ]'; +LP.cc(:,2,2) = [ -0.053575 0 0 ]'; +LP.cc(:,2,3) = [ 0.053575 0 0 ]'; +LP.cc(:,3,3) = [ -0.07315 -0.0005969 -0.00039545]'; + +LP.cc(:,4,4) = [ -0.014224 0.00045844232 -0.0087645 ]'; +LP.cc(:,4,5) = [ 0.014224 -0.00045844232 0.0087645 ]'; +LP.cc(:,5,5) = [ -0.053575 0 0 ]'; +LP.cc(:,5,6) = [ 0.053575 0 0 ]'; +LP.cc(:,6,6) = [ -0.07315 -0.0005969 -0.00039545 ]'; + +LP.cc(:,7,7) = [ -0.014224 0.00045844232 -0.0087645 ]'; +LP.cc(:,7,8) = [ 0.014224 -0.00045844232 0.0087645 ]'; +LP.cc(:,8,8) = [ -0.053575 0 0 ]'; +LP.cc(:,8,9) = [ 0.053575 0 0 ]'; +LP.cc(:,9,9) = [ -0.07315 -0.0005969 -0.00039545 ]'; + +LP.cc(:,10,10) = [ -0.014224 0.00045844232 -0.0087645]'; +LP.cc(:,10,11) = [ 0.014224 -0.00045844232 0.0087645]'; +LP.cc(:,11,11) = [ -0.053575 0 0 ]'; +LP.cc(:,11,12) = [ 0.053575 0 0 ]'; +LP.cc(:,12,12) = [ -0.07315 -0.0005969 -0.00039545]'; + +% Position vector from end link CoM to end point +LP.ce = [ 0 0 0.07315 0 0 0.07315 0 0 0.07315 0 0 0.07315 ; + 0 0 0.0005969 0 0 0.0005969 0 0 0.0005969 0 0 0.0005969 ; + 0 0 0.00039545 0 0 0.00039545 0 0 0.00039545 0 0 0.00039545 ]; +% Rotational relationship of end link frames +LP.Qe = [ 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; + +% EOF \ No newline at end of file diff --git a/src/robot/ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N.m b/src/robot/ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N.m new file mode 100644 index 0000000..38605f6 --- /dev/null +++ b/src/robot/ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N.m @@ -0,0 +1,121 @@ +%%%%%% Initialize +%%%%%% ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N +%%%%%% +%%%%%% Mass/inertia parameters and link connections for HubRobo with grippers +%%%%%% Last link length is defined as the distance from +%%%%%% joint 3 (joint_Femur_to_Tibia) to the center of palm of gripper +%%%%%% +%%%%%% NOTE: all parameters are based on the HubRobo full CAD version 3.2 +%%%%%% +%%%%%% Created on 2021-06-07 by Keigo Haji +%%%%%% Last updated on 2021-06-07 by Keigo Haji + +function LP = ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N() + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of the robot performance %%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% state the type of the joint configuration -> IK analytical solution is switched +LP.joint_allocation_type = 'insect'; + +% Max. endurable gripping force [N] +LP.F_grip = 13.0; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%% Definition of each link parameters %%%%%%%%%%%%%%%%%%%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Define link connection relationship +LP.BB = [ 0 1 2 0 4 5 0 7 8 0 10 11]; + +% Number of links/joints +LP.num_q = length(LP.BB); + +LP.SS = [ -1 1 0 0 0 0 0 0 0 0 0 0; + 0 -1 1 0 0 0 0 0 0 0 0 0; + 0 0 -1 0 0 0 0 0 0 0 0 0; + 0 0 0 -1 1 0 0 0 0 0 0 0; + 0 0 0 0 -1 1 0 0 0 0 0 0; + 0 0 0 0 0 -1 0 0 0 0 0 0; + 0 0 0 0 0 0 -1 1 0 0 0 0; + 0 0 0 0 0 0 0 -1 1 0 0 0; + 0 0 0 0 0 0 0 0 -1 0 0 0; + 0 0 0 0 0 0 0 0 0 -1 1 0; + 0 0 0 0 0 0 0 0 0 0 -1 1; + 0 0 0 0 0 0 0 0 0 0 0 -1]; +% Links connected to base (link 0) +LP.S0 = [ 1 0 0 1 0 0 1 0 0 1 0 0]; +% End link (leg) +LP.SE = [ 0 0 1 0 0 1 0 0 1 0 0 1 ]; +% Type of joint +LP.J_type = [ 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' 'R' ]; +% Movable limitation of joint +LP.joint_limit=[-60 60; -60 90; -160 0]; %[deg] + +% Position vector from base CoM to i-th joint +LP.c0 = [ 0.054 0 0 -0.054 0 0 -0.054 0 0 0.054 0 0; + 0.054 0 0 0.054 0 0 -0.054 0 0 -0.054 0 0; + 0.000 0 0 0.000 0 0 0.000 0 0 0.000 0 0]; +% Mass of the base (link 0) <- copied from CAD file +LP.m0 = 0.4417225774; % [kg] +% Moment of inertia of base <- copied from CAD file +LP.inertia0 = [ 0.0009254132 0.000051036 0.0001792838 ; % [kg*m^2] + 0.000051036 0.0014081514 -0.0000140803 ; + 0.0001792838 -0.0000140803 0.0012652109 ]; + +% Mass of each link <- based on manual measurement +LP.m = [ 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ... + 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ... + 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ... + 0.205573522155681 0.0273056120035877 0.111252512707499 + 0.10891073026 ]; % [kg] +% Total mass +LP.mass = sum(LP.m) + LP.m0; % [kg] +% Rotational relationship of links frames +LP.Qi = [ 0 pi/2 0 0 pi/2 0 0 pi/2 0 0 pi/2 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + pi/4 0 0 pi*3/4 0 0 pi*5/4 0 0 pi*7/4 0 0]; +% Moment of inertia of each link <- copied from hubrobo_v3_2_parameters.dat @TODO: current each last link inertial matrixes are replaced with the ones of Tibia links, and the coodinates are based on the URDF ones. These should be adjusted for SpaceDyn one.% [kg*m^2] +LP.inertia = [ 4.17250591050379E-05 -3.280237461361E-07 -3.28017174197894E-07 1.53134966250451E-05 1.56836205467462E-14 3.77019535347995E-15 2.20258150957412E-05 -3.2869671194301E-07 -3.31326402019975E-07 4.1725E-05 3.3003E-07 -3.3004E-07 1.53134966258197E-05 1.55870791195499E-14 4.18826057628801E-14 2.20258136176618E-05 -3.28696725826472E-07 -3.31325909357267E-07 4.17250600316099E-05 -3.28023694988862E-07 -3.28016569372976E-07 1.53134966250456E-05 1.56835951357578E-14 3.7701969258325E-15 2.20258152259854E-05 -3.28696694818991E-07 -3.31326354131817E-07 4.1725054229097E-05 3.30034360628209E-07 -3.30041495547628E-07 1.531349662582E-05 1.5587029991639E-14 4.18826171023906E-14 4.18826171023906E-14 -1.5341346770838E-14 3.67324197174481E-05; + -3.280237461361E-07 2.75916233673388E-05 6.28753118367121E-07 1.56836205467462E-14 2.40171123385755E-05 6.33438629632886E-16 -3.2869671194301E-07 1.76456846672338E-05 -3.14370002187497E-07 3.3003E-07 2.7592E-05 -6.1624E-07 1.55870791195499E-14 2.40171123385758E-05 -1.53413473909909E-14 -3.28696725826472E-07 1.76456831216546E-05 -3.14370065697366E-07 -3.28023694988862E-07 2.75916244818459E-05 6.28753097529483E-07 1.56835951357578E-14 2.40171123385754E-05 6.33432116769803E-16 -3.28696694818991E-07 1.76456848327505E-05 -3.14370044226523E-07 3.30034360628209E-07 2.75916215197804E-05 -6.16243771991015E-07 1.5587029991639E-14 2.40171123385757E-05 -1.5341346770838E-14 -3.2869672582661E-07 1.76456831216548E-05 -3.14370065697396E-07; + -3.28017174197894E-07 6.28753118367121E-07 2.75915908175676E-05 3.77019535347995E-15 6.33438629632886E-16 3.67324197182656E-05 -3.31326402019975E-07 -3.14370002187497E-07 1.19815307988945E-05 -3.3004E-07 -6.1624E-07 2.7592E-05 4.18826057628801E-14 -1.53413473909909E-14 3.67324197174477E-05 -3.31325909357267E-07 -3.14370065697366E-07 1.19815307394935E-05 -3.28016569372976E-07 6.28753097529483E-07 2.75915909936714E-05 3.7701969258325E-15 6.33432116769803E-16 3.67324197182661E-05 -3.31326354131817E-07 -3.1437E-07 1.19815308159254E-05 -3.30041495547628E-07 -6.16243771991015E-07 2.759158797365E-05 4.18826171023906E-14 -1.5341346770838E-14 3.67324197174481E-05 -3.31325909357463E-07 -3.14370065697396E-07 1.19815307394936E-05]; + +% Position vector from each link CoM to i-th joint +LP.cc = zeros( 3,12,12 ); + +% Postion vector from link 1 CoM to joint 1 +LP.cc(:,1,1) = [ -0.014224 0.00045844232 -0.0087645 ]'; +% Postion vector from link 1 CoM to joint 2 +LP.cc(:,1,2) = [ 0.014224 -0.00045844232 0.0087645 ]'; +LP.cc(:,2,2) = [ -0.053575 0 0 ]'; +LP.cc(:,2,3) = [ 0.053575 0 0 ]'; +LP.cc(:,3,3) = [ -0.07315 -0.0005969 -0.00039545]'; + +LP.cc(:,4,4) = [ -0.014224 0.00045844232 -0.0087645 ]'; +LP.cc(:,4,5) = [ 0.014224 -0.00045844232 0.0087645 ]'; +LP.cc(:,5,5) = [ -0.053575 0 0 ]'; +LP.cc(:,5,6) = [ 0.053575 0 0 ]'; +LP.cc(:,6,6) = [ -0.07315 -0.0005969 -0.00039545 ]'; + +LP.cc(:,7,7) = [ -0.014224 0.00045844232 -0.0087645 ]'; +LP.cc(:,7,8) = [ 0.014224 -0.00045844232 0.0087645 ]'; +LP.cc(:,8,8) = [ -0.053575 0 0 ]'; +LP.cc(:,8,9) = [ 0.053575 0 0 ]'; +LP.cc(:,9,9) = [ -0.07315 -0.0005969 -0.00039545 ]'; + +LP.cc(:,10,10) = [ -0.014224 0.00045844232 -0.0087645]'; +LP.cc(:,10,11) = [ 0.014224 -0.00045844232 0.0087645]'; +LP.cc(:,11,11) = [ -0.053575 0 0 ]'; +LP.cc(:,11,12) = [ 0.053575 0 0 ]'; +LP.cc(:,12,12) = [ -0.07315 -0.0005969 -0.00039545]'; + +% Position vector from end link CoM to end point +LP.ce = [ 0 0 0.07315 0 0 0.07315 0 0 0.07315 0 0 0.07315 ; + 0 0 0.0005969 0 0 0.0005969 0 0 0.0005969 0 0 0.0005969 ; + 0 0 0.00039545 0 0 0.00039545 0 0 0.00039545 0 0 0.00039545 ]; +% Rotational relationship of end link frames +LP.Qe = [ 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0; + 0 0 0 0 0 0 0 0 0 0 0 0 ]; + +% EOF \ No newline at end of file diff --git a/src/robot/ini_joint_angle.m b/src/robot/ini_joint_angle.m index 84df0f1..c031da8 100644 --- a/src/robot/ini_joint_angle.m +++ b/src/robot/ini_joint_angle.m @@ -4,8 +4,9 @@ %%%%%% Calculate the initial joints angles given the initial end-effectors' positions and map points %%%%%% %%%%%% Created 2019-09-30 -%%%%%% Victoria Keo, Warley Ribeiro -%%%%%% Last update: 2020-02-25 +%%%%%% by Victoria Keo, Warley Ribeiro +%%%%%% Last update: 2021-05-17 +%%%%%% by Kentaro Uno % % % For a given set of points xp, obtain the closest points in the map regardless of the map's resolution, including the map @@ -14,26 +15,48 @@ % Function variables: % % OUTPUT -% q0 : Initial angular position for joints (1xn vector) +% q0 : Initial angular position for joints (1xn vector) % INPUT -% LP : Link Parameters (SpaceDyn class) -% SV : State Variables (SpaceDyn class) -% foot_dist : Horizontal distance from base center until end-effector position for both x and y coordinates [m] (scalar) - +% LP : Link Parameters (SpaceDyn class) +% SV : State Variables (SpaceDyn class) +% robot_param : Parameter for robot (class) +% x_foot_dist : Horizontal distance from base center until end-effector position for both x and y coordinates [m] (scalar) +% y_foot_dist : same as above +% surface_param : Parameters for surface (class) +% surface_param.graspable_points : 3xN matrix contains position vectors of graspable points [m] (x;y;z) + -function q0 = ini_joint_angle(LP,SV,foot_dist) +function q0 = ini_joint_angle(LP,SV,robot_param,x_foot_dist,y_foot_dist,surface_param) +% rotate the initial foot position based on the robot_param.initial_yaw +re0 = rpy2dc([0;0;-pi*robot_param.initial_yaw/180]) * [+x_foot_dist -x_foot_dist -x_foot_dist +x_foot_dist; ... + +y_foot_dist +y_foot_dist -y_foot_dist -y_foot_dist; ... + 0 0 0 0]; % End-effectors initial (x,y) positions -xe0 = [SV.R0(1)+foot_dist SV.R0(1)-foot_dist SV.R0(1)-foot_dist SV.R0(1)+foot_dist]; -ye0 = [SV.R0(2)+foot_dist SV.R0(2)+foot_dist SV.R0(2)-foot_dist SV.R0(2)-foot_dist]; - +xe0 = [SV.R0(1)+re0(1,1) SV.R0(1)+re0(1,2) SV.R0(1)+re0(1,3) SV.R0(1)+re0(1,4)]; +ye0 = [SV.R0(2)+re0(2,1) SV.R0(2)+re0(2,2) SV.R0(2)+re0(2,3) SV.R0(2)+re0(2,4)]; + +xe0 = xe0 - robot_param.x_base_pos_offset_from_the_neutral_pos; +ye0 = ye0 - robot_param.y_base_pos_offset_from_the_neutral_pos; + % Get the nearest points on the map and obtain height -[xe0,ye0,ze0] = get_map_pos(xe0,ye0) ; +[xe0,ye0,ze0] = get_map_pos_of_graspable_points(xe0,ye0,surface_param) ; POS_e0 = [xe0;ye0;ze0]; % Inverse kinematics for i = 1:LP.num_limb - q0(3*i-2:3*i) = get_i_kine_3dof(LP,SV,POS_e0(:,i) , i); + if LP.joint_allocation_type == 'mammal' + q0(3*i-2:3*i) = get_i_kine_mammal_config_3dof_limb(LP,SV,POS_e0(:,i) , i); + elseif LP.joint_allocation_type == 'insect' + q0(3*i-2:3*i) = get_i_kine_insect_config_3dof_limb(LP,SV,POS_e0(:,i) , i); + end + if LP.joint_limit(1,1)>= rad2deg( q0(3*i-2) ) || rad2deg( q0(3*i-2) ) >= LP.joint_limit(1,2) || ... + LP.joint_limit(2,1)>= rad2deg( q0(3*i-1) ) || rad2deg( q0(3*i-1) ) >= LP.joint_limit(2,2) || ... + LP.joint_limit(3,1)>= rad2deg( q0(3*i ) ) || rad2deg( q0(3*i ) ) >= LP.joint_limit(3,2) + disp("the solution of IK is as follows [deg]"); + disp( rad2deg(q0(3*i-2:3*i)) ); + error("in ini_joint_angle(): the solution of IK escapes from the LP.joint_limitation"); + end end q0 = q0'; diff --git a/src/robot/ini_reachable_area.m b/src/robot/ini_reachable_area.m index 80c4083..ba2d981 100644 --- a/src/robot/ini_reachable_area.m +++ b/src/robot/ini_reachable_area.m @@ -3,9 +3,10 @@ %%%%%% %%%%%% Calculate the reachable area and max./min. range from CoM %%%%%% -%%%%%% Created 2020-05-13 -%%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-06-24 by Kentaro Uno (changed the joint limitaion) +%%%%%% Created: 2020-05-13 +%%%%%% by Yusuke Koizumi +%%%%%% Last update: 2020-12-21 +%%%%%% by Kentaro Uno % % % Calculate a reachable area and and max./min. range from CoM @@ -19,26 +20,29 @@ % LP : Link Parameters (SpaceDyn class) % SV : State Variables (SpaceDyn class) % floor : height of floor [m] (scalar) -% joint_limit : min./max. position of joints in deg. (3x2 matrix) +% LP.joint_limit : min./max. position of joints in deg. (3x2 matrix) % HubRobo : joint1;joint2;joint3 => [-60 60; 0 90; -135 0] -function [ LP ] = ini_reachable_area(LP,SV,floor,joint_limit) +function [ LP ] = ini_reachable_area(LP,SV,floor) % set the reachable area from the limitations of the motion of joint servo motor -LP.joint_limit=joint_limit; SV.R0=zeros(3,1); % set the initial joint angles of the robot SV.q = zeros(12,1); +% Initial Base orientation +SV.Q0 = [0; 0; 0]; +SV.A0 = rpy2dc(-SV.Q0); + cnt = 1; -for j= joint_limit(2,1):0.5:joint_limit(2,2) % change of the link 2 when link 3 bends up maximumly +for j= LP.joint_limit(2,1):0.5:LP.joint_limit(2,2) % change of the link 2 when link 3 bends up maximumly SV.q = zeros(12,1); SV.q(1) = 45*pi/180; SV.q(2) = j*pi/180; -SV.q(3) = joint_limit(3,2)*pi/180; +SV.q(3) = LP.joint_limit(3,2)*pi/180; SV = calc_aa( LP, SV ); SV = calc_pos( LP, SV ); @@ -52,11 +56,11 @@ cnt = 1; -for j= joint_limit(2,1):0.5:joint_limit(2,2) % change of the link 2 when link 3 bends down minimumly +for j= LP.joint_limit(2,1):0.5:LP.joint_limit(2,2) % change of the link 2 when link 3 bends down minimumly SV.q = zeros(12,1); SV.q(1) = 45*pi/180; SV.q(2) = j*pi/180; -SV.q(3) = joint_limit(3,1)*pi/180; +SV.q(3) = LP.joint_limit(3,1)*pi/180; SV = calc_aa( LP, SV ); SV = calc_pos( LP, SV ); @@ -69,11 +73,11 @@ end cnt = 1; -for j= joint_limit(3,1):0.5:joint_limit(3,2) % change of the link 3 when link 2 bends down minimumly +for j= LP.joint_limit(3,1):0.5:LP.joint_limit(3,2) % change of the link 3 when link 2 bends down minimumly SV.q = zeros(12,1); SV.q(1) = 45*pi/180; -SV.q(2) = joint_limit(2,1)*pi/180; +SV.q(2) = LP.joint_limit(2,1)*pi/180; SV.q(3) = j*pi/180; SV = calc_aa( LP, SV ); @@ -87,11 +91,11 @@ end cnt = 1; -for j= joint_limit(3,1):0.5:joint_limit(3,2) % change of the link 3 when link 2 bends up maximumly +for j= LP.joint_limit(3,1):0.5:LP.joint_limit(3,2) % change of the link 3 when link 2 bends up maximumly SV.q = zeros(12,1); SV.q(1) = 45*pi/180; -SV.q(2) = joint_limit(2,2)*pi/180; +SV.q(2) = LP.joint_limit(2,2)*pi/180; SV.q(3) = j*pi/180; SV = calc_aa( LP, SV ); @@ -107,10 +111,11 @@ data_far =[RA.POS_e1_tmp3' RA.POS_e1_tmp1']; [~,I_min] = min(data_far(3,:)); -LP.reachable_area.data_near =[fliplr(data_far(:,1:I_min)) data_near ]; -LP.reachable_area.data_far =[data_far(:,I_min:length(data_far))]; +LP.reachable_area.data_near = [fliplr(data_far(:,1:I_min)) data_near ]; +LP.reachable_area.data_far = [data_far(:,I_min:length(data_far))]; + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%%% find the points where z = -0.08 (ground) out of dataset %%% +%%% find the points where z = floor (ground) out of dataset %%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% diff1 = abs( LP.reachable_area.data_far(3,:) - floor ); diff2 = abs( LP.reachable_area.data_near(3,:) - floor ); diff --git a/src/robot/ini_robot.m b/src/robot/ini_robot.m index 6e11b30..de1567d 100644 --- a/src/robot/ini_robot.m +++ b/src/robot/ini_robot.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2019-10-30 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-11 by Kentaro Uno +%%%%%% Last update: 2021-06-07 +%%%%%% Keigo Hajisa % % % Initialize robot's links parameters and state variables, including gripping force and end-effector positions and @@ -23,7 +24,7 @@ % cont_POS : Initial point of contact which is used as equilibrium position [m] (3xnum_limb matrix) % INPUT % robot_param : Parameter for robot (class) -% gait_param : Gait parameters +% gait_planning_param : Gait planning parameters % surface_param: Surface parameters % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -35,7 +36,7 @@ % SV.slip : vector to indicate if the i-th limb is slipping (SV.slip(i) = 1) or not (SV.slip(i) = 0)(1xn vector) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -function [LP, SV, des_SV, F_grip, POS_e, ORI_e, cont_POS] = ini_robot(robot_param, gait_param, surface_param) +function [LP, SV, des_SV, POS_e, ORI_e, cont_POS, ani_settings] = ini_robot(robot_param, gait_planning_param, surface_param, ani_settings) global contact_f global x ; global y ; global z; @@ -44,34 +45,34 @@ case 'HubRobo_grip_to_spine_old' LP = ini_HubRobo_grip_to_spine_LP_old(); - F_grip = 3.2; - % Calculate reachable area - joint_limit=[-60 60; 0 90; -135 0]; %[deg] case 'HubRobo_v2_2_no_grip' LP = ini_HubRobo_v2_2_no_grip_LP(); - F_grip = 0.0; - % Calculate reachable area - joint_limit=[-60 60; 0 90; -135 0]; %[deg] case 'HubRobo_v2_2_grip_to_spine' LP = ini_HubRobo_v2_2_grip_to_spine_LP(); - F_grip = 3.2; - % Calculate reachable area - joint_limit=[-60 60; -15 90; -135 0]; %[deg] case 'HubRobo_v2_2_grip_to_palm' LP = ini_HubRobo_v2_2_grip_to_palm_LP(); - F_grip = 3.2; - % Calculate reachable area - joint_limit=[-60 60; 0 90; -135 0]; %[deg] case 'HubRobo_v3_1_grip_to_palm' LP = ini_HubRobo_v3_1_grip_to_palm_LP(); - F_grip = 9.0; - % Calculate reachable area - joint_limit=[-60 60; -15 90; -125 0]; %[deg] - + +case 'HubRobo_v3_2_grip_to_palm' + LP = ini_HubRobo_v3_2_grip_to_palm_LP(); + +case 'ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N' + LP = ini_HubRobo_v3_2_grip_to_palm_LP_Fgrip13N(); + +case 'ANYmal_B' + LP = ini_ANYmal_B_LP(); + +case 'Climbing_ANYmal' + [LP, ani_settings] = ini_Climbing_ANYmal_LP(robot_param, ani_settings); + +case 'ALPHRED' + LP = ini_ALPHRED_LP(); + otherwise disp('invalid robot type'); @@ -88,6 +89,12 @@ else SV.R0 = [x(round(length(x)/2)) ; y(round(length(y)/2)) ; robot_param.base_height + surface_param.floor_level ] ; % center of the map end + +% Initial Base orientation +SV.Q0 = [0; 0;pi*robot_param.initial_yaw/180]; +SV.A0 = rpy2dc(-SV.Q0); + + % Supporting limb SV.sup = zeros(1,LP.num_limb); % Supporting limb flag SV.slip = zeros(1,LP.num_limb);% Supporting limb slip flag @@ -95,8 +102,9 @@ SV.sup = [1 1 1 1]; % Set initial joint angle [rad] -foot_dist = robot_param.foot_dist; -SV.q = ini_joint_angle(LP,SV,foot_dist); +x_foot_dist = robot_param.x_foot_dist; +y_foot_dist = robot_param.y_foot_dist; +SV.q = ini_joint_angle(LP,SV,robot_param,x_foot_dist,y_foot_dist,surface_param); %%% Links positions %%% SV = calc_aa( LP, SV ); @@ -116,7 +124,8 @@ % Initialize desired state des_SV = SV; -if strcmp(gait_param.type,'crawl_uno_ver') || strcmp(gait_param.type,'nonperiodic_uno_ver') - LP = ini_reachable_area(LP,SV, POS_e(3,1)-SV.R0(3,1),joint_limit); +if strcmp(gait_planning_param.type,'crawl_gait_for_discrete_footholds') || strcmp(gait_planning_param.type, 'diagonal_gait_for_discrete_footholds') || strcmp(gait_planning_param.type,'nonperiodic_gait_for_discrete_footholds') || strcmp(gait_planning_param.type,'GIA_opt_based_pose_planner') + LP = ini_reachable_area(LP,SV, POS_e(3,1)-SV.R0(3,1)); end + end \ No newline at end of file diff --git a/src/script/README.md b/src/script/README.md new file mode 100644 index 0000000..aae8c3e --- /dev/null +++ b/src/script/README.md @@ -0,0 +1,15 @@ +# main_iterative_sim.m + + +This script can reproduce the dataset used in the following publication. + + +``` +@inproceedings{uno2021simulation, + title={Simulation-Based Climbing Capability Analysis for Quadrupedal Robots}, + author={Kentaro Uno and Giorgio Valsecchi and Marco Hutter and Kazuya Yoshida}, + booktitle={Proceedings of the 24th International Conference on Climbing and Walking Robots and the Support Technologies for Mobile Machines (CLAWAR)}, + pages={}, + year={2021} +} +``` diff --git a/src/script/main_iterative_sim.m b/src/script/main_iterative_sim.m new file mode 100644 index 0000000..c7b0902 --- /dev/null +++ b/src/script/main_iterative_sim.m @@ -0,0 +1,281 @@ +%%%%%% MAIN +%%%%%% main_itterative_sim +%%%%%% +%%%%%% Main file for the itterative static analysis for the CLAWAR2021 +%%%%%% paper. Maintained by Kentaro Uno (unoken@dc.tohoku.ac.jp). +%%%%%% +%%%%%% -------------------------------------------------------------------- +%%%%%% This configration file can reproduce the simulation result used in +%%%%%% CLAWAR 2021 proceedings paper by K. Uno and G. Valsecchi et al. +%%%%%% Proceedings Paper URL: +%%%%%% https://******** (to be added) +%%%%%% -------------------------------------------------------------------- +%%%%%% +%%%%%% Created: 2021-02-17 +%%%%%% by Kentaro Uno +%%%%%% Last update: 2021-07-28 +%%%%%% by Kentaro Uno + +clc; clear; close all; +tic; ini_path(); variables_saved = []; evaluation_param = []; + +%%% Select configuration: +%%% - 'clawar_2021_statistical_analysis' +config = 'clawar_2021_statistical_analysis'; + +%%% Load all initial parameters from configuration files stored in config/ +[robot_param, environment_param, gait_planning_param, control_param, equilibrium_eval_param, ani_settings, save_settings, ... + plot_settings, gripper_param, map_param, matching_settings, sensing_camera_param] = config_simulation(config); + +%%% Define a code for current set of simulations +run_cod = 'clawar_2021_statistical_analysis'; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize environment %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +global d_time; global Gravity; global Ez; global contact_f; global x; global y; global z; + +ini_environment(environment_param); +surface_param = ini_surface(environment_param); +surface_param = ini_graspable_points(environment_param, surface_param, gripper_param, map_param, matching_settings); + +%%% For the static analysis, we want to itterate the simulation with randomized +%%% base positions + +base_pos_plus_metrics = []; % prepare the array to store the metrics +RMSJointTorqueArray = []; % prepare the array for convergence detection +i = 1; + +base_pos_x_min = 0.00; base_pos_x_max = 0.15; +base_pos_y_min = 0.00; base_pos_y_max = 0.15; +base_pos_z_min = 0.30; base_pos_z_max = 0.30; +for base_pos_x = base_pos_x_min:0.015:base_pos_x_max + for base_pos_y = base_pos_y_min:0.015:base_pos_y_max + for base_pos_z = base_pos_z_min:0.015:base_pos_z_max + + variables_saved = []; + + %%% Initial x and y "offset" position of the base from the neutral position of the base center [m] + robot_param.x_base_pos_offset_from_the_neutral_pos = base_pos_x; + robot_param.y_base_pos_offset_from_the_neutral_pos = base_pos_y; + %%% Height of base relative to map [m] + robot_param.base_height = base_pos_z; + %%% Base position [m] 2x1 vector. or 'default' for default setting + robot_param.base_pos_xy = [base_pos_x;base_pos_y]; + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize robot %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + [LP, SV, des_SV, POS_e, ORI_e, cont_POS, ani_settings] = ini_robot(robot_param, gait_planning_param, surface_param, ani_settings); + shape_robot = vis_create_robot_model(ani_settings, LP); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize gait %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + [gait_planning_param, motion_planning_param] = ini_gait(gait_planning_param, SV); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Initialize files %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + [run_id, run_date] = ini_id(robot_param, environment_param, gait_planning_param, control_param); + video = ini_video_file(ani_settings, run_cod, run_id, run_date); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Simulation loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + for time = 0:d_time:environment_param.time_max + % Display time + disp(time) + + % if norm(SV.R0(1:2) - gait_planning_param.goal(1:2)) < 0.02 + % break + % end + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Gait Planning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + gait_planning_param = upd_gait_planning(gait_planning_param, surface_param, des_SV, SV, LP, POS_e, ... + environment_param, robot_param, time); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Motion Planning %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + [motion_planning_param, des_SV] = upd_motion_planning(motion_planning_param, gait_planning_param, ... + LP, SV, des_SV, cont_POS, POS_e, time); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Equilibrium Evaluation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + equilibrium_eval_param = upd_equilibrium_eval(equilibrium_eval_param, surface_param, ani_settings, LP, SV, POS_e); + + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Robot controller %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + SV = upd_control(SV, des_SV, control_param); + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Kinematics and dynamics %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + cont_POS = upd_collision_endeffector(LP, SV, POS_e, cont_POS); + SV = upd_ground_reaction_forces_spring_damper(LP, SV, surface_param, POS_e, cont_POS); + SV = upd_fwd_dynamics(environment_param, LP, SV, des_SV); + [POS_e, Qe_deg, Q0_deg, SV] = upd_fwd_kinematics(LP, SV); + + evaluation_param = upd_manipulability(LP,SV,evaluation_param); + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Save selected variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + variables_saved = upd_variables_saved(variables_saved, save_settings, time, LP, SV, POS_e, environment_param, equilibrium_eval_param, evaluation_param, gait_planning_param); + + SV = upd_grasp_detach(LP, SV, des_SV, environment_param, equilibrium_eval_param, variables_saved); + % SV.sup = des_SV.sup; + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Climbing animation %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% % Visualization +% if rem(time,1/ani_settings.frame_rate) == 0 && strcmp(ani_settings.display,'on') +% inc = environment_param.inc; +% figure(1); clf; hold on; +% vis_robot(LP, SV, POS_e, shape_robot, inc, ani_settings); +% ani_settings = vis_animation_range(motion_planning_param,surface_param,ani_settings,inc); +% vis_surface(inc, ani_settings); +% vis_graspable_points(surface_param,inc,ani_settings); +% vis_graspable_points_in_reachable_area(gait_planning_param, inc, ani_settings); +% vis_reachable_area(SV, LP, gait_planning_param, inc, ani_settings, surface_param); +% vis_goal(gait_planning_param, inc, ani_settings); +% vis_support_triangle(SV,POS_e,inc,ani_settings); +% vis_com_projection(LP, SV, inc, ani_settings); +% vis_next_desired_position(gait_planning_param, inc, ani_settings); +% vis_stability_polyhedron(inc, equilibrium_eval_param, ani_settings); +% vis_vectors(inc,ani_settings,equilibrium_eval_param,LP,SV); +% vis_animation_settings(ani_settings,surface_param,time); +% vis_trajectory_4legged(environment_param,ani_settings,variables_saved); +% vis_sensing_camera_fov(SV, inc, sensing_camera_param, ani_settings); +% writeVideo(video,getframe(1)); +% end + + %% %%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% coinvergence detection + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% if the previous 10 data every 0.02 sec is within the preset + %%% variance, it is detected as convergent, and escape from + %%% this for loop without waiting until time_max + if rem(time, 0.02) == 0 + RMSJointTorqueArray = [RMSJointTorqueArray; rms(variables_saved.tau(end,:))]; + end + if time >= 0.2 + lastRMSJointTorqueArray = RMSJointTorqueArray(size(RMSJointTorqueArray,1)-(10-1):size(RMSJointTorqueArray,1)); + meanLastRMSJointTorqueArray = mean(lastRMSJointTorqueArray); + convergence_is_OK = true; + % + threshold_to_detect_convergence = 0.5; + for count = 1:1:10 + if abs( RMSJointTorqueArray(size(RMSJointTorqueArray,1)-(count-1)) - meanLastRMSJointTorqueArray ) > threshold_to_detect_convergence + convergence_is_OK = false; + end + end + + if convergence_is_OK == true + break + end + end + + end + %% + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Saving variables %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + close(video); + data = sav_data_file(variables_saved, save_settings, run_cod, run_id, run_date, config); + % % Plot graphs + % vis_plot_graph(data, plot_settings, LP, inc, surface_param, gait_planning_param); + + toc + %% + %%% Extraxt the each metric: TSM, GIA margin, torque, and manipulability from the one loop result + % 0) store the base position + static_analyzation_dataset.base_position_x(i,1) = data.R0(1,1); + static_analyzation_dataset.base_position_y(i,1) = data.R0(1,2); + static_analyzation_dataset.base_position_z(i,1) = data.R0(1,3); + + % maximum torque in all the joints + static_analyzation_dataset.max_absolute_torque_of_all_joints(i,1) = max( abs(data.tau(end,:)) ); + % root mean square value of the torque + static_analyzation_dataset.rms_torque_of_all_joints(i,1) = rms( data.tau(end,:) ); + % average of the manipularbility measure + static_analyzation_dataset.mean_manipularbility_measure_of_four_limbs(i,1) = mean([data.manipulability1(end) data.manipulability2(end) ... + data.manipulability3(end) data.manipulability4(end)]); + % minimum of the manipularbility measure + static_analyzation_dataset.min_manipularbility_measure_of_four_limbs(i,1) = min([data.manipulability1(end) data.manipulability2(end) ... + data.manipulability3(end) data.manipulability4(end)]); + % maximum of the manipularbility measure + static_analyzation_dataset.max_manipularbility_measure_of_four_limbs(i,1) = max([data.manipulability1(end) data.manipulability2(end) ... + data.manipulability3(end) data.manipulability4(end)]); + + % average of TSM + static_analyzation_dataset.tsm(i,1) = data.tsm(end); + % average of TSM + static_analyzation_dataset.gia_margin(i,1) = data.gia_margin(end); +% % finally, metrics are coupled with the base position vector +% base_pos_plus_metrics(i,:) = [ data.R0(1,:) max_torque rms_torque mean_manipularbility mean_tsm mean_gia_margin]; +% + i = i + 1; + end + end +end +%% +% Convert to table and save csv +% dir_name = ['dat/' run_cod '/' run_id]; +dataset = struct2table(static_analyzation_dataset); +writetable(dataset,['dat/' run_cod '/' run_date '_static_analyzation_dataset.csv']); + +% %% set the robot pose neutral agian before the figure visualization +% %%% Initial x and y "offset" position of the base from the neutral position of the base center [m] +% robot_param.x_base_pos_offset_from_the_neutral_pos = 0; +% robot_param.y_base_pos_offset_from_the_neutral_pos = 0; +% %%% Height of base relative to map [m] +% robot_param.base_height = ( base_pos_z_min + base_pos_z_max ) / 2; +% %%% Base position [m] 2x1 vector. or 'default' for default setting +% robot_param.base_pos_xy = [( base_pos_x_min + base_pos_x_max ) / 2; ( base_pos_y_min + base_pos_y_max ) / 2]; +% [LP, SV, des_SV, POS_e, ORI_e, cont_POS, ani_settings] = ini_robot(robot_param, gait_planning_param, surface_param, ani_settings); +% shape_robot = vis_create_robot_model(ani_settings, LP); + +inc = environment_param.inc; + +% Rotation matrix +rot = rpy2dc([0;pi*inc/180;0])'; +for j=1:size(dataset,1) + base_positions(j,1:3) = ( rot' * [dataset.base_position_x(j,1) dataset.base_position_y(j,1) dataset.base_position_z(j,1)]' )'; +end + +%% +for fig_num = 1 : 1 : 5 + figure(fig_num); clf; hold on; + vis_robot(LP, SV, POS_e, shape_robot, inc, ani_settings); + ani_settings = vis_animation_range(motion_planning_param,surface_param,ani_settings,inc); + vis_surface(inc, ani_settings, environment_param, surface_param); + vis_graspable_points(surface_param,inc,ani_settings); + vis_graspable_points_in_reachable_area(gait_planning_param, inc, ani_settings); + vis_reachable_area(SV, LP, gait_planning_param, inc, ani_settings, surface_param); + vis_goal(gait_planning_param, inc, ani_settings); + vis_support_triangle(SV,LP,POS_e,inc,ani_settings); + gait_planning_param = vis_com_projection(LP, SV, inc, ani_settings, gait_planning_param); + vis_next_desired_position(gait_planning_param, inc, ani_settings); + vis_stability_polyhedron(inc, equilibrium_eval_param, ani_settings); + vis_vectors(inc,ani_settings,equilibrium_eval_param,LP,SV,POS_e,gait_planning_param,sensing_camera_param); + vis_animation_settings(ani_settings,surface_param,time); + vis_com_proj_trajectory(gait_planning_param, ani_settings, inc); + vis_trajectory_4legged(environment_param, motion_planning_param, ani_settings); + vis_sensing_camera_fov(SV, inc, sensing_camera_param, ani_settings, surface_param); + vis_sensed_graspable_points(surface_param, inc, ani_settings); + vis_global_path(gait_planning_param, inc, ani_settings); + vis_goal(gait_planning_param, inc, ani_settings); + % writeVideo(video,getframe(1)); + + x_for_vis = base_positions(:,1); + y_for_vis = base_positions(:,2); + z_for_vis = base_positions(:,3); + % c = base_pos_plus_metrics(:,4); + scatter3( x_for_vis, y_for_vis, z_for_vis, 50, dataset.gia_margin(:,1), 'filled', 'MarkerFaceAlpha', 0.5); + + % Axis, grid and camera angle (copied from vis_animationa_settings) + axis equal; grid on; view(ani_settings.camera_az,ani_settings.camera_el); + + % colomap and bar + colormap(jet); + colorbar; + + % title and labels (copied from vis_animationa_settings) + title('distribution'); + xlabel('\it{x} \rm{[m]}','FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); + ylabel('\it{y} \rm{[m]}','FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); + zlabel('\it{z} \rm{[m]}','FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); + xlim(ani_settings.x_lim); ylim(ani_settings.y_lim); zlim(ani_settings.z_lim); + + data = sav_data_file(variables_saved, save_settings, run_cod, run_id, run_date, config); + % Plot graphs + vis_plot_graph(data, plot_settings, LP, inc, surface_param, gait_planning_param); +end + +disp("Itterative calculation is done. The analysis dataset results are produced under the following folder: ") +disp("dat/") +disp(run_cod) + + +% EOF \ No newline at end of file diff --git a/src/sensing/ini_sensed_graspable_points.m b/src/sensing/ini_sensed_graspable_points.m new file mode 100644 index 0000000..798e9af --- /dev/null +++ b/src/sensing/ini_sensed_graspable_points.m @@ -0,0 +1,70 @@ +%%%%%% Initialize +%%%%%% ini_sensed_points.m +%%%%%% +%%%%%% Initialize sensed graspable points +%%%%%% +%%%%%% Created 2021-02-15 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-02-25 +%%%%%% Keigo Haji +%%%%%% +% +% Initialize the sensed graspable points based on the initial robot base +% position +% +% Function variables: +% +% OUTPUT +% surface_param.sensed_graspable_points : +% graspable points that have ever been in the camera's FOV +% 3*n matrix +% +% +% INPUT +% SV : State Variables (SpaceDyn class) +% RO : Initial position of the robot +% surface_param : Surface parameters +% graspable_points : All the graapable points on the map (3*n) +% sensing_camera_param : Sensing Camera parameters +% +% + +function[surface_param] = ini_sensed_graspable_points(SV, surface_param, sensing_camera_param) + +if strcmp(sensing_camera_param.sensing_flag, 'on') + % Prepare a matrix that stores the points within the camera range + [n,m] = size(surface_param.graspable_points); + surface_param.sensed_graspable_points = NaN(n,m); + + if strcmp(sensing_camera_param.known_area_shape, 'rectangle') + + % Set the sensed graspable points which the robot has already done + % sensing before the initial position + for i = 1:length(surface_param.graspable_points) + % For the x-axis + if surface_param.graspable_points(1,i) < SV.R0(1)+sensing_camera_param.ini_margin_from_base_pos_to_plus_x + % For the y-axis + if (surface_param.graspable_points(2,i) < SV.R0(2) +sensing_camera_param.ini_margin_from_base_pos_to_y) && (surface_param.graspable_points(2,i) > SV.R0(2) - sensing_camera_param.ini_margin_from_base_pos_to_y) + % Add the point positons to the matrix. + surface_param.sensed_graspable_points(:,i) = surface_param.graspable_points(:,i); + end + end + end + end + if strcmp(sensing_camera_param.known_area_shape, 'circle') + % Convert the coordinates of graspable points to coordinates from the base center. + graspable_points_pos_from_base_pos = surface_param.graspable_points - SV.R0; + for i = 1:length(surface_param.graspable_points) + % Calculate the distance from the base pos to each graspable + % point + distance_from_base_pos = sqrt(graspable_points_pos_from_base_pos(1,i)^2 + graspable_points_pos_from_base_pos(2,i)^2); + if distance_from_base_pos < sensing_camera_param.ini_circular_radius_from_base_pos + surface_param.sensed_graspable_points(:,i) = surface_param.graspable_points(:,i); + end + end + end + +end + +end + diff --git a/src/sensing/upd_sensed_graspable_points.m b/src/sensing/upd_sensed_graspable_points.m new file mode 100644 index 0000000..97e12f4 --- /dev/null +++ b/src/sensing/upd_sensed_graspable_points.m @@ -0,0 +1,83 @@ +%%%%%% Update +%%%%%% upd_sensed_points.m +%%%%%% +%%%%%% Update sensed graspable points +%%%%%% +%%%%%% Created 2021-02-15 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-02-25 +%%%%%% Keigo Haji +%%%%%% +% +% Update sensed_graspable_points with the points that entered the camera's +% angle of view +% +% +% Function variables: +% +% OUTPUT +% surface_param.sensed_graspable_points : +% graspable points that have ever been in the camera's FOV +% 3*n matrix +% +% INPUT +% SV : State Variables (SpaceDyn class) +% surface_param : Surface parameters +% graspable_points : All the graapable points on the map (3*n) +% sensing_camera_param : Sensing Camera parameters +% +% + +function [ surface_param ] = upd_sensed_graspable_points(SV, surface_param, sensing_camera_param) + +if strcmp(sensing_camera_param.sensing_flag, 'on') + if strcmp(sensing_camera_param.sensing_type, 'RealSense_d435i') + % Converts the camera orientation to the direction cosine in the + % reference coordinate system + camera_ori = SV.A0 * rpy2dc(sensing_camera_param.mounting_angle); + + % Calvulate the inverse matrix + camera_ori_inv = camera_ori'; + + % Find the camera position in the reference coordinate system + camera_pos = SV.R0 + SV.A0 * sensing_camera_param.mounting_position; + + % Transform the coordinates of the map points from the reference + % coordinate system into RealSense camera coordinate system + graspable_points_in_camera_coordinate = camera_ori_inv * (surface_param.graspable_points - camera_pos); + + + for i = 1:length(surface_param.graspable_points) + % Check the distance between the point and the camera in the x-axis + % direction in the camera coordinate system. + if graspable_points_in_camera_coordinate(1,i) > sensing_camera_param.fov_min_distance && graspable_points_in_camera_coordinate(1,i) < sensing_camera_param.fov_max_distance + % Check if the angle made in xy coordinates is in the + % horizontal fov of the camera. + theta_xy = atan( graspable_points_in_camera_coordinate(2,i)/graspable_points_in_camera_coordinate(1,i) ); + if theta_xy < sensing_camera_param.fov_horizontal*0.5 && theta_xy > -sensing_camera_param.fov_horizontal*0.5 + % Check if the angle made in xz coordinates is in the + % vertical fov of the camera. + theta_xz = atan(graspable_points_in_camera_coordinate(3,i)/graspable_points_in_camera_coordinate(1,i)); + if theta_xz < sensing_camera_param.fov_vertical*0.5 && theta_xz > -sensing_camera_param.fov_vertical*0.5 + % Add the point's positon to the matrix. + surface_param.sensed_graspable_points(:,i) = surface_param.graspable_points(:,i); + end + end + end + end + end + if strcmp(sensing_camera_param.sensing_type, 'circle') + % Convert the coordinates of graspable points to coordinates from the base center. + graspable_points_pos_from_base_pos = surface_param.graspable_points - SV.R0; + for i = 1:length(surface_param.graspable_points) + % Calculate the distance from the base projection on the + % ground to each graspable point + distance_from_base_proj = sqrt(graspable_points_pos_from_base_pos(1,i)^2 + graspable_points_pos_from_base_pos(2,i)^2); + if distance_from_base_proj < sensing_camera_param.circular_radius_from_base_pos + surface_param.sensed_graspable_points(:,i) = surface_param.graspable_points(:,i); + end + end + end + +end +end \ No newline at end of file diff --git a/src/visualization/vis_animation_settings.m b/src/visualization/vis_animation_settings.m index db6c3b2..92d623f 100644 --- a/src/visualization/vis_animation_settings.m +++ b/src/visualization/vis_animation_settings.m @@ -4,7 +4,8 @@ %%%%%% %%%%%% Created 2020-04-09 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-09 +%%%%%% Last update: 2020-03-16 +%%%%%% Kentaro Uno % % % Modify three-dimensional figure parameters @@ -31,7 +32,10 @@ function vis_animation_settings(ani_settings, surface_param, time) %%% Definition of files for writing simulation results %%% % Labels -xlabel({'\it{x} \rm{[m]}';['Time: ',num2str(time,'%.2f'), ' [s]']},'FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); +xlabel({'\it{x} \rm{[m]}';['Time: ',num2str(time,'%.2f'), ' [s] ']},'FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); +if strcmp(ani_settings.animation_elapsed_time_show,'off') + xlabel('\it{x} \rm{[m]}','FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); +end ylabel('\it{y} \rm{[m]}','FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); zlabel('\it{z} \rm{[m]}','FontName',ani_settings.font_name,'FontSize',ani_settings.font_size); % Define fonts diff --git a/src/visualization/vis_com_proj_trajectory.m b/src/visualization/vis_com_proj_trajectory.m new file mode 100644 index 0000000..d9cfc79 --- /dev/null +++ b/src/visualization/vis_com_proj_trajectory.m @@ -0,0 +1,46 @@ +%%%%%% Results +%%%%%% vis_CoM_proj_trajectory +%%%%%% +%%%%%% Draw trajectory of CoM projection +%%%%%% +%%%%%% Created 2021-06-24 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-07-09 +%%%%%% Keigo Haji +% +% +% Draw trajectory of CoM projection +% +% Function variables: +% +% OUTPUT +% - +% INPUT +% gait_planning_param : Parameters for gait planning +% gait_planning_param.com_projection_history : 3*n matrix of com +% projection history which is saved in vis_com_projection. The +% thrid row is 0 +% +% ani_settings : Settings for animation +% +function vis_com_proj_trajectory(gait_planning_param, ani_settings, inc) + +% Visualize Trajectory +if strcmp(ani_settings.com_proj_trajectory_show,'on') + + + % Rotation matrix + rot = rpy2dc([0;pi*inc/180;0])'; + % Rotate vectors + traj = rot'* gait_planning_param.com_projection_history; + + color = ani_settings.com_proj_trajectory_color; + width = ani_settings.com_proj_trajectory_width; + line_type = ani_settings.com_proj_trajectory_line_type; + + plot3(traj(1,:),traj(2,:),traj(3,:), line_type,'Color',color,'LineWidth',width); + +end + +end + diff --git a/src/visualization/vis_com_projection.m b/src/visualization/vis_com_projection.m index 6d62433..a31758a 100644 --- a/src/visualization/vis_com_projection.m +++ b/src/visualization/vis_com_projection.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-05-28 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-10-16 +%%%%%% Last update: 2021-05-17 %%%%%% Kentaro Uno % % @@ -14,7 +14,7 @@ % Function variables: % % OUTPUT -% - +% gait_planning_param.com_proj_history : com projection history % INPUT % LP : Link parameters (SpaceDyn class) % SV : State Variables @@ -27,12 +27,26 @@ % ani_settings.com_projection_show : on / off -function vis_com_projection(LP, SV, inc, ani_settings) +function gait_planning_param = vis_com_projection(LP, SV, inc, ani_settings, gait_planning_param) + +CoM = upd_CoM(LP, SV); + if strcmp(ani_settings.com_projection_show,'on') - CoM = upd_CoM(LP, SV); [a,b,c] = get_map_pos(CoM(1),CoM(2)); c = c + ani_settings.com_projection_vis_height; com_proj = rpy2dc([0;pi*inc/180;0])*[CoM(1)-norm(CoM(3)-c)*tan(inc*pi/180);CoM(2);c]; plot3(com_proj(1,1),com_proj(2,1),com_proj(3,1),ani_settings.com_projection_marker,'Color',ani_settings.com_projection_color,'MarkerSize',ani_settings.com_projection_size); end + +if strcmp(ani_settings.robot_top_com_projection_show,'on') + [a,b,c] = get_map_pos(CoM(1),CoM(2)); + c = c + ani_settings.com_projection_vis_height; + com_proj = rpy2dc([0;pi*inc/180;0])*[CoM(1)-norm(CoM(3)-c)*tan(inc*pi/180);CoM(2);c]; + base_position_in_inertial_frame = rpy2dc([0;pi*inc/180;0]) * SV.R0; + plot3(com_proj(1,1),com_proj(2,1),base_position_in_inertial_frame(3)+2.0*CoM(3), ani_settings.com_projection_marker,'Color',ani_settings.com_projection_color,'MarkerSize',ani_settings.com_projection_size); +end + + % log the history path of the CoM projection + com_proj_hist = rpy2dc([0;pi*inc/180;0])*[CoM(1)-norm(CoM(3))*tan(inc*pi/180);CoM(2);0]; + gait_planning_param.com_projection_history = [gait_planning_param.com_projection_history com_proj_hist]; end \ No newline at end of file diff --git a/src/visualization/vis_create_robot_model.m b/src/visualization/vis_create_robot_model.m index a22259e..17e830b 100644 --- a/src/visualization/vis_create_robot_model.m +++ b/src/visualization/vis_create_robot_model.m @@ -3,9 +3,10 @@ %%%%%% %%%%%% Create links of the robot for 3D plot %%%%%% -%%%%%% Created 2020-04-08 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 +%%%%%% Created: 2020-04-08 +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2021-01-18 +%%%%%% by Kentaro Uno % % % Create the shapes for the links of the robot. Each link is made from a hexagonal shape and the base is a rectangular one. @@ -18,39 +19,45 @@ % shape_robot.F_leg : Faces numbering for legs' links (10x4 matrix) % shape_robot.grip_F : Faces numbering for gripper parts (6x4 matrix) % shape_robot.grip_V : Vertices for gripper parts (8x3) -% shape_robot.base_V : Vertices for base (8x3) -% shape_robot.base_F : Faces numbering for base (6x4 matrix) +% shape_robot.base_upper_V : Vertices for upper part of the base (8x3) +% shape_robot.base_upper_F : Faces numbering for upper part of the base (6x4 matrix) +% shape_robot.base_lower_V : Vertices for lower part of the base (8x3) +% shape_robot.base_lower_F : Faces numbering for lower part of the base (6x4 matrix) % INPUT % LP : Link parameters (SpaceDyn class) -% F_grip : Maximum gripping force [N] (scalar) +% LP.F_grip : Maximum gripping force [N] (scalar) % ani_settings.link_radius : Radius for the links [m] (scalar) -% ani_settings.base_thickness : Thickness of the base [m] (scalar) +% ani_settings.base_upper_thickness : Upper thickness of the base [m] (scalar) +% ani_settings.base_lower_thickness : Lower thickness of the base [m] (scalar) -function shape_robot = vis_create_robot_model(ani_settings, LP, F_grip) +function shape_robot = vis_create_robot_model(ani_settings, LP) if strcmp(ani_settings.display,'on') - - % Create links - for i = 1:3 - for j = 1:6 - shape_robot.V_leg(j,:,i) = (rpy2dc([(j-1)/3*pi;0;0])'*[LP.cc(1,i,i); ani_settings.link_radius;0])'; - end - for j = 7:12 - shape_robot.V_leg(j,:,i) = (rpy2dc([(j-1)/3*pi;0;0])'*[-LP.cc(1,i,i); ani_settings.link_radius;0])'; - end - shape_robot.F_leg(:,:,i) = [1 2 3 6; - 3 4 5 6; - 1 2 8 7; - 2 3 9 8; - 3 4 10 9; - 4 5 11 10; - 5 6 12 11; - 6 1 7 12; - 7 8 9 12; - 9 10 11 12]; - end - if F_grip > 0 + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + %%% Old code for link visualization part + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% % Create links +% for i = 1:3 +% for j = 1:6 +% shape_robot.V_leg(j,:,i) = (rpy2dc([(j-1)/3*pi;0;0])'*[LP.cc(1,i,i); ani_settings.link_radius;0])'; +% end +% for j = 7:12 +% shape_robot.V_leg(j,:,i) = (rpy2dc([(j-1)/3*pi;0;0])'*[-LP.cc(1,i,i); ani_settings.link_radius;0])'; +% end +% shape_robot.F_leg(:,:,i) = [1 2 3 6; +% 3 4 5 6; +% 1 2 8 7; +% 2 3 9 8; +% 3 4 10 9; +% 4 5 11 10; +% 5 6 12 11; +% 6 1 7 12; +% 7 8 9 12; +% 9 10 11 12]; +% end + + if LP.F_grip > 0 % Create gripper shape_robot.grip_F = [1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8]; finger_length = 1.75*ani_settings.link_radius; @@ -64,16 +71,26 @@ finger_length -finger_thickness/2 -0.2*finger_thickness; -finger_length -finger_thickness/2 -0.2*finger_thickness]; end - + % Create base + % upper box of the base + scale = ani_settings.base_xy_scale_factor; for i = 1:LP.num_limb - shape_robot.base_V(i,:) = [LP.c0(1:2,3*i-2)' ani_settings.base_thickness/2]; + shape_robot.base_upper_V(i,:) = [LP.c0(1:2,3*i-2)'*scale ani_settings.base_upper_thickness]; end for i = 1:LP.num_limb - shape_robot.base_V(LP.num_limb+i,:) = [LP.c0(1:2,3*i-2)' -ani_settings.base_thickness/2]; + shape_robot.base_upper_V(LP.num_limb+i,:) = [LP.c0(1:2,3*i-2)'*scale 0]; end - shape_robot.base_F = [1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8]; - + shape_robot.base_upper_F = [1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8]; + + % lower box of the base + for i = 1:LP.num_limb + shape_robot.base_lower_V(i,:) = [LP.c0(1:2,3*i-2)'*scale 0]; + end + for i = 1:LP.num_limb + shape_robot.base_lower_V(LP.num_limb+i,:) = [LP.c0(1:2,3*i-2)'*scale -ani_settings.base_lower_thickness]; + end + shape_robot.base_lower_F = [1 2 3 4; 1 2 6 5; 2 3 7 6; 3 4 8 7; 1 4 8 5; 5 6 7 8]; else shape_robot = []; diff --git a/src/visualization/vis_cylinder.m b/src/visualization/vis_cylinder.m new file mode 100644 index 0000000..53ee490 --- /dev/null +++ b/src/visualization/vis_cylinder.m @@ -0,0 +1,109 @@ +%%%%%% Initialize +%%%%%% vis_cylinder +%%%%%% +%%%%%% isualize the cylinder connect any two points X1 and X2 +%%%%%% +%%%%%% Created: 2021-01-19 +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2021-01-19 +%%%%%% by Kentaro Uno +% +% Note: this function is created based on the following function. Based on +% the functon, the transparency of the face is allowed to be editable. +% ------------------------------------------------------------------------ +% J. Divahar (2021). Cylinder (https://www.mathworks.com/matlabcentral/fileexchange/13995-cylinder), MATLAB Central File Exchange. Retrieved January 19, 2021. +% ------------------------------------------------------------------------ +% +function [Cylinder EndPlate1 EndPlate2] = vis_cylinder(X1,X2,r,n,cyl_color,cyl_alpha,closed,lines) +% +% This function constructs a cylinder connecting two center points +% +% Usage : +% [Cylinder EndPlate1 EndPlate2] = Cylinder(X1+20,X2,r,n,'r',closed,lines) +% +% Cylinder-------Handle of the cylinder +% EndPlate1------Handle of the Starting End plate +% EndPlate2------Handle of the Ending End plate +% X1 and X2 are the 3x1 vectors of the two points +% r is the radius of the cylinder +% n is the no. of elements on the cylinder circumference (more--> refined) +% cyl_color is the color definition like 'r','b',[0.52 0.52 0.52] +% closed=1 for closed cylinder or 0 for hollow open cylinder +% lines=1 for displaying the line segments on the cylinder 0 for only +% surface +% +% Typical Inputs +% X1=[10 10 10]; +% X2=[35 20 40]; +% r=1; +% n=20; +% cyl_color='b'; +% closed=1; +% +% NOTE: There is a MATLAB function "cylinder" to revolve a curve about an +% axis. This "Cylinder" provides more customization like direction and etc +% Calculating the length of the cylinder +length_cyl=norm(X2-X1); +% Creating a circle in the YZ plane +t=linspace(0,2*pi,n)'; +x2=r*cos(t); +x3=r*sin(t); +% Creating the points in the X-Direction +x1=[0 length_cyl]; +% Creating (Extruding) the cylinder points in the X-Directions +xx1=repmat(x1,length(x2),1); +xx2=repmat(x2,1,2); +xx3=repmat(x3,1,2); +% Drawing two filled cirlces to close the cylinder +if closed==1 + hold on + EndPlate1=fill3(xx1(:,1),xx2(:,1),xx3(:,1),'r'); + EndPlate2=fill3(xx1(:,2),xx2(:,2),xx3(:,2),'r'); +end +% Plotting the cylinder along the X-Direction with required length starting +% from Origin +Cylinder=mesh(xx1,xx2,xx3); +% Defining Unit vector along the X-direction +unit_Vx=[1 0 0]; +% Calulating the angle between the x direction and the required direction +% of cylinder through dot product +angle_X1X2=acos( dot( unit_Vx,(X2-X1) )/( norm(unit_Vx)*norm(X2-X1)) )*180/pi; +% Finding the axis of rotation (single rotation) to roate the cylinder in +% X-direction to the required arbitrary direction through cross product +axis_rot=cross([1 0 0],(X2-X1) ); +% Rotating the plotted cylinder and the end plate circles to the required +% angles +if angle_X1X2~=0 % Rotation is not needed if required direction is along X + rotate(Cylinder,axis_rot,angle_X1X2,[0 0 0]) + if closed==1 + rotate(EndPlate1,axis_rot,angle_X1X2,[0 0 0]) + rotate(EndPlate2,axis_rot,angle_X1X2,[0 0 0]) + end +end +% Till now cylinder has only been aligned with the required direction, but +% position starts from the origin. so it will now be shifted to the right +% position +if closed==1 + set(EndPlate1,'XData',get(EndPlate1,'XData')+X1(1)) + set(EndPlate1,'YData',get(EndPlate1,'YData')+X1(2)) + set(EndPlate1,'ZData',get(EndPlate1,'ZData')+X1(3)) + + set(EndPlate2,'XData',get(EndPlate2,'XData')+X1(1)) + set(EndPlate2,'YData',get(EndPlate2,'YData')+X1(2)) + set(EndPlate2,'ZData',get(EndPlate2,'ZData')+X1(3)) +end +set(Cylinder,'XData',get(Cylinder,'XData')+X1(1)) +set(Cylinder,'YData',get(Cylinder,'YData')+X1(2)) +set(Cylinder,'ZData',get(Cylinder,'ZData')+X1(3)) +% Setting the color to the cylinder and the end plates +set(Cylinder,'FaceColor',cyl_color,'Edgecolor','k','FaceAlpha',cyl_alpha); +if closed==1 + set([EndPlate1 EndPlate2],'FaceColor',cyl_color,'Edgecolor','none','FaceAlpha',cyl_alpha); +else + EndPlate1=[]; + EndPlate2=[]; +end +% If lines are not needed making it disapear +if lines==0 + set(Cylinder,'EdgeAlpha',0) +end \ No newline at end of file diff --git a/src/visualization/vis_footholds.m b/src/visualization/vis_footholds.m index 2bfe19d..43cce1b 100644 --- a/src/visualization/vis_footholds.m +++ b/src/visualization/vis_footholds.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-06-01 %%%%%% Kentaro Uno -%%%%%% Last update: 2020-09-22 +%%%%%% Last update: 2021-04-05 +%%%%%% Kentaro Uno % % % Plot time history of footholds @@ -20,36 +21,38 @@ % plot_settings.footholds_fig_number: Figure number (integer) % inc : Surface inclination [deg] (scalar) % surface_param.graspable_points : Position of graspable points -% path_planning_param.footholds_history_limb +% gait_planning_param.footholds_history_limb % : Three-dimensional matrix to store footholds of one limb (double), which is updated in upd_swing_next_pos_crawl_fixed_stride() -% e.g.) path_planning_param.footholds_history_limb(1,3,2) means: x(1) position of the third foothold of limb 2 +% e.g.) gait_planning_param.footholds_history_limb(1,3,2) means: x(1) position of the third foothold of limb 2 -function vis_footholds(LP, plot_settings, inc, surface_param, path_planning_param) +function vis_footholds(LP, plot_settings, inc, surface_param, gait_planning_param) if strcmp(plot_settings.footholds,'on') figure(plot_settings.footholds_fig_number) % plot the graspabe points first - plot(surface_param.graspable_points(1,:),surface_param.graspable_points(2,:),'xb') + plot(surface_param.graspable_points(1,:),surface_param.graspable_points(2,:),'.','color',[0.5,0.5,0.5]) hold on surf_AA = rpy2dc([0;pi*inc/180;0])'; - % plot(path_planning_param.com_projection_history(1,:), path_planning_param.com_projection_history(2,:),'-k','LineWidth',2) + % plot(gait_planning_param.com_projection_history(1,:), gait_planning_param.com_projection_history(2,:),'-k','LineWidth',2) % color settings hue_for_limb_1 = 0.80; % magenta hue_for_limb_2 = 0.6; % blue hue_for_limb_3 = 1.0; % red - hue_for_limb_4 = 0.35; % green + hue_for_limb_4 = 0.3; % green % plot footholds history for i=1:LP.num_limb footholds = []; -% footholds = path_planning_param.footholds_history_limb(:,:,i); +% footholds = gait_planning_param.footholds_history_limb(:,:,i); - %%% ignore the invalid footholds (which is labeled as [0;0;0] in path_planning_param.footholds_history_limb) - for j=1:size(path_planning_param.footholds_history_limb,2) - if path_planning_param.footholds_history_limb(:,j,i) ~= [0;0;0] - footholds = horzcat( footholds, path_planning_param.footholds_history_limb(:,j,i) ); + %%% ignore the invalid footholds (which is labeled as [0;0;0] in gait_planning_param.footholds_history_limb) + for j=1:size(gait_planning_param.footholds_history_limb,2) + if gait_planning_param.footholds_history_limb(1,j,i) ~= 0 ... + || gait_planning_param.footholds_history_limb(2,j,i) ~= 0 ... + || gait_planning_param.footholds_history_limb(3,j,i) ~= 0 + footholds = horzcat( footholds, gait_planning_param.footholds_history_limb(:,j,i) ); end end @@ -81,16 +84,20 @@ function vis_footholds(LP, plot_settings, inc, surface_param, path_planning_para hold on end - % plot the CoM projection history (to be added) + % plot the CoM projection history + plot(gait_planning_param.com_projection_history(1,:),gait_planning_param.com_projection_history(2,:),... + 'k-','LineWidth',2.0); % figure settings - title('Footholds history'); - xlabel('\itx_g \rm[m]','FontName','calibri','FontSize',10); ylabel('\ity_g \rm[m]','FontName','calibri','FontSize',10); - set(gca,'fontsize',10); + title('Footholds history', 'FontSize', plot_settings.font_size, 'FontName', plot_settings.font_name); + xlabel('\itx_g \rm[m]', 'FontSize', plot_settings.font_size, 'FontName', plot_settings.font_name); + ylabel('\ity_g \rm[m]', 'FontSize', plot_settings.font_size, 'FontName', plot_settings.font_name); + set(gca,'fontsize',plot_settings.font_size,'LineWidth',plot_settings.width/2); + set(gcf,'color','w'); ax = gca; ax.XLim = [min(surface_param.graspable_points(1,:)) max(surface_param.graspable_points(1,:))]; ax.YLim = [min(surface_param.graspable_points(2,:)) max(surface_param.graspable_points(2,:))]; ax.DataAspectRatio = [1 1 1]; - grid on; + grid off; end % % save as fig file (to be added) % if sav == 1 diff --git a/src/visualization/vis_goal.m b/src/visualization/vis_goal.m index c1eaac7..ae04321 100644 --- a/src/visualization/vis_goal.m +++ b/src/visualization/vis_goal.m @@ -5,18 +5,18 @@ %%%%%% %%%%%% Created 2020-05-28 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-10-16 -%%%%%% Kentaro Uno +%%%%%% Last update: 2021-05-04 +%%%%%% Keigo Haji % % -% Display goal position +% Display goals' positions % % Function variables: % % OUTPUT % - % INPUT -% gait_param.goal : Position vector of goal +% gait_planning_param.goal : Position vector of goal % inc : Surface inclination [deg] (scalar) % ani_settings.goal_color : Color for points [RGB] (1x3 vector) % ani_settings.goal_marker : Type of marker (String) @@ -26,12 +26,12 @@ % ani_settings.goal_show : 'on','off' -function vis_goal(gait_param, inc, ani_settings) +function vis_goal(gait_planning_param, inc, ani_settings) if strcmp(ani_settings.goal_show,'on') % visualization position is slightly located vertically upper to the ground to show it well - goal = rpy2dc([0;pi*inc/180;0]) * [gait_param.goal(1); - gait_param.goal(2); - gait_param.goal(3) + ani_settings.goal_vis_height]; + goal = rpy2dc([0;pi*inc/180;0]) * [gait_planning_param.goal(1,:); + gait_planning_param.goal(2,:); + gait_planning_param.goal(3,:) + ani_settings.goal_vis_height]; plot3(goal(1,:),goal(2,:),goal(3,:),ani_settings.goal_marker,'Color',ani_settings.goal_color,'MarkerSize',ani_settings.goal_size); end end \ No newline at end of file diff --git a/src/visualization/vis_graph_base_pos.m b/src/visualization/vis_graph_base_pos.m deleted file mode 100644 index 100d215..0000000 --- a/src/visualization/vis_graph_base_pos.m +++ /dev/null @@ -1,36 +0,0 @@ -%%%%%% Visualization -%%%%%% vis_graph_base_pos -%%%%%% -%%%%%% Visualize time history of base position -%%%%%% -%%%%%% Created 2020-07-03 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-03 -% -% -% Plot time history of base position -% -% Function variables: -% -% OUTPUT -% - -% INPUT -% data : Saved data table -% plot_settings : plot settings (struct) - -function vis_graph_base_pos(data, plot_settings) - -if strcmp(plot_settings.base_pos,'on') - figure(plot_settings.base_pos_fig_number) - - % plot TSM - plot(data.time,data.R0,'-','LineWidth',plot_settings.width); - % figure settings - title('Base position','FontName',plot_settings.font_name,'FontSize',plot_settings.font_size); - xlabel('\itt \rm[s]','FontName',plot_settings.font_name,'FontSize',plot_settings.font_size); - ylabel('\it{x}_{\rmb} \rm[m]','FontName',plot_settings.font_name,'FontSize',plot_settings.font_size); - set(gca,'fontsize',plot_settings.font_size,'LineWidth',plot_settings.width/2); - grid on; -end - -end \ No newline at end of file diff --git a/src/visualization/vis_graph_inclination_margin.m b/src/visualization/vis_graph_gia_inclination_margin.m similarity index 62% rename from src/visualization/vis_graph_inclination_margin.m rename to src/visualization/vis_graph_gia_inclination_margin.m index 20e0eeb..471dd09 100644 --- a/src/visualization/vis_graph_inclination_margin.m +++ b/src/visualization/vis_graph_gia_inclination_margin.m @@ -1,14 +1,15 @@ %%%%%% Visualization -%%%%%% vis_graph_inclination_margin +%%%%%% vis_graph_gia_inclination_margin %%%%%% -%%%%%% Visualize time history of inclination margin +%%%%%% Visualize time history of GIA inclination margin %%%%%% %%%%%% Created 2020-07-03 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-27 +%%%%%% Last update: 2021-02-12 +%%%%%% Kentaro Uno % % -% Plot time history of inclination margin +% Plot time history of GIA inclination margin % % Function variables: % @@ -18,13 +19,13 @@ % data : Saved data table % plot_settings : plot settings (struct) -function vis_graph_inclination_margin(data, plot_settings) +function vis_graph_gia_inclination_margin(data, plot_settings) -if strcmp(plot_settings.inclination_margin,'on') - figure(plot_settings.inclination_margin_fig_number) +if strcmp(plot_settings.gia_inclination_margin,'on') + figure(plot_settings.gia_inclination_margin_fig_number) % plot TSM - plot(data.time,data.inclination_margin,'-','LineWidth',plot_settings.width); + plot(data.time,data.gia_inclination_margin,'-','LineWidth',plot_settings.width); % figure settings title('GIA Inclination Margin','FontName',plot_settings.font_name,'FontSize',plot_settings.font_size); xlabel('\itt \rm[s]','FontName',plot_settings.font_name,'FontSize',plot_settings.font_size); diff --git a/src/visualization/vis_graph_acc_margin.m b/src/visualization/vis_graph_gia_margin.m similarity index 70% rename from src/visualization/vis_graph_acc_margin.m rename to src/visualization/vis_graph_gia_margin.m index 701daaa..7dd4b34 100644 --- a/src/visualization/vis_graph_acc_margin.m +++ b/src/visualization/vis_graph_gia_margin.m @@ -1,11 +1,12 @@ %%%%%% Visualization -%%%%%% vis_graph_acc_margin +%%%%%% vis_graph_gia_margin %%%%%% -%%%%%% Visualize time history of acceleration margin +%%%%%% Visualize time history of GIA acceleration margin %%%%%% %%%%%% Created 2020-07-03 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-27 +%%%%%% Last update: 2022-02-12 +%%%%%% Kentaro Uno % % % Plot time history of acceleration margin @@ -18,13 +19,13 @@ % data : Saved data table % plot_settings : plot settings (struct) -function vis_graph_acc_margin(data, plot_settings) +function vis_graph_gia_margin(data, plot_settings) -if strcmp(plot_settings.acc_margin,'on') - figure(plot_settings.acc_margin_fig_number) +if strcmp(plot_settings.gia_margin,'on') + figure(plot_settings.gia_margin_fig_number) % plot TSM - plot(data.time,data.acc_margin,'-','LineWidth',plot_settings.width); + plot(data.time,data.gia_margin,'-','LineWidth',plot_settings.width); % figure settings title('GIA Acceleration Margin','FontName',plot_settings.font_name,'FontSize',plot_settings.font_size); xlabel('\itt \rm[s]','FontName',plot_settings.font_name,'FontSize',plot_settings.font_size); diff --git a/src/visualization/vis_graspable_points.m b/src/visualization/vis_graspable_points.m index 08f35dc..c407b7a 100644 --- a/src/visualization/vis_graspable_points.m +++ b/src/visualization/vis_graspable_points.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-05-14 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-05-14 +%%%%%% Last update: 2021-04-08 +%%%%%% Keigo Haji % % % Display graspable points @@ -18,14 +19,27 @@ % surface_param : Position of Graspable points % inc : Surface inclination [deg] (scalar) % ani_settings.graspable_color : Color for points [RGB] (1x3 vector) +% ani_settings.graspable_alpha : Alpha for marker (0 -- 1) % ani_settings.graspable_marker: Type of marker (String) % ani_settings.graspable_size : Size of points (scalar) function vis_graspable_points(surface_param, inc, ani_settings) if strcmp(ani_settings.graspable_points_show,'on') + % Rotation matrix rot = rpy2dc([0;pi*inc/180;0])'; + + % Transform coordinates from the ground coordinate system to the + % inertial coordinate system. graspable_points = rot' * surface_param.graspable_points; - plot3(graspable_points(1,:),graspable_points(2,:),graspable_points(3,:),ani_settings.graspable_points_marker,'Color',ani_settings.graspable_points_color,'MarkerSize',ani_settings.graspable_points_size); + + + % Display + scatter3(graspable_points(1,:),graspable_points(2,:),graspable_points(3,:), ... + ani_settings.graspable_points_size, ... + ani_settings.graspable_points_marker, ... + 'MarkerEdgeColor','none', ... + 'MarkerFaceColor',ani_settings.graspable_points_color, ... + 'MarkerFaceAlpha',ani_settings.graspable_points_alpha); end end \ No newline at end of file diff --git a/src/visualization/vis_graspable_points_in_reachable_area.m b/src/visualization/vis_graspable_points_in_reachable_area.m index 9abfcfe..c0c3447 100644 --- a/src/visualization/vis_graspable_points_in_reachable_area.m +++ b/src/visualization/vis_graspable_points_in_reachable_area.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-05-14 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-05-14 +%%%%%% Last updated: 2020-11-18 +%%%%%% Kentaro Uno % % % Display graspable points in reachable area @@ -15,21 +16,21 @@ % OUTPUT % - % INPUT -% path_planning_param : Parameters for Path Plannning (class) +% gait_planning_param : Parameters for Gait 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) % ani_settings.graspable_points_in_reachable_area_size : Size of points (scalar) -function vis_graspable_points_in_reachable_area(path_planning_param, inc, ani_settings) +function vis_graspable_points_in_reachable_area(gait_planning_param, inc, ani_settings) if strcmp(ani_settings.graspable_points_in_reachable_area_show,'on') rot = rpy2dc([0;pi*inc/180;0])'; % get an index of the swing leg - i = path_planning_param.swing_number; - if ~isempty(path_planning_param.graspable_points_in_reachable_area(:,:,i)) - graspable_points_in_reachable_area = rot'*path_planning_param.graspable_points_in_reachable_area(:,:,i); + i = gait_planning_param.swing_number; + if ~isempty(gait_planning_param.graspable_points_in_reachable_area(:,:,i)) + graspable_points_in_reachable_area = rot'*gait_planning_param.graspable_points_in_reachable_area(:,:,i); for i = 1 : 1 : size(graspable_points_in_reachable_area,2) % if the contents is zero, it should not be drawn diff --git a/src/visualization/vis_next_desired_position.m b/src/visualization/vis_next_desired_position.m index 515a553..af1f460 100644 --- a/src/visualization/vis_next_desired_position.m +++ b/src/visualization/vis_next_desired_position.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-05-28 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-10-16 +%%%%%% Last updated: 2020-11-18 %%%%%% Kentaro Uno % % @@ -16,7 +16,7 @@ % OUTPUT % - % INPUT -% path_planning_param.POS_next : Next desired position of end-effector +% gait_planning_param.POS_next : Next desired position of end-effector % inc : Surface inclination [deg] (scalar) % ani_settings.next_desired_position_color : Color for points [RGB] (1x3 vector) % ani_settings.next_desired_position_marker: Type of marker (String) @@ -25,11 +25,11 @@ -function vis_next_desired_position(path_planning_param, inc, ani_settings) +function vis_next_desired_position(gait_planning_param, inc, ani_settings) if strcmp(ani_settings.next_desired_position_show,'on') - dr = 0.015; + dr = 0.030; dz = 0; - des_pos_now = path_planning_param.POS_next(:,path_planning_param.swing_number); + des_pos_now = gait_planning_param.POS_next(:,gait_planning_param.swing_number); des_pos_square(:,1) = des_pos_now + [dr; dr; dz]; des_pos_square(:,2) = des_pos_now + [dr;-dr; dz]; des_pos_square(:,3) = des_pos_now + [-dr;-dr;dz]; diff --git a/src/visualization/vis_one_vector.m b/src/visualization/vis_one_vector.m index 4650e4e..32526ba 100644 --- a/src/visualization/vis_one_vector.m +++ b/src/visualization/vis_one_vector.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2019-10-31 %%%%%% Warley Ribeiro -%%%%%% Last update: 2019-07-02 +%%%%%% Last update: 2021-02-02 +%%%%%% Kentaro Uno % % % Draw a three-dimensional vector from a specific origin point @@ -15,14 +16,16 @@ % OUTPUT % - % INPUT -% origin : Origin point for the vector (3x1 vector) -% vector : Vector to be plotted (3x1 vector) -% inc : Inclination [deg] (scalar) -% color : Color for vector [RGB] (1x3 vector) -% width : Width for vector line (scalar) +% origin : Origin point for the vector (3x1 vector) +% vector : Vector to be plotted (3x1 vector) +% inc : Inclination [deg] (scalar) +% color : Color for vector [RGB] (1x3 vector) +% width : Width for vector line (scalar) +% arrow_head_size : Soze of the arrow head marker (scalar) -function vis_one_vector(origin,vector,inc,color,width) + +function vis_one_vector(origin,vector,inc,color,width,arrow_head_size) % Rotation matrix rot = rpy2dc([0;pi*inc/180;0])'; @@ -35,6 +38,8 @@ function vis_one_vector(origin,vector,inc,color,width) plot3(VEC(1,:),VEC(2,:),VEC(3,:),'-','Color',color,'LineWidth',width); hold on % Arrow head -plot3(VEC(1,2),VEC(2,2),VEC(3,2),'v','Color',color,'MarkerFaceColor',color,'LineWidth',width,'MarkerSize',3); +if arrow_head_size ~= 0 + plot3(VEC(1,2),VEC(2,2),VEC(3,2),'v','Color',color,'MarkerFaceColor',color,'LineWidth',width,'MarkerSize',arrow_head_size); +end end \ No newline at end of file diff --git a/src/visualization/vis_plot_graph.m b/src/visualization/vis_plot_graph.m index 55024e5..6367867 100644 --- a/src/visualization/vis_plot_graph.m +++ b/src/visualization/vis_plot_graph.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-07-27 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-09-08 +%%%%%% Last updated: 2021-07-09 +%%%%%% Keigo Haji % % % Plot graphs @@ -19,10 +20,10 @@ % plot_settings : plot settings (struct) % LP : Link Parameters (SpaceDyn class) % surface_param : Parameters for surface (struct) -% path_planning_param : Parameters for path planning (struct) +% gait_planning_param : Parameters for Gait Planning (struct) -function vis_plot_graph(data, plot_settings, LP, inc, surface_param, path_planning_param) +function vis_plot_graph(data, plot_settings, LP, inc, surface_param, gait_planning_param) % Plot time history of base position if strcmp(plot_settings.base_pos,'on') @@ -87,7 +88,6 @@ function vis_plot_graph(data, plot_settings, LP, inc, surface_param, path_planni vis_graph_time_history(data, plot_settings, tau, fig_number, 'Joints torque', '\it{tau} \rm[Nm]') end - % Plot time history of legs position if strcmp(plot_settings.leg_pos,'on') for i = 1:4 @@ -119,15 +119,45 @@ function vis_plot_graph(data, plot_settings, LP, inc, surface_param, path_planni end % Plot time history of footholds -vis_footholds(LP, plot_settings, inc, surface_param, path_planning_param); +vis_footholds(LP, plot_settings, inc, surface_param, gait_planning_param); % Plot TSM (requires saving CSV and equilibrium method as 'tsm') vis_graph_tsm(data, plot_settings); % Plot GIA acceleration margin (requires saving CSV and equilibrium method as 'gia') -vis_graph_acc_margin(data, plot_settings); +vis_graph_gia_margin(data, plot_settings); % Plot GIA inclination margin (requires saving CSV and equilibrium method as 'gia') -vis_graph_inclination_margin(data, plot_settings); +vis_graph_gia_inclination_margin(data, plot_settings); + +% Plot time history of manipulability +if strcmp(plot_settings.manipulability,'on') +vis_graph_time_history(data, plot_settings, [data.manipulability1 data.manipulability2 data.manipulability3 data.manipulability4]', plot_settings.manipulability_fig_number, 'Manipulability', ... + '\it{w\rm{_{i}}}'); +end + +% Plot time history of mean manipulability of all limbs +if strcmp(plot_settings.mean_manipulability,'on') + fig_number = plot_settings.mean_manipulability_fig_number; + vis_graph_time_history(data, plot_settings, data.mean_manipulability, fig_number, 'Mean Manipulability Measure', '\rm{mean} \it{w\rm{_{i}}}'); +end +% Plot time history of minimum manipulability of all limbs +if strcmp(plot_settings.mean_manipulability,'on') + fig_number = plot_settings.min_manipulability_fig_number; + vis_graph_time_history(data, plot_settings, data.min_manipulability, fig_number, 'Min. Manipulability Measure', '\rm{min.} \it{w\rm{_{i}}}'); +end + +% Plot time history of the maximum of absolute torque of all joint +if strcmp(plot_settings.joint_max_torque,'on') + tau_max = data.tau_max; + fig_number = plot_settings.joint_max_torque_fig_number; + vis_graph_time_history(data, plot_settings, tau_max, fig_number, 'Max. joint torque', '\it{tau\rm{_{max}}} \rm[Nm]') +end +% Plot time history of the RMS value of torque of all joint +if strcmp(plot_settings.joint_rms_torque,'on') + tau_rms = data.tau_rms; + fig_number = plot_settings.joint_rms_torque_fig_number; + vis_graph_time_history(data, plot_settings, tau_rms, fig_number, 'RMS joint torque', '\it{tau\rm{_{rms}}} \rm[Nm]') +end end \ No newline at end of file diff --git a/src/visualization/vis_reachable_area.m b/src/visualization/vis_reachable_area.m index 1f35b64..0b3cbea 100644 --- a/src/visualization/vis_reachable_area.m +++ b/src/visualization/vis_reachable_area.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-05-20 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-07-11 by Kentaro Uno +%%%%%% Last updated: 2020-05-17 +%%%%%% Kentaro Uno % % % Display reachable points @@ -17,7 +18,7 @@ % INPUT % SV : State Variables % LP : Link Parameters -% path_planning_param : Parameters for Path Planning (class) +% gait_planning_param : Parameters for Gait Planning (class) % inc : Surface inclination [deg] (scalar) % ani_settings.reachable_area_line_color : Color for line [RGB] (1x3 vector) % ani_settings.reachable_area_line_width : Line width of marker (scalar) @@ -27,12 +28,12 @@ -function vis_reachable_area(SV, LP, path_planning_param, inc, ani_settings, surface_param) +function vis_reachable_area(SV, LP, gait_planning_param, inc, ani_settings, surface_param) if strcmp(ani_settings.reachable_area_show, 'on') % get an index of the swing leg - i = path_planning_param.swing_number; + i = gait_planning_param.swing_number; - alpha = LP.Qi(3, (3*i-2)); + alpha = LP.Qi(3, (3*i-2)) + SV.Q0(3); joint = SV.R0 + SV.A0*LP.c0(:,3*i-2); r_min = LP.reachable_area.min-LP.c0(1,1); r_max = LP.reachable_area.max-LP.c0(1,1); diff --git a/src/visualization/vis_robot.m b/src/visualization/vis_robot.m index 6d56a76..4dc2a86 100644 --- a/src/visualization/vis_robot.m +++ b/src/visualization/vis_robot.m @@ -3,9 +3,10 @@ %%%%%% %%%%%% Draw robot %%%%%% -%%%%%% Created 2020-04-09 -%%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 +%%%%%% Created: 2020-04-09 +%%%%%% by Warley Ribeiro +%%%%%% Last update: 2020-01-19 +%%%%%% by Kentaro Uno % % % Draw robot from current parameters and from links' simplified shapes @@ -21,47 +22,126 @@ % POS_e : End-effector positions (3xnum_limb matrix) % shape_robot : Vertices and faces indicators for base and legs (structure) % inc : Surface inclination [deg] (scalar) -% ani_settings.robot_color : Color for robot [RGB] (1x3 vector) +% ani_settings.robot_base_color : Color for robot base [RGB] (1x3 vector) +% ani_settings.robot_base_alpha : Transparency for robot base [RGB] (1x3 vector) +% ani_settings.robot_limb_color : Color for robot limb [RGB] (1x3 vector) +% ani_settings.robot_limb_alpha : Transparency for robot limb [RGB] (1x3 vector) - -function vis_robot(LP, SV, F_grip, POS_e, shape_robot, inc, ani_settings) +function vis_robot(LP, SV, POS_e, shape_robot, inc, ani_settings) % Rotation matrix rot = rpy2dc([0;pi*inc/180;0])'; +%% Draw base % Change in the vertices of the base of the robot based on actual position and orientation -shape_robot.base_V = shape_robot.base_V*SV.A0' + ones(size(shape_robot.base_V,1),1)*SV.R0'; -shape_robot.base_V = shape_robot.base_V*rot; -% Draw base -patch('Vertices',shape_robot.base_V,'Faces',shape_robot.base_F,'FaceColor',ani_settings.robot_color,'Edgecolor','none',... - 'FaceAlpha',ani_settings.robot_alpha); +shape_robot.base_upper_V = shape_robot.base_upper_V*SV.A0' + ones(size(shape_robot.base_upper_V,1),1)*SV.R0'; +shape_robot.base_upper_V = shape_robot.base_upper_V*rot; +shape_robot.base_lower_V = shape_robot.base_lower_V*SV.A0' + ones(size(shape_robot.base_lower_V,1),1)*SV.R0'; +shape_robot.base_lower_V = shape_robot.base_lower_V*rot; -% Change in the vertices of each link of the robot based on actual position and orientation -k=1; -for i = 1:LP.num_limb - for j = 1:3 - shape_robot.link_V(:,:,k) = shape_robot.V_leg(:,:,j)*SV.AA(:,3*k-2:3*k)' + ... - ones(size(shape_robot.V_leg(:,:,1),1),1)*SV.RR(:,k)'; - shape_robot.link_F(:,:,k) = shape_robot.F_leg(:,:,j); - k=k+1; - end +patch('Vertices',shape_robot.base_upper_V,'Faces',shape_robot.base_upper_F,'FaceColor',ani_settings.robot_base_upper_color,'Edgecolor','none',... + 'FaceAlpha',ani_settings.robot_base_alpha); +patch('Vertices',shape_robot.base_lower_V,'Faces',shape_robot.base_lower_F,'FaceColor',ani_settings.robot_base_lower_color,'Edgecolor','none',... + 'FaceAlpha',ani_settings.robot_base_alpha); + +%% Draw the links +% prepare the necessary vector for the f_kin_j +joints = []; +for j = 1:LP.num_q + joints = [joints j]; end -% Draw each link -for i=1:LP.num_q - shape_robot.link_V(:,:,i) = shape_robot.link_V(:,:,i)*rot; - patch('Vertices',shape_robot.link_V(:,:,i),'Faces',shape_robot.link_F(:,:,i),'FaceColor',ani_settings.robot_color,... - 'Edgecolor','none','FaceAlpha',ani_settings.robot_alpha); +[ POS_j, ORI_j ] = f_kin_j( LP, SV, joints ); + + +% Draw the joint frames +joint_frame_x_axes = zeros( 3, length(joints) ); +joint_frame_y_axes = zeros( 3, length(joints) ); +joint_frame_z_axes = zeros( 3, length(joints) ); +ex=[1;0;0]; ey=[0;1;0]; ez=[0;0;1]; + +if strcmp(ani_settings.frames_show,'on') + + % parameters for drawing the frames + width = ani_settings.frames_line_width; + magnitude = ani_settings.frames_size; + arrow_head_size = 0.0; % not to visualize arrow head for simplicity + % draw the frame at the CoM + CoM = upd_CoM(LP, SV); + vis_one_vector(CoM, magnitude*SV.A0*ex, inc, 'r', width, arrow_head_size); + vis_one_vector(CoM, magnitude*SV.A0*ey, inc, 'g', width, arrow_head_size); + vis_one_vector(CoM, magnitude*SV.A0*ez, inc, 'b', width, arrow_head_size); + % draw the frames at each joint + for joint_num = 1:LP.num_q + joint_frame_x_axes(:,joint_num) = ORI_j(:,3*joint_num-2:3*joint_num)*ex; + joint_frame_y_axes(:,joint_num) = ORI_j(:,3*joint_num-2:3*joint_num)*ey; + joint_frame_z_axes(:,joint_num) = ORI_j(:,3*joint_num-2:3*joint_num)*ez; + vis_one_vector(POS_j(:,joint_num), magnitude*joint_frame_x_axes(:,joint_num), inc, 'r', width, arrow_head_size); + vis_one_vector(POS_j(:,joint_num), magnitude*joint_frame_y_axes(:,joint_num), inc, 'g', width, arrow_head_size); + vis_one_vector(POS_j(:,joint_num), magnitude*joint_frame_z_axes(:,joint_num), inc, 'b', width, arrow_head_size); + end +end + +% Draw the links +% position of the end-effector is adjusted for the inclination of the ground +POS_e = rot' * POS_e; + +% necessary parameters for vis_cylinder() +r = ani_settings.link_radius; n = 7; closed=1; lines=0; + +k = 1; +for i = 1:LP.num_q + % position of each joint is adjusted for the inclination of the ground + POS_j(:,i) = rot' * POS_j(:,i); + % if the joint i is not connected the base (link index:0), connect + % the joint i and its parent joint, which number is obtained by LP.BB(i) + if LP.BB(i) ~= 0 + [Cylinder EndPlate1 EndPlate2] = vis_cylinder(POS_j(:,i),POS_j(:,LP.BB(i)),... + r,n,ani_settings.robot_limb_color,ani_settings.robot_limb_alpha,closed,lines); + % moreover, if joint i is not the first joint, joint i-1 should be the + % end-joint of the branch. -> connect it with the EE + elseif i ~= 1 + [Cylinder EndPlate1 EndPlate2] = vis_cylinder(POS_j(:,i-1),POS_e(:,k),... + r,n,ani_settings.robot_limb_color,ani_settings.robot_limb_alpha,closed,lines); + k = k + 1; + end + % Finaslly, if joint i is the last joint, joint i should be also the + % end-joint of the branch. -> connect it with the EE + if i == LP.num_q + [Cylinder EndPlate1 EndPlate2] = vis_cylinder(POS_j(:,i),POS_e(:,k),... + r,n,ani_settings.robot_limb_color,ani_settings.robot_limb_alpha,closed,lines); + end end -% Draw grippers -if F_grip > 0 - POS_e = rot'* POS_e; +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +%%% Old code for link visualization part +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% % Change in the vertices of each link of the robot based on actual position and orientation +% k=1; +% for i = 1:LP.num_limb +% for j = 1:3 +% shape_robot.link_V(:,:,k) = shape_robot.V_leg(:,:,j)*SV.AA(:,3*k-2:3*k)' + ... +% ones(size(shape_robot.V_leg(:,:,1),1),1)*SV.RR(:,k)'; +% shape_robot.link_F(:,:,k) = shape_robot.F_leg(:,:,j); +% k=k+1; +% end +% end +% % Draw each link +% for i=1:LP.num_q +% shape_robot.link_V(:,:,i) = shape_robot.link_V(:,:,i)*rot; +% patch('Vertices',shape_robot.link_V(:,:,i),'Faces',shape_robot.link_F(:,:,i),'FaceColor',ani_settings.robot_color,... +% 'Edgecolor','none','FaceAlpha',ani_settings.robot_alpha); +% end + +%% Draw grippers +if LP.F_grip > 0 +% POS_e = rot'* POS_e; for i = 1:LP.num_limb shape_robot.gripper_V(:,:,i) = shape_robot.grip_V*rot + ones(size(shape_robot.grip_V,1),1)*POS_e(1:3,i)'; - patch('Vertices',shape_robot.gripper_V(:,:,i),'Faces',shape_robot.grip_F,'FaceColor',ani_settings.robot_color,... - 'Edgecolor','none','FaceAlpha',ani_settings.robot_alpha); + patch('Vertices',shape_robot.gripper_V(:,:,i),'Faces',shape_robot.grip_F,'FaceColor',ani_settings.robot_limb_color,... + 'Edgecolor','none','FaceAlpha',ani_settings.robot_limb_alpha); shape_robot.gripper_V(:,:,i) = shape_robot.grip_V*rpy2dc([0;0;pi/2])*rot + ones(size(shape_robot.grip_V,1),1)*POS_e(1:3,i)'; - patch('Vertices',shape_robot.gripper_V(:,:,i),'Faces',shape_robot.grip_F,'FaceColor',ani_settings.robot_color,... - 'Edgecolor','none','FaceAlpha',ani_settings.robot_alpha); + patch('Vertices',shape_robot.gripper_V(:,:,i),'Faces',shape_robot.grip_F,'FaceColor',ani_settings.robot_limb_color,... + 'Edgecolor','none','FaceAlpha',ani_settings.robot_limb_alpha); end end \ No newline at end of file diff --git a/src/visualization/vis_sensed_graspable_points.m b/src/visualization/vis_sensed_graspable_points.m new file mode 100644 index 0000000..f1a90e9 --- /dev/null +++ b/src/visualization/vis_sensed_graspable_points.m @@ -0,0 +1,47 @@ +%%%%%% Visualize +%%%%%% vis_sensed_graspable_points +%%%%%% +%%%%%% Display sensed graspable points +%%%%%% +%%%%%% Created 2021-02-15 +%%%%%% Keigo Haji +%%%%%% Last update: 2021-04-05 +%%%%%% Kentaro Uno +% +% +% Display graspable points sensed by the RealSense Camera +% +% Function variables: +% +% OUTPUT +% - +% INPUT +% surface_param.sensed_graspable_points : Position of Sensed Graspable points +% inc : Surface inclination [deg] (scalar) +% ani_settings.sensed_graspable_color : Color for points [RGB] (1x3 vector) +% ani_settings.sensed_graspable_alpha : Alpha for marker (0 -- 1) +% ani_settings.sensed_graspable_marker : Type of marker (String) +% ani_settings.sensed_graspable_size : Size of points (scalar) + + +function vis_sensed_graspable_points(surface_param, inc, ani_settings) + +if strcmp(ani_settings.sensed_graspable_points_show,'on') + % Rotation matrix + rot = rpy2dc([0;pi*inc/180;0])'; + + % Transform coordinates from the ground coordinate system to the + % inertial coordinate system. + sensed_graspable_points = rot' * surface_param.sensed_graspable_points; + + % Display + scatter3(sensed_graspable_points(1,:),sensed_graspable_points(2,:),sensed_graspable_points(3,:), ... + ani_settings.sensed_graspable_points_size, ... + ani_settings.sensed_graspable_points_marker, ... + 'MarkerEdgeColor', 'none', ... + 'MarkerFaceColor',ani_settings.sensed_graspable_points_color, ... + 'MarkerFaceAlpha',ani_settings.sensed_graspable_points_alpha); +end + +end + diff --git a/src/visualization/vis_sensing_camera_fov.m b/src/visualization/vis_sensing_camera_fov.m new file mode 100644 index 0000000..d0648ec --- /dev/null +++ b/src/visualization/vis_sensing_camera_fov.m @@ -0,0 +1,141 @@ +%%%%%% Visualize +%%%%%% vis_sensing_camera_fov +%%%%%% +%%%%%% Visualize the fov of a RealSense camera +%%%%%% +%%%%%% Created: 2021-02-10 +%%%%%% by Keigo Haji +%%%%%% Last update: 2021-03-03 +%%%%%% by Keigo Haji +% +% +% Visualize the fov of RealSense camera +% +% Function variables: +% +% OUTPUT +% - +% +% INPUT +% SV : State Variables (SpaceDyn class) +% inc : Surface inclination [deg] (scalar) +% sensing_camera_param : Setting the fov of the camera +% mounting_angle : Mounting angle of the camera to the base +% mounting_position : Mounting position of the camera on the base +% fov_horizontal : Horizontal angle of view of the camera +% fov_vertical : Vertical angle of view of the camera +% fov_max_distance : Maximum distance that depth can be detected +% fov_min_distance : Minimum distance that depth can be detected +% ani_settings : Setting the line width and color for drawing the fov +% + +function vis_sensing_camera_fov(SV, inc, sensing_camera_param, ani_settings, surface_param) + +if strcmp(ani_settings.sensing_fov_show, 'on') + % Rotation matrix + rot = rpy2dc([0;pi*inc/180;0])'; + if strcmp(sensing_camera_param.sensing_type, 'RealSense_d435i') + + % Update camera position + camera.pos = SV.R0 + SV.A0 * sensing_camera_param.mounting_position; + + + % Set the camera range for the far side + far_y_minus = sensing_camera_param.fov_max_distance * tan(-sensing_camera_param.fov_horizontal * 0.5); % < 0 + far_y_plus = sensing_camera_param.fov_max_distance * tan( sensing_camera_param.fov_horizontal * 0.5); % > 0 + far_z_minus = sensing_camera_param.fov_max_distance * tan(-sensing_camera_param.fov_vertical * 0.5); % < 0 + far_z_plus = sensing_camera_param.fov_max_distance * tan( sensing_camera_param.fov_vertical * 0.5); % > 0 + % Set the camera range for the near side + near_y_minus = sensing_camera_param.fov_min_distance * tan(-sensing_camera_param.fov_horizontal * 0.5); % < 0 + near_y_plus = sensing_camera_param.fov_min_distance * tan( sensing_camera_param.fov_horizontal * 0.5); % > 0 + near_z_minus = sensing_camera_param.fov_min_distance * tan(-sensing_camera_param.fov_vertical * 0.5); % < 0 + near_z_plus = sensing_camera_param.fov_min_distance * tan( sensing_camera_param.fov_vertical * 0.5); % > 0 + + + % Set the coordinates of the end points that form the fov with respect + % to the camera coordinate system. The four points on the near side and + % the four points on the far side are set separately. + % Each row represents an xyz coordinate, and each column represents a + % point. + camera.max_scan_range =... + [sensing_camera_param.fov_max_distance sensing_camera_param.fov_max_distance sensing_camera_param.fov_max_distance sensing_camera_param.fov_max_distance ; + far_y_minus far_y_plus far_y_plus far_y_minus ; + far_z_plus far_z_plus far_z_minus far_z_minus ]; + camera.min_scan_range =... + [sensing_camera_param.fov_min_distance sensing_camera_param.fov_min_distance sensing_camera_param.fov_min_distance sensing_camera_param.fov_min_distance ; + near_y_minus near_y_plus near_y_plus near_y_minus ; + near_z_plus near_z_plus near_z_minus near_z_minus ]; + + + % Transform coordinates from a camera-based coordinate system to a + % ground-based coordinate system. + camera.max_scan_range = camera.pos ... + + SV.A0 * rpy2dc(sensing_camera_param.mounting_angle) * camera.max_scan_range; + camera.min_scan_range = camera.pos ... + + SV.A0 * rpy2dc(sensing_camera_param.mounting_angle) * camera.min_scan_range; + + % Transform coordinates from the ground coordinate system to the + % inertial coordinate system. + camera_pos = rot' * camera.pos; + max_scan_range = rot' * camera.max_scan_range; + min_scan_range = rot' * camera.min_scan_range; + + + %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Visualize %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Draw the camera mounting position + plot3(camera_pos(1),camera_pos(2),camera_pos(3), ani_settings.sensing_camera_MarkerColor,'MarkerSize',ani_settings.sensing_camera_MarkerSize); + + % Draw the end points forming fov + plot3(max_scan_range(1,:),max_scan_range(2,:),max_scan_range(3,:), ani_settings.sensing_camera_fov_LineColor,'LineWidth', ani_settings.sensing_camera_fov_LineWidth); + plot3(min_scan_range(1,:),min_scan_range(2,:),min_scan_range(3,:), ani_settings.sensing_camera_fov_LineColor,'LineWidth', ani_settings.sensing_camera_fov_LineWidth); + + % Draw two rectangles connecting the end points + extended_matrix_of_max_scan_range_for_plotting = [max_scan_range max_scan_range(:,1)]; + plot3(extended_matrix_of_max_scan_range_for_plotting(1,:), extended_matrix_of_max_scan_range_for_plotting(2,:), extended_matrix_of_max_scan_range_for_plotting(3,:),ani_settings.sensing_camera_fov_LineColor,'LineWidth', ani_settings.sensing_camera_fov_LineWidth) + extended_matrix_of_min_scan_range_for_plotting = [min_scan_range min_scan_range(:,1)]; + plot3(extended_matrix_of_min_scan_range_for_plotting(1,:), extended_matrix_of_min_scan_range_for_plotting(2,:), extended_matrix_of_min_scan_range_for_plotting(3,:),ani_settings.sensing_camera_fov_LineColor,'LineWidth', ani_settings.sensing_camera_fov_LineWidth) + + % Draw four trapezoids connecting the near end-points and far + % end-points + for i = 1:4 + plot3([min_scan_range(1,i) max_scan_range(1,i)],[min_scan_range(2,i) max_scan_range(2,i)],[min_scan_range(3,i) max_scan_range(3,i)], ani_settings.sensing_camera_fov_LineColor,'LineWidth', ani_settings.sensing_camera_fov_LineWidth) + end + + if strcmp(ani_settings.sensing_fov_face_filling, 'on') + % Colors fov + patch([min_scan_range(1,1) max_scan_range(1,1) max_scan_range(1,2),min_scan_range(1,2)],[min_scan_range(2,1) max_scan_range(2,1) max_scan_range(2,2),min_scan_range(2,2)],[min_scan_range(3,1) max_scan_range(3,1) max_scan_range(3,2),min_scan_range(3,2)],ani_settings.sensing_fov_face_color,'FaceAlpha',ani_settings.sensing_fov_face_alpha, 'EdgeColor', ani_settings.sensing_camera_fov_LineColor); + patch([min_scan_range(1,2) max_scan_range(1,2) max_scan_range(1,3),min_scan_range(1,3)],[min_scan_range(2,2) max_scan_range(2,2) max_scan_range(2,3),min_scan_range(2,3)],[min_scan_range(3,2) max_scan_range(3,2) max_scan_range(3,3),min_scan_range(3,3)],ani_settings.sensing_fov_face_color,'FaceAlpha',ani_settings.sensing_fov_face_alpha, 'EdgeColor', ani_settings.sensing_camera_fov_LineColor); + patch([min_scan_range(1,4) max_scan_range(1,4) max_scan_range(1,3),min_scan_range(1,3)],[min_scan_range(2,4) max_scan_range(2,4) max_scan_range(2,3),min_scan_range(2,3)],[min_scan_range(3,4) max_scan_range(3,4) max_scan_range(3,3),min_scan_range(3,3)],ani_settings.sensing_fov_face_color,'FaceAlpha',ani_settings.sensing_fov_face_alpha, 'EdgeColor', ani_settings.sensing_camera_fov_LineColor); + patch([min_scan_range(1,1) max_scan_range(1,1) max_scan_range(1,4),min_scan_range(1,4)],[min_scan_range(2,1) max_scan_range(2,1) max_scan_range(2,4),min_scan_range(2,4)],[min_scan_range(3,1) max_scan_range(3,1) max_scan_range(3,4),min_scan_range(3,4)],ani_settings.sensing_fov_face_color,'FaceAlpha',ani_settings.sensing_fov_face_alpha, 'EdgeColor', ani_settings.sensing_camera_fov_LineColor); + patch([min_scan_range(1,1) min_scan_range(1,2) min_scan_range(1,3),min_scan_range(1,4)],[min_scan_range(2,1) min_scan_range(2,2) min_scan_range(2,3),min_scan_range(2,4)],[min_scan_range(3,1) min_scan_range(3,2) min_scan_range(3,3),min_scan_range(3,4)],ani_settings.sensing_fov_face_color,'FaceAlpha',ani_settings.sensing_fov_face_alpha, 'EdgeColor', ani_settings.sensing_camera_fov_LineColor); + patch([max_scan_range(1,1) max_scan_range(1,2) max_scan_range(1,3),max_scan_range(1,4)],[max_scan_range(2,1) max_scan_range(2,2) max_scan_range(2,3),max_scan_range(2,4)],[max_scan_range(3,1) max_scan_range(3,2) max_scan_range(3,3),max_scan_range(3,4)],ani_settings.sensing_fov_face_color,'FaceAlpha',ani_settings.sensing_fov_face_alpha, 'EdgeColor', ani_settings.sensing_camera_fov_LineColor); + end + end + + if strcmp(sensing_camera_param.sensing_type, 'circle') + + r = sensing_camera_param.circular_radius_from_base_pos; + t = linspace(0, 2*pi, 50); + + % Arc + X1 = [r*cos(t) ; r*sin(t) ; ones(1,length(t)) * surface_param.floor_level]; + X2 = [r*cos(-t) ; r*sin(-t) ; ones(1,length(t))*surface_param.floor_level]; + + % Move origin + X1 = X1+[SV.R0(1:2);0]; + X2 = X2+[SV.R0(1:2);0]; + + % Adjust inclination + X1 = rot' * X1; + X2 = rot' * X2; + + % Plot + plot3([X1(1,:) X2(1,:) X1(1,1)],[X1(2,:) X2(2,:) X1(2,1)],[X1(3,:) X2(3,:) X1(3,1)]+0.005,'Color',ani_settings.sensing_camera_fov_LineColor,'LineWidth',ani_settings.sensing_camera_fov_LineWidth); + + + + end +end +end + diff --git a/src/visualization/vis_stability_polyhedron.m b/src/visualization/vis_stability_polyhedron.m index 31b6dcc..a1873a3 100644 --- a/src/visualization/vis_stability_polyhedron.m +++ b/src/visualization/vis_stability_polyhedron.m @@ -1,14 +1,15 @@ %%%%%% Results %%%%%% vis_stability_polyhedron %%%%%% -%%%%%% Draw stability polyhedron +%%%%%% Draw stability polyhedron (gia stable region) %%%%%% %%%%%% Created 2020-02-05 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-07-01 +%%%%%% Last update: 2021-08-20 +%%%%%% Warley Ribeiro % % -% Draw stablity polyhedron for acceleration from intersection points with ground surface +% Draw stablity polyhedron (gia stable region) for acceleration from intersection points with ground surface % % Function variables: % @@ -21,7 +22,7 @@ function vis_stability_polyhedron(inc, equilibrium_eval_param, ani_settings) -if strcmp(equilibrium_eval_param.plot_polyhedron,'on') +if strcmp(ani_settings.gia_stable_region_show,'on') && equilibrium_eval_param.tumbling_axes_number > 1 % Rotation matrix rot = rpy2dc([0;pi*inc/180;0])'; % Rotate polyhedron to match surface inclination @@ -29,10 +30,10 @@ function vis_stability_polyhedron(inc, equilibrium_eval_param, ani_settings) for i = 1:size(polyhedron.vertex,2)-1 patch('Vertices',[polyhedron.vertex(:,i) polyhedron.vertex(:,i+1) polyhedron.vertex(:,end)]',... - 'Faces',[1 2 3],'FaceAlpha',ani_settings.polyh_Face_alpha ,'Edgecolor',ani_settings.polyh_Edge_color ,'Facecolor',ani_settings.polyh_Face_color,'LineWidth',ani_settings.polyh_Edge_width) + 'Faces',[1 2 3],'FaceAlpha',ani_settings.gia_stable_region_Face_alpha ,'Edgecolor',ani_settings.gia_stable_region_Edge_color ,'Facecolor',ani_settings.gia_stable_region_Face_color,'LineWidth',ani_settings.gia_stable_region_Edge_width) end patch('Vertices',[polyhedron.vertex(:,end-1) polyhedron.vertex(:,1) polyhedron.vertex(:,end)]',... - 'Faces',[1 2 3],'FaceAlpha',ani_settings.polyh_Face_alpha ,'Edgecolor',ani_settings.polyh_Edge_color ,'Facecolor',ani_settings.polyh_Face_color,'LineWidth',ani_settings.polyh_Edge_width) + 'Faces',[1 2 3],'FaceAlpha',ani_settings.gia_stable_region_Face_alpha ,'Edgecolor',ani_settings.gia_stable_region_Edge_color ,'Facecolor',ani_settings.gia_stable_region_Face_color,'LineWidth',ani_settings.gia_stable_region_Edge_width) end end \ No newline at end of file diff --git a/src/visualization/vis_support_triangle.m b/src/visualization/vis_support_triangle.m index 9f66a5d..6a5a81b 100644 --- a/src/visualization/vis_support_triangle.m +++ b/src/visualization/vis_support_triangle.m @@ -5,7 +5,7 @@ %%%%%% %%%%%% Created 2020-05-14 %%%%%% Yusuke Koizumi -%%%%%% Last update: 2020-10-16 +%%%%%% Last update: 2021-05-17 %%%%%% Kentaro Uno % % @@ -17,6 +17,7 @@ % - % INPUT % SV : State Variables +% LP : Link Parameter % POS_e : Position of end-effector % inc : Surface inclination [deg] (scalar) % ani_settings.support_triangle_show 'on', 'off' @@ -24,15 +25,32 @@ % ani_settings.support_triangle_alpha : Transparency of triangle (scalar) % ani_settings.support_triangle_edge_color : edge line color of triangle [RGB] (1x3 vector) -function vis_support_triangle(SV, POS_e,inc, ani_settings) +function vis_support_triangle(SV, LP, POS_e, inc, ani_settings) + if strcmp(ani_settings.support_triangle_show,'on') i=find(~SV.sup); p = POS_e; + p(3,:) = p(3,:) + 0.015; % support polygon should be drawn a bit higher not to be burried in the surface visualization if ~isempty(i) p(:,i) = []; end p = rpy2dc([0;pi*inc/180;0])*p; - fill3(p(1,:),p(2,:),p(3,:)+0.005, ani_settings.support_triangle_color, 'FaceAlpha', ani_settings.support_triangle_alpha, ... + fill3(p(1,:),p(2,:),p(3,:), ani_settings.support_triangle_color, 'FaceAlpha', ani_settings.support_triangle_alpha, ... 'EdgeColor', ani_settings.support_triangle_edge_color); end + +if strcmp(ani_settings.robot_top_support_triangle_show,'on') + i=find(~SV.sup); + p = POS_e; + CoM = upd_CoM(LP, SV); +% p(3,:) = p(3,:) + 0.015; % support polygon should be drawn a bit higher not to be burried in the surface visualization + if ~isempty(i) + p(:,i) = []; + end + base_position_in_inertial_frame = rpy2dc([0;pi*inc/180;0]) * SV.R0; + fill3(p(1,:)*cos(pi*inc/180),p(2,:),p(3,:)+base_position_in_inertial_frame(3)+2.0*CoM(3), ani_settings.support_triangle_color, 'FaceAlpha', ani_settings.support_triangle_alpha, ... + 'EdgeColor', ani_settings.support_triangle_edge_color); +end + + end \ No newline at end of file diff --git a/src/visualization/vis_surface.m b/src/visualization/vis_surface.m index 13aa7a8..29f1fa6 100644 --- a/src/visualization/vis_surface.m +++ b/src/visualization/vis_surface.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2020-04-09 %%%%%% Warley Ribeiro -%%%%%% Last update: 2020-04-17 +%%%%%% Last update: 2021-04-19 +%%%%%% Keigo Haji % % % Draw map from .mat file @@ -18,20 +19,38 @@ % inc : Surface inclination [deg] (scalar) % ani_settings.grid_color : Color for surface grid % ani_settings.surface_color : Color for surface +% environment_param.surface_type : surface name +% -function vis_surface(inc, ani_settings) - +function vis_surface(inc, ani_settings, environment_param, surface_param) +if strcmp(ani_settings.surface_show,'on') + % Global variables for map positions global x; global y; global z % Total number of points n = size(z,1)*size(z,2); [X,Y] = meshgrid(x,y); +% Creat vectors array +MAP(1,:) = reshape(X,1,size(X,1)*size(X,2)); +MAP(2,:) = reshape(Y,1,size(Y,1)*size(Y,2)); +MAP(3,:) = reshape(z , 1 , n ); -if inc ~= 0 - % Creat vectors array - MAP(1,:) = reshape(X,1,size(X,1)*size(X,2)); - MAP(2,:) = reshape(Y,1,size(Y,1)*size(Y,2)); - MAP(3,:) = reshape(z , 1 , n ); +if strfind(environment_param.surface_type,'climbing_holds') > 0 + % Plot climbing holds map + map = MAP; + if inc ~= 0 + % Rotate map to match the surface inclination + rot = rpy2dc([0;pi*inc/180;0])'; + map = rot' * MAP; + end + % Plot points by scatter3 + scatter3(map(1,:),map(2,:),map(3,:),'Marker','.','CData',MAP(3,:)); + colormap(ani_settings.surface_color); + % Adjust colormap properly + caxis([surface_param.min surface_param.max]); +else + % Plot grid map + if inc ~= 0 % Rotate map to match the surface inclination rot = rpy2dc([0;pi*inc/180;0])'; map = rot' * MAP; @@ -39,15 +58,14 @@ function vis_surface(inc, ani_settings) X = reshape(map(1,:),size(x,2),size(y,2)); Y = reshape(map(2,:),size(x,2),size(y,2)); Z = reshape(map(3,:),size(X,1),size(X,2)); -else + else Z = z; -end - -if strcmp(ani_settings.surface_show,'on') + end % Plot map mesh(X,Y,Z,'edgecolor', ani_settings.grid_color); colormap(ani_settings.surface_color); end +end end \ No newline at end of file diff --git a/src/visualization/vis_trajectory_4legged.m b/src/visualization/vis_trajectory_4legged.m index 191f5c2..44fd7b0 100644 --- a/src/visualization/vis_trajectory_4legged.m +++ b/src/visualization/vis_trajectory_4legged.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2019-08-07 %%%%%% Warley Ribeiro -%%%%%% Last update: 2019-08-07 +%%%%%% Last update: 2021-04-26 +%%%%%% Kentaro Uno % % % Draw trajectory of all 4 legs of a robot @@ -15,29 +16,27 @@ % OUTPUT % - % INPUT -% environment_param : Parameters for environment related variables (struct) -% ani_settings : Animation settings (struct) -% variables_saved : Variables to be saved (struct) +% environment_param : Parameters for environment related variables (struct) +% motion_planning_param : Motiion planning parameters (struct) +% motion_planning_param.trajectories.[LF, LH, RH, RF]: +% trajectories of EE updated in upd_motion_planning +% ani_settings : Animation settings (struct) -function vis_trajectory_4legged(environment_param,ani_settings,variables_saved) +function vis_trajectory_4legged(environment_param, motion_planning_param, ani_settings) % Visualize Trajectory if strcmp(ani_settings.trajectory_show,'on') + color = ani_settings.trajectory_color; width = ani_settings.trajectory_width; line_type = ani_settings.trajectory_line_type; inc = environment_param.inc; - trajectory1 = variables_saved.pos_e1'; - vis_trajectory(trajectory1,inc,color,width,line_type); - trajectory2 = variables_saved.pos_e2'; - vis_trajectory(trajectory2,inc,color,width,line_type); - trajectory3 = variables_saved.pos_e3'; - vis_trajectory(trajectory3,inc,color,width,line_type); - trajectory4 = variables_saved.pos_e4'; - vis_trajectory(trajectory4,inc,color,width,line_type); + vis_trajectory(motion_planning_param.trajectories.LF, inc, color, width, line_type); + vis_trajectory(motion_planning_param.trajectories.LH, inc, color, width, line_type); + vis_trajectory(motion_planning_param.trajectories.RH, inc, color, width, line_type); + vis_trajectory(motion_planning_param.trajectories.RF, inc, color, width, line_type); end - end \ No newline at end of file diff --git a/src/visualization/vis_vectors.m b/src/visualization/vis_vectors.m index e50d2c7..1107d88 100644 --- a/src/visualization/vis_vectors.m +++ b/src/visualization/vis_vectors.m @@ -5,7 +5,8 @@ %%%%%% %%%%%% Created 2019-07-02 %%%%%% Warley Ribeiro -%%%%%% Last update: 2019-07-02 +%%%%%% Last update: 2021-07-09 +%%%%%% Keigo Haji % % % Draw all selectable three-dimensional vectors @@ -20,19 +21,21 @@ % equilibrium_eval_param : Parameters for equilibrium evaluation (class) % LP : Link parameters (SpaceDyn class) % SV : State variables (SpaceDyn class) +% POS_e : Positions of end-effectors -function vis_vectors(inc,ani_settings,equilibrium_eval_param,LP,SV) +function vis_vectors(inc,ani_settings,equilibrium_eval_param,LP,SV,POS_e,gait_planning_param,sensing_camera_param) global Gravity -% Visualize Gravity -if strcmp(ani_settings.gravity_vec_show,'on') +% Visualize Gravitational Acceralation +if strcmp(ani_settings.gravitational_acceleration_vec_show,'on') CoM = upd_CoM(LP, SV); - color = ani_settings.gravity_vec_color; - width = ani_settings.gravity_vec_width; - magnitude = Gravity*equilibrium_eval_param.expansion_factor; - vis_one_vector(CoM,magnitude,inc,color,width); + color = ani_settings.gravitational_acceleration_vec_color; + width = ani_settings.gravitational_acceleration_vec_width; + magnitude = Gravity*ani_settings.acceleration_expansion_factor; + arrow_head_size = 3.0; + vis_one_vector(CoM,magnitude,inc,color,width,arrow_head_size); end % Visualize GIA @@ -40,8 +43,30 @@ function vis_vectors(inc,ani_settings,equilibrium_eval_param,LP,SV) CoM = upd_CoM(LP, SV); color = ani_settings.gia_vec_color; width = ani_settings.gia_vec_width; - magnitude = equilibrium_eval_param.gia*equilibrium_eval_param.expansion_factor; - vis_one_vector(CoM,magnitude,inc,color,width); + magnitude = equilibrium_eval_param.gia*ani_settings.acceleration_expansion_factor; + arrow_head_size = 3.0; + vis_one_vector(CoM,magnitude,inc,color,width,arrow_head_size); +end + +% Visualize Gravitational Force +if strcmp(ani_settings.gravitational_force_vec_show,'on') + CoM = upd_CoM(LP, SV); + color = ani_settings.gravitational_force_vec_color; + width = ani_settings.gravitational_force_vec_width; + magnitude = Gravity*LP.mass*ani_settings.force_expansion_factor; + arrow_head_size = 3.0; + vis_one_vector(CoM,magnitude,inc,color,width,arrow_head_size); +end + +% Visualize Reaction Force +if strcmp(ani_settings.reaction_force_show,'on') + for i = 1:LP.num_limb + color = ani_settings.reaction_force_vec_color; + width = ani_settings.reaction_force_vec_width; + arrow_head_size = 0.0; + magnitude = SV.Fe(:,3*i)*ani_settings.force_expansion_factor; + vis_one_vector(POS_e(:,i),magnitude,inc,color,width,arrow_head_size); + end end