├── .gitignore ├── LICENSE ├── README.md ├── Records ├── (sample)_2023-12-20_18-48-04 │ ├── 2023-12-20_18-48-04__feedback.dat │ ├── 2023-12-20_18-48-04__foot_step.dat │ ├── 2023-12-20_18-48-04__joint_states.dat │ ├── 2023-12-20_18-48-04__walking_pattern.dat │ └── 2023-12-20_18-48-04__walking_stabilization.dat ├── README.md └── plot_programs │ └── matlab_plot.m ├── convert_to_joint_states ├── CMakeLists.txt ├── LICENSE ├── include │ └── convert_to_joint_states │ │ └── ConvertToJointStates.hpp ├── package.xml ├── plugins.xml └── src │ └── ConvertToJointStates.cpp ├── foot_step_planner ├── CMakeLists.txt ├── LICENSE ├── include │ └── foot_step_planner │ │ └── FootStepPlanner.hpp ├── package.xml ├── plugins.xml └── src │ └── FootStepPlanner.cpp ├── kinematics ├── CMakeLists.txt ├── include │ └── kinematics │ │ ├── ForwardKinematics.hpp │ │ ├── InverseKinematics.hpp │ │ ├── Jacobian.hpp │ │ └── visibility_control.h ├── package.xml ├── plugins.xml └── src │ ├── ForwardKinematics.cpp │ ├── InverseKinematics.cpp │ └── Jacobian.cpp ├── robot_bringup ├── CMakeLists.txt ├── config │ ├── param_control.yaml │ ├── param_debug_mode.yaml │ ├── param_mode_switch.yaml │ └── param_robot_description.yaml ├── launch │ └── robot_bringup_launch.py ├── package.xml └── src │ └── ParameterServer.cpp ├── robot_description ├── CMakeLists.txt ├── config │ ├── rdc_robot_v1 │ │ ├── param_rdc_robot_v1_joints.yaml │ │ ├── param_rdc_robot_v1_limb.yaml │ │ └── param_rdc_robot_v1_name_lists.yaml │ └── robotis_op2 │ │ ├── README.md │ │ ├── param_robotis_op2_joints.yaml │ │ ├── param_robotis_op2_limb.yaml │ │ └── param_robotis_op2_name_lists.yaml ├── models │ ├── rdc_robot_v1 │ │ ├── README.md │ │ ├── meshes │ │ │ ├── body.stl │ │ │ ├── body_luggage.stl │ │ │ ├── link_heel2end.stl │ │ │ ├── link_kp2heel.stl │ │ │ ├── link_wp2kp.stl │ │ │ ├── link_wr2wp_l.stl │ │ │ └── link_wr2wp_r.stl │ │ ├── parts_properties.md │ │ ├── proto │ │ │ └── simple_model.proto │ │ └── urdf │ │ │ └── simple_model.urdf │ └── robotis_op2 │ │ ├── CHANGELOG.rst │ │ ├── README.md │ │ ├── meshes │ │ ├── geo_op_body.stl │ │ ├── geo_op_head.stl │ │ ├── geo_op_left_ankle.stl │ │ ├── geo_op_left_foot.stl │ │ ├── geo_op_left_hip-roll.stl │ │ ├── geo_op_left_hip-yaw.stl │ │ ├── geo_op_left_lower-arm.stl │ │ ├── geo_op_left_shin.stl │ │ ├── geo_op_left_shoulder.stl │ │ ├── geo_op_left_thigh.stl │ │ ├── geo_op_left_upper-arm.stl │ │ ├── geo_op_neck.stl │ │ ├── geo_op_right_ankle.stl │ │ ├── geo_op_right_foot.stl │ │ ├── geo_op_right_hip-roll.stl │ │ ├── geo_op_right_hip-yaw.stl │ │ ├── geo_op_right_lower-arm.stl │ │ ├── geo_op_right_shin.stl │ │ ├── geo_op_right_shoulder.stl │ │ ├── geo_op_right_thigh.stl │ │ └── geo_op_right_upper-arm.stl │ │ └── urdf │ │ ├── robotis_op2.gazebo.xacro │ │ ├── robotis_op2.inertia.xacro │ │ ├── robotis_op2.structure.arm.xacro │ │ ├── robotis_op2.structure.head.xacro │ │ ├── robotis_op2.structure.leg.xacro │ │ ├── robotis_op2.transmissions.xacro │ │ ├── robotis_op2.urdf │ │ ├── robotis_op2.urdf.xacro │ │ ├── robotis_op2.visuals.xacro │ │ └── robotis_op2.visuals_simple.xacro └── package.xml ├── robot_manager ├── CMakeLists.txt ├── LICENSE ├── include │ └── robot_manager │ │ ├── control_plugin_bases │ │ ├── PluginBase_ConvertToJointStates.hpp │ │ ├── PluginBase_FootStepPlanner.hpp │ │ ├── PluginBase_ForwardKinematics.hpp │ │ ├── PluginBase_InverseKinematics.hpp │ │ ├── PluginBase_Jacobian.hpp │ │ ├── PluginBase_WalkingPatternGenerator.hpp │ │ └── PluginBase_WalkingStabilizationController.hpp │ │ └── robot_manager.hpp ├── package.xml └── src │ ├── robot_manager.cpp │ └── robot_manager_main.cpp ├── robot_messages ├── CMakeLists.txt ├── msg │ ├── Feedback.msg │ ├── FootStepRecord.msg │ ├── JointStateRecord.msg │ ├── WalkingPatternRecord.msg │ └── WalkingStabilizationRecord.msg └── package.xml ├── robot_recorder ├── CMakeLists.txt ├── launch │ └── robot_recorder.launch.py ├── package.xml └── src │ ├── robot_feedback_recorder.cpp │ ├── robot_footStepPlanner_recorder.cpp │ ├── robot_jointStates_recorder.cpp │ ├── robot_walkingPatternGenerator_recorder.cpp │ └── robot_walkingStabilizationController_recorder.cpp ├── robot_tools ├── LICENSE ├── README.md ├── package.xml ├── resource │ └── robot_tools ├── robot_tools │ ├── __init__.py │ └── model_parameter_generator.py ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── robot_visualizer ├── CMakeLists.txt ├── LICENSE ├── launch │ ├── robot_visualizer.launch.py │ └── rviz.launch.py ├── package.xml └── rviz │ └── display.rviz ├── walking_pattern_generator ├── CMakeLists.txt ├── include │ └── walking_pattern_generator │ │ ├── LinearInvertedPendulumModel.hpp │ │ └── WalkingPatternGenerator.hpp ├── package.xml ├── plugins.xml └── src │ ├── LinearInvertedPendulumModel.cpp │ ├── WalkingPatternGenerator.cpp │ └── WalkingPatternGenerator_main.cpp ├── walking_stabilization_controller ├── CMakeLists.txt ├── include │ └── walking_stabilization_controller │ │ └── WalkingStabilizationController.hpp ├── package.xml ├── plugins.xml └── src │ └── WalkingStabilizationController.cpp └── webots_robot_handler ├── CMakeLists.txt ├── include └── webots_robot_handler │ └── WebotsRobotHandler.hpp ├── launch ├── __pycache__ │ └── robot_launch.cpython-38.pyc └── start_launch.py ├── package.xml ├── resource ├── robotis_op2_ros2_control.yaml ├── webots_robot_handler └── webots_robotis_op2_description.urdf ├── src └── WebotsRobotHander.cpp ├── webots_robot_handler.xml └── worlds ├── .webots_simple_world.wbproj └── webots_simple_world.wbt /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | Log/WPG_log_plot.asv 3 | Draft 4 | */**/*.pyc 5 | MEMO.txt 6 | log/ 7 | venv 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | > This README was created by translating the Japanese text using Google Translate. 2 | 3 | # ROS2_Walking_Pattern_Generator 4 | Walking Controller using ROS_2 for Humanoid Robots. 5 |

6 | A walking control software for humanoid robots using ROS 2. We are proceeding with development with an emphasis on scalability, versatility, and simplicity, and we plan to be able to make humanoid robots walk without any advanced work.
7 |
8 | 9 | ## Emvironment 10 | It is compatible with the following OS:
11 | * Ubuntu 24.04 (no WSL) ([official](https://ubuntu.com/desktop)) 12 | 13 | Not tested on other OSes, but may work.
14 | 15 | > [!NOTE] 16 | > If you use WSL, 17 | > 1. Install WSL2, ROS 2, webots_ros2 and Webots (Windows). reference: [official](https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Installation-Windows.html) 18 | > 2. Change `WEBOTS_CONTROLLER_URL`. See `./webots_robot_handler/launch/start_launch.py`. 19 | > 3. Disable vEthernet (WSL) Firewall. 20 | 21 | 22 | ## Dependencies 23 | It depends on the following software:
24 | * ROS 2 Jazzy ([official](https://docs.ros.org/en/jazzy/index.html)) 25 | * Eigen ([official](https://eigen.tuxfamily.org/index.php?title=Main_Page)) 26 | * Webots R2025a ([official](https://cyberbotics.com/)) 27 | * webots_ros2 ([official](https://github.com/cyberbotics/webots_ros2)) 28 | 29 | The version of this dependency is determined based on your development environment, so it may work with other versions.
30 | 31 | ## Install 32 | 33 | ### Step1 34 | Please install dependencies. 35 | 36 | * Install ROS 2 -> [official document](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debians.html) 37 | * Install Eigen -> Installed together with ROS 2. 38 | * Install Webots -> [official document](https://cyberbotics.com/doc/guide/installation-procedure) 39 | * Install webots_ros2 -> [official document](https://docs.ros.org/en/jazzy/Tutorials/Advanced/Simulators/Webots/Installation-Ubuntu.html) 40 | 41 | ### Step2 42 | Please clone this repository.
43 | 44 | Example: 45 | ```bash 46 | mkdir -p ~/ros2_ws/src/ && cd ~/ros2_ws/src/ 47 | git clone https://github.com/open-rdc/ROS2_Walking_Pattern_Generator.git 48 | ``` 49 | 50 | ### Step3 51 | Please build packages.
52 | 53 | Example: 54 | ```bash 55 | cd ~/ros2_ws/ 56 | colcon build --symlink-install --packages-up-to robot_messages 57 | source install/setup.bash 58 | colcon build --symlink-install 59 | source install/setup.bash 60 | ``` 61 | 62 | ## Usage 63 | Run the program with the following command: 64 | ```bash 65 | ros2 launch robot_bringup robot_bringup_launch.py 66 | ``` 67 | 68 | Example of operation by executing this command: 69 | 70 | https://github.com/open-rdc/ROS2_Walking_Pattern_Generator/assets/91410662/f884f510-2d0a-4b5f-9acd-73ad64a8446d 71 | 72 |
73 |
74 | 75 | ## Software Architecture 76 | ![software structure](https://github.com/open-rdc/ROS2_Walking_Pattern_Generator/assets/91410662/f3dd77da-4cc7-4a26-92b7-a974c0c666ee) 77 | 78 |
79 |
80 | 81 | ## License & Author 82 | The robot_description package contains parts of [ROBOTIS-GIT/ROBOTIS-OP2-Common](https://github.com/ROBOTIS-GIT/ROBOTIS-OP2-Common) (license: Apache-2.0 license).
83 | For details, see README.md in [robot_description/models/robotis_op2](https://github.com/open-rdc/ROS2_Walking_Pattern_Generator/tree/devel/robot_description/models/robotis_op2).
84 | 85 | This repository is licensed under [Apache License 2.0](https://opensource.org/license/apache-2-0/).
86 | 87 | Main developer & maintainer: Yusuke-Yamasaki-555 ([GitHub](https://github.com/Yusuke-Yamasaki-555), [X (twitter)](https://twitter.com/OdoOdomeme555), [blog (jp)](https://odome.hatenablog.com/))
88 | 89 | 90 | 91 | 92 | 104 | 105 | 118 | -------------------------------------------------------------------------------- /Records/(sample)_2023-12-20_18-48-04/2023-12-20_18-48-04__feedback.dat: -------------------------------------------------------------------------------- 1 | # record data: step_count | acceleration (x y z) | gyro (x y z) 2 | -------------------------------------------------------------------------------- /Records/(sample)_2023-12-20_18-48-04/2023-12-20_18-48-04__foot_step.dat: -------------------------------------------------------------------------------- 1 | # record data: step_count | foot_step (x y) 2 | -------------------------------------------------------------------------------- /Records/(sample)_2023-12-20_18-48-04/2023-12-20_18-48-04__joint_states.dat: -------------------------------------------------------------------------------- 1 | # record data: step_count | legL_position (6-joints [rad] value) | legR_position (6-joints [rad] value) | legL_velocity (6-joints [rad/s] value) | legR_velocity (6-joints [rad/s] value) 2 | -------------------------------------------------------------------------------- /Records/(sample)_2023-12-20_18-48-04/2023-12-20_18-48-04__walking_pattern.dat: -------------------------------------------------------------------------------- 1 | # record data: step_count | walking_pattern_position (x y z) | walking_pattern_velocity (x y z) 2 | -------------------------------------------------------------------------------- /Records/(sample)_2023-12-20_18-48-04/2023-12-20_18-48-04__walking_stabilization.dat: -------------------------------------------------------------------------------- 1 | # record data: step_count | fixed_CoG_position (x y z) | fixed_CoG_velocity (x y z) | fixed_ZMP_position (x y z) 2 | -------------------------------------------------------------------------------- /Records/README.md: -------------------------------------------------------------------------------- 1 | # Records 2 | -------------------------------------------------------------------------------- /Records/plot_programs/matlab_plot.m: -------------------------------------------------------------------------------- 1 | clear all; 2 | close all; 3 | 4 | n = newline; 5 | 6 | load("../2023-12-26_16-47-32/feedback0.dat"); 7 | load("../2023-12-26_16-47-32/foot_step0.dat"); 8 | load("../2023-12-26_16-47-32/joint_states0.dat"); 9 | load("../2023-12-26_16-47-32/walking_pattern0.dat"); 10 | load("../2023-12-26_16-47-32/walking_stabilization0.dat"); 11 | 12 | [nf, pf] = size(feedback0); 13 | [nw, pw] = size(walking_pattern0); 14 | [ns, ps] = size(foot_step0); 15 | [nj, pj] = size(joint_states0); 16 | 17 | t1feedback = 1:nf; 18 | t1walking = 1:nw; 19 | t1footstep = 1:ns; 20 | for t = 1:ns 21 | foot_step0(t,3) = foot_step0(t,3) - 0.037; 22 | end 23 | t1jointstates = 1:nj; 24 | 25 | figure; 26 | subplot(2,3,1) 27 | plot(t1feedback(1:700), feedback0(1:700,2)); 28 | title("Accelerometer X"+n+"(mapping: 0~1024. -39.24~39.24[m/s^2])") 29 | xlabel("step") 30 | ylabel("acceleration (0~1024)") 31 | ylim([400, 750]) 32 | grid on 33 | %https://cyberbotics.com/doc/reference/accelerometer 34 | subplot(2,3,2) 35 | plot(t1feedback(1:700), feedback0(1:700,3)); 36 | title("Accelerometer Y"+n+"(mapping: 0~1024. -39.24~39.24[m/s^2])") 37 | xlabel("step") 38 | ylabel("acceleration (0~1024)") 39 | ylim([400, 750]) 40 | grid on 41 | 42 | subplot(2,3,3) 43 | plot(t1feedback(1:700), feedback0(1:700,4)); 44 | title("Accelerometer Z"+n+"(mapping: 0~1024. -39.24~39.24[m/s^2])") 45 | xlabel("step") 46 | ylabel("acceleration (0~1024)") 47 | ylim([400, 750]) 48 | grid on 49 | 50 | subplot(2,3,4) 51 | plot(t1feedback(1:700), feedback0(1:700,5)); 52 | title("Gyro X"+n+"(mapping: 0~1024. -27.925~27.925[m/s^2])") 53 | xlabel("step") 54 | ylabel("gyro (0~1024)") 55 | ylim([400, 750]) 56 | grid on 57 | %https://cyberbotics.com/doc/reference/gyro 58 | subplot(2,3,5) 59 | plot(t1feedback(1:700), feedback0(1:700,6)); 60 | title("Gyro Y"+n+"(mapping: 0~1024. -27.925~27.925[m/s^2])") 61 | xlabel("step") 62 | ylabel("gyro (0~1024)") 63 | ylim([400, 750]) 64 | grid on 65 | 66 | subplot(2,3,6) 67 | plot(t1feedback(1:700), feedback0(1:700,7)); 68 | title("Gyro Z"+n+"(mapping: 0~1024. -27.925~27.925[m/s^2])") 69 | xlabel("step") 70 | ylabel("gyro (0~1024)") 71 | ylim([400, 750]) 72 | grid on 73 | 74 | figure; 75 | subplot(2,1,1) 76 | plot(t1walking, walking_pattern0(:,2)) 77 | hold on; 78 | plot(t1walking, walking_pattern0(:,3)) 79 | %plot(t1walking, walkingpattern1(:,5)) 80 | %plot(t1walking, walkingpattern1(:,6)) 81 | plot(t1walking, walking_pattern0(:,8)) 82 | plot(t1walking, walking_pattern0(:,9)) 83 | plot(t1footstep, foot_step0(:,2)) 84 | plot(t1footstep, foot_step0(:,3)) 85 | legend("CoG Pos X [m]", "CoG Pos Y [m]", "Fixed ZMP Pos X [m]", "Fixed ZMP Pos Y [m]", "ZMP Pos X [m]", "ZMP Pos Y [m]", Location="eastoutside") 86 | xlabel("step") 87 | ylabel("position [m]") 88 | title("CoG & ZMP Trajectory") 89 | grid on 90 | 91 | subplot(2,1,2) 92 | plot(walking_pattern0(:,2), walking_pattern0(:,3)) 93 | hold on; 94 | plot(walking_pattern0(:,2), walking_pattern0(:,9)) 95 | plot(walking_pattern0(:,2), foot_step0(:,3)) 96 | legend("CoG Pos [m]", "Fixed ZMP Pos [m]", "ZMP Pos [m]", Location="eastoutside") 97 | xlabel("x-axis position [m]") 98 | ylabel("y-axis position [m]") 99 | grid on 100 | 101 | %figure; 102 | %plot(t1jointstates, jointstates1(:,5)); 103 | -------------------------------------------------------------------------------- /convert_to_joint_states/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(convert_to_joint_states) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | # find dependencies 11 | find_package(ament_cmake REQUIRED) 12 | find_package(ament_cmake_ros REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(robot_manager REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(Eigen3 REQUIRED) 17 | 18 | pluginlib_export_plugin_description_file(robot_manager plugins.xml) 19 | 20 | add_library(${PROJECT_NAME} 21 | src/ConvertToJointStates.cpp 22 | ) 23 | ament_target_dependencies(${PROJECT_NAME} 24 | rclcpp 25 | robot_manager 26 | pluginlib 27 | ) 28 | target_include_directories(${PROJECT_NAME} 29 | PUBLIC 30 | $ 31 | $ 32 | $ 33 | PRIVATE 34 | include 35 | ) 36 | 37 | target_compile_definitions(${PROJECT_NAME} 38 | PRIVATE 39 | "CONVERT_TO_JOINT_STATES_BUILDING_LIBRARY" 40 | ) 41 | 42 | install( 43 | DIRECTORY include/ 44 | DESTINATION include 45 | ) 46 | install(TARGETS ${PROJECT_NAME} 47 | EXPORT export_${PROJECT_NAME} 48 | ARCHIVE DESTINATION lib 49 | LIBRARY DESTINATION lib 50 | RUNTIME DESTINATION bin 51 | ) 52 | 53 | ament_export_include_directories( 54 | include 55 | ) 56 | ament_export_libraries( 57 | ${PROJECT_NAME} 58 | ) 59 | ament_export_targets( 60 | export_${PROJECT_NAME} 61 | ) 62 | 63 | if(BUILD_TESTING) 64 | find_package(ament_lint_auto REQUIRED) 65 | # the following line skips the linter which checks for copyrights 66 | # comment the line when a copyright and license is added to all source files 67 | set(ament_cmake_copyright_FOUND TRUE) 68 | # the following line skips cpplint (only works in a git repo) 69 | # comment the line when this package is in a git repo and when 70 | # a copyright and license is added to all source files 71 | set(ament_cmake_cpplint_FOUND TRUE) 72 | ament_lint_auto_find_test_dependencies() 73 | endif() 74 | 75 | ament_package() 76 | -------------------------------------------------------------------------------- /convert_to_joint_states/include/convert_to_joint_states/ConvertToJointStates.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CONVERT_TO_JOINT_STATES_HPP 2 | #define CONVERT_TO_JOINT_STATES_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "pluginlib/class_loader.hpp" 6 | #include "robot_manager/control_plugin_bases/PluginBase_ConvertToJointStates.hpp" 7 | 8 | #include "robot_manager/control_plugin_bases/PluginBase_InverseKinematics.hpp" 9 | #include "robot_manager/control_plugin_bases/PluginBase_Jacobian.hpp" 10 | 11 | #include "Eigen/Dense" 12 | 13 | #include 14 | 15 | namespace convert_to_joint_states 16 | { 17 | pluginlib::ClassLoader ik_loader("robot_manager", "control_plugin_base::InverseKinematics"); 18 | pluginlib::ClassLoader jac_loader("robot_manager", "control_plugin_base::Jacobian"); 19 | 20 | class Default_ConvertToJointStates : public control_plugin_base::ConvertToJointStates 21 | { 22 | public: 23 | Default_ConvertToJointStates(); 24 | ~Default_ConvertToJointStates(){} 25 | 26 | std::unique_ptr convert_to_joint_states( 27 | const std::shared_ptr walking_stabilization_ptr, 28 | const std::shared_ptr foot_step_ptr, 29 | uint32_t walking_step, 30 | uint32_t control_step, 31 | float t 32 | ) override; 33 | 34 | private: 35 | // std::unique_ptr leg_joint_states_pat_ptr_ = std::make_unique(); 36 | 37 | Eigen::Vector3d Convert_strVec2eigenVec(const std::string str_vec); 38 | Eigen::Vector3d Convert_strVec2eigenVec_UnitVec(const std::string str_vec, std::vector* direction_); 39 | 40 | std::shared_ptr ik_; 41 | std::shared_ptr jac_; 42 | 43 | std::shared_ptr legL_states_ik_ptr_ = std::make_shared(); 44 | std::shared_ptr legR_states_ik_ptr_ = std::make_shared(); 45 | std::shared_ptr legL_states_jac_ptr_ = std::make_shared(); 46 | std::shared_ptr legR_states_jac_ptr_ = std::make_shared(); 47 | 48 | // parameters 49 | // client parameters 50 | rclcpp::Node::SharedPtr node_ptr_; 51 | std::shared_ptr client_param_; 52 | // param: description 53 | std::string ROBOT_NAME_; 54 | // param: control 55 | double WAIST_HEIGHT_ = 0; 56 | double HEIGHT_LEG_LIFT_ = 0; 57 | double WALKING_CYCLE_ = 0; 58 | double CONTROL_CYCLE_ = 0; 59 | double BOTH_LEG_SUPPORT_PERIOD_ = 0; 60 | double SINGLE_LEG_SUPPORT_PERIOD_ = 0; 61 | // param limb 62 | std::string LEFT_LEG_NAME_; 63 | std::string RIGHT_LEG_NAME_; 64 | std::vector LEFT_LEG_UNIT_VECTOR_STRING_; 65 | std::vector RIGHT_LEG_UNIT_VECTOR_STRING_; 66 | std::vector LEFT_LEG_LINK_LENGTH_STRING_; 67 | std::vector RIGHT_LEG_LINK_LENGTH_STRING_; 68 | std::array LEFT_LEG_UNIT_VECTOR_; 69 | std::array RIGHT_LEG_UNIT_VECTOR_; 70 | std::array LEFT_LEG_LINK_LENGTH_; 71 | std::array RIGHT_LEG_LINK_LENGTH_; 72 | // robot 73 | std::vector left_leg_unit_vector_direction_; 74 | std::vector right_leg_unit_vector_direction_; 75 | // std::array UnitVec_legL_; 76 | // std::array UnitVec_legR_; 77 | // std::array P_legL_waist_standard_; 78 | // std::array P_legR_waist_standard_; 79 | Eigen::Matrix3d end_eff_rot_; 80 | // float length_leg_ = 0; 81 | // // time 82 | // float control_cycle_ = 0; 83 | // float T_sup_ = 0; 84 | // float T_dsup_ = 0; 85 | 86 | // std::ofstream WPG_log_FootTrajectory; 87 | // std::string WPG_log_FootTrajectory_path = "src/Log/WPG_log_FootTrajectory.dat"; 88 | // std::ofstream WPG_log_SwingTrajectory; 89 | // std::string WPG_WPG_log_SwingTrajectory_path = "src/Log/WPG_log_SwingTrajectory.dat"; 90 | 91 | // trajectory 92 | double swing_trajectory_ = 0.0; // 遊脚軌道の値を記録 93 | double old_swing_trajectory_ = 0.0; // 微分用 94 | double vel_swing_trajectory_ = 0.0; // 遊脚軌道の速度 95 | // float height_leg_lift_ = 0; // 遊脚軌道の足上げ高さ 96 | 97 | // IKと歩行パラメータの定義・遊脚軌道の反映 98 | Eigen::Vector Foot_3D_Pos_ = {0, 0, 0}; 99 | Eigen::Vector Foot_3D_Pos_Swing_ = {0, 0 ,0}; 100 | Eigen::Vector CoG_3D_Vel_ = {0, 0, 0, 0, 0, 0}; 101 | Eigen::Vector CoG_3D_Vel_Swing_ = {0, 0, 0, 0, 0, 0}; 102 | std::array Q_legR_{0, 0, 0, 0, 0, 0}; 103 | std::array Q_legL_{0, 0, 0, 0, 0, 0}; 104 | Eigen::Vector jointVel_legR_ = {0, 0, 0, 0, 0, 0}; 105 | Eigen::Vector jointVel_legL_ = {0, 0, 0, 0, 0, 0}; 106 | Eigen::Matrix Jacobi_legR_ = Eigen::MatrixXd::Zero(6, 6); 107 | Eigen::Matrix Jacobi_legL_ = Eigen::MatrixXd::Zero(6, 6); 108 | }; 109 | } 110 | 111 | #endif -------------------------------------------------------------------------------- /convert_to_joint_states/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | convert_to_joint_states 5 | 0.0.0 6 | TODO: Package description 7 | ubu22 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | robot_manager 14 | pluginlib 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /convert_to_joint_states/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Default convert_to_joint_states plugin. 4 | 5 | -------------------------------------------------------------------------------- /foot_step_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(foot_step_planner) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | # find dependencies 11 | find_package(ament_cmake REQUIRED) 12 | find_package(ament_cmake_ros REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(robot_manager REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | 17 | pluginlib_export_plugin_description_file(robot_manager plugins.xml) 18 | 19 | add_library(foot_step_planner 20 | src/FootStepPlanner.cpp 21 | ) 22 | ament_target_dependencies(foot_step_planner 23 | robot_manager 24 | rclcpp 25 | pluginlib 26 | ) 27 | target_include_directories(foot_step_planner 28 | PUBLIC 29 | $ 30 | $ 31 | PRIVATE 32 | include 33 | ) 34 | 35 | target_compile_definitions(foot_step_planner 36 | PRIVATE 37 | "FOOT_STEP_PLANNER_BUILDING_LIBRARY" 38 | ) 39 | 40 | install( 41 | DIRECTORY include/ 42 | DESTINATION include 43 | ) 44 | install(TARGETS foot_step_planner 45 | EXPORT export_${PROJECT_NAME} 46 | ARCHIVE DESTINATION lib 47 | LIBRARY DESTINATION lib 48 | RUNTIME DESTINATION bin 49 | ) 50 | ament_export_include_directories( 51 | include 52 | ) 53 | ament_export_libraries( 54 | foot_step_planner 55 | ) 56 | ament_export_targets( 57 | export_${PROJECT_NAME} 58 | ) 59 | 60 | if(BUILD_TESTING) 61 | find_package(ament_lint_auto REQUIRED) 62 | # the following line skips the linter which checks for copyrights 63 | # comment the line when a copyright and license is added to all source files 64 | set(ament_cmake_copyright_FOUND TRUE) 65 | # the following line skips cpplint (only works in a git repo) 66 | # comment the line when this package is in a git repo and when 67 | # a copyright and license is added to all source files 68 | set(ament_cmake_cpplint_FOUND TRUE) 69 | ament_lint_auto_find_test_dependencies() 70 | endif() 71 | 72 | ament_package() 73 | -------------------------------------------------------------------------------- /foot_step_planner/include/foot_step_planner/FootStepPlanner.hpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "robot_manager/control_plugin_bases/PluginBase_FootStepPlanner.hpp" 3 | 4 | namespace foot_step_planner 5 | { 6 | class Default_FootStepPlanner : public control_plugin_base::FootStepPlanner 7 | { 8 | public: 9 | Default_FootStepPlanner(); 10 | ~Default_FootStepPlanner(){} 11 | 12 | std::unique_ptr foot_step_planner(void) override; 13 | 14 | private: 15 | rclcpp::Node::SharedPtr node_ptr_; 16 | std::shared_ptr client_param_; 17 | 18 | double WALKING_CYCLE_ = 0; 19 | double WAIST_HEIGHT_ = 0; 20 | }; 21 | } -------------------------------------------------------------------------------- /foot_step_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | foot_step_planner 5 | 0.0.0 6 | TODO: Package description 7 | ubu22 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | robot_manager 14 | pluginlib 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /foot_step_planner/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Default foot_step_planner plugin. 4 | 5 | -------------------------------------------------------------------------------- /foot_step_planner/src/FootStepPlanner.cpp: -------------------------------------------------------------------------------- 1 | #include "foot_step_planner/FootStepPlanner.hpp" 2 | 3 | namespace foot_step_planner 4 | { 5 | std::unique_ptr Default_FootStepPlanner::foot_step_planner(void) { 6 | auto foot_step_ptr = std::make_unique(); 7 | 8 | foot_step_ptr->walking_step_time = WALKING_CYCLE_; // 歩行周期[s] 9 | 10 | // world 11 | /* foot_step_ptr->foot_pos = { // 着地位置{x, y}[m] 12 | {0.0, 0.037}, 13 | {0.0, 0.074}, 14 | {0.03, 0.0}, 15 | {0.06, 0.074}, 16 | {0.09, 0.0}, 17 | {0.12, 0.074}, 18 | {0.12, 0.037}, 19 | {0.12, 0.037} 20 | }; */ 21 | 22 | // local 23 | foot_step_ptr->foot_pos = { 24 | {0.0, 0.0}, 25 | {0.0, 0.037}, 26 | {0.03, -0.037}, 27 | {0.03, 0.037}, 28 | {0.03, -0.037}, 29 | {0.03, 0.037}, 30 | {0.0, 0.0}, 31 | {0.0, 0.0} 32 | }; 33 | 34 | foot_step_ptr->waist_height = WAIST_HEIGHT_; 35 | //foot_step_ptr->waist_height = 171.856 / 1000; // 腰高さ 36 | 37 | // std::cout << "Here is default foot_step_controller plugin." << std::endl; 38 | 39 | return foot_step_ptr; 40 | } 41 | 42 | Default_FootStepPlanner::Default_FootStepPlanner() { 43 | node_ptr_ = rclcpp::Node::make_shared("FootStepPlanner"); 44 | client_param_ = std::make_shared(node_ptr_, "RobotParameterServer"); 45 | 46 | WALKING_CYCLE_ = client_param_->get_parameter("control_times.walking_cycle"); 47 | WAIST_HEIGHT_ = client_param_->get_parameter("control_constant.waist_pos_z"); 48 | 49 | RCLCPP_INFO(node_ptr_->get_logger(), "Start Up FootStepPlanner."); 50 | } 51 | } 52 | 53 | 54 | #include 55 | 56 | PLUGINLIB_EXPORT_CLASS(foot_step_planner::Default_FootStepPlanner, control_plugin_base::FootStepPlanner) 57 | -------------------------------------------------------------------------------- /kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(kinematics) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | # find dependencies 11 | find_package(ament_cmake REQUIRED) 12 | find_package(ament_cmake_ros REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(robot_manager REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(Eigen3 REQUIRED) 17 | 18 | pluginlib_export_plugin_description_file(robot_manager plugins.xml) 19 | 20 | add_library(kinematics 21 | src/ForwardKinematics.cpp 22 | src/InverseKinematics.cpp 23 | src/Jacobian.cpp 24 | ) 25 | # add_library(InverseKinematics 26 | # src/InverseKinematics.cpp 27 | # ) 28 | # add_library(Jacobian 29 | # src/Jacobian.cpp 30 | # ) 31 | 32 | ament_target_dependencies(kinematics 33 | rclcpp 34 | pluginlib 35 | robot_manager 36 | ) 37 | # ament_target_dependencies(InverseKinematics 38 | # rclcpp 39 | # pluginlib 40 | # robot_manager 41 | # ) 42 | # ament_target_dependencies(Jacobian 43 | # rclcpp 44 | # pluginlib 45 | # robot_manager 46 | # ) 47 | 48 | target_compile_features(kinematics # Require C99 and C++17 49 | PUBLIC 50 | c_std_99 51 | cxx_std_17 52 | ) 53 | # target_compile_features(InverseKinematics # Require C99 and C++17 54 | # PUBLIC 55 | # c_std_99 56 | # cxx_std_17 57 | # ) 58 | # target_compile_features(Jacobian 59 | # PUBLIC 60 | # c_std_99 61 | # cxx_std_17 62 | # ) 63 | 64 | target_include_directories(kinematics 65 | PUBLIC 66 | $ 67 | $ 68 | $ 69 | ) 70 | # target_include_directories(InverseKinematics 71 | # PUBLIC 72 | # $ 73 | # $ 74 | # $ 75 | # ) 76 | # target_include_directories(Jacobian 77 | # PUBLIC 78 | # $ 79 | # $ 80 | # $ 81 | # ) 82 | 83 | target_compile_definitions(kinematics 84 | PRIVATE 85 | "KINEMATICS_BUILDING_LIBRARY" 86 | ) 87 | # target_compile_definitions(InverseKinematics 88 | # PRIVATE 89 | # "KINEMATICS_BUILDING_LIBRARY" 90 | # ) 91 | # target_compile_definitions(Jacobian 92 | # PRIVATE 93 | # "KINEMATICS_BUILDING_LIBRARY" 94 | # ) 95 | 96 | install( 97 | DIRECTORY 98 | include/ 99 | DESTINATION 100 | include 101 | ) 102 | install(TARGETS kinematics 103 | # ForwardKinematics 104 | # Jacobian 105 | # InverseKinematics 106 | EXPORT export_${PROJECT_NAME} 107 | ARCHIVE DESTINATION lib 108 | LIBRARY DESTINATION lib 109 | RUNTIME DESTINATION bin 110 | ) 111 | 112 | 113 | ament_export_include_directories( 114 | include 115 | ) 116 | ament_export_libraries( 117 | kinematics 118 | # ForwardKinematics 119 | # InverseKinematics 120 | # Jacobian 121 | ) 122 | ament_export_targets( 123 | export_${PROJECT_NAME} 124 | HAS_LIBRARY_TARGET 125 | ) 126 | 127 | if(BUILD_TESTING) 128 | find_package(ament_lint_auto REQUIRED) 129 | # the following line skips the linter which checks for copyrights 130 | # comment the line when a copyright and license is added to all source files 131 | set(ament_cmake_copyright_FOUND TRUE) 132 | # the following line skips cpplint (only works in a git repo) 133 | # comment the line when this package is in a git repo and when 134 | # a copyright and license is added to all source files 135 | set(ament_cmake_cpplint_FOUND TRUE) 136 | ament_lint_auto_find_test_dependencies() 137 | endif() 138 | 139 | ament_package() 140 | -------------------------------------------------------------------------------- /kinematics/include/kinematics/ForwardKinematics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FORWARD_KINEMATICS_HPP 2 | #define FORWARD_KINEMATICS_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "robot_manager/control_plugin_bases/PluginBase_ForwardKinematics.hpp" 6 | 7 | #include "Eigen/Dense" 8 | 9 | namespace kinematics 10 | { 11 | class Default_ForwardKinematics : public control_plugin_base::ForwardKinematics { 12 | public: 13 | Default_ForwardKinematics(){} 14 | ~Default_ForwardKinematics(){} 15 | 16 | void forward_kinematics( 17 | std::shared_ptr leg_states_ptr, 18 | Eigen::Vector3d& end_eff_pos_ptr 19 | ) override; 20 | void forward_kinematics( 21 | std::shared_ptr leg_states_ptr, 22 | int joint_point, 23 | Eigen::Vector3d& end_eff_pos_ptr 24 | ) override; 25 | 26 | std::array getR_leg( 27 | std::array Q_leg 28 | ); 29 | 30 | private: 31 | // TODO: これら汎用的なやつは、他のところに分けたい。これこそライブラリとかにしたい。 32 | Eigen::Matrix3d Rx(double rad = 0); // ここ、Eigen系の関数として、また別に共有ライブラリを作りたい 33 | Eigen::Matrix3d Ry(double rad = 0); 34 | Eigen::Matrix3d Rz(double rad = 0); 35 | Eigen::Matrix3d IdentifyMatrix(void); 36 | }; 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /kinematics/include/kinematics/InverseKinematics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INVERSE_KINEMATICS_HPP 2 | #define INVERSE_KINEMATICS_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "robot_manager/control_plugin_bases/PluginBase_InverseKinematics.hpp" 6 | 7 | #include "Eigen/Dense" 8 | 9 | namespace kinematics 10 | { 11 | class Default_InverseKinematics : public control_plugin_base::InverseKinematics { 12 | public: 13 | Default_InverseKinematics(){} 14 | ~Default_InverseKinematics(){} 15 | 16 | void inverse_kinematics( 17 | const std::shared_ptr leg_states_ptr, 18 | std::array& joint_ang_ptr 19 | ) override; 20 | 21 | // 便利関数として、他ライブラリにまとめたい 22 | Eigen::Vector3d Array2Vector(std::array array); // std::array型をEigen::Vector3d型に変換（３次元） 23 | Eigen::Matrix3d Array2Matrix(std::array array); // std::array型をEigen::Matrix3d型に変換（3*3行列） 24 | 25 | private: 26 | Eigen::Matrix3d Rx(double rad = 0); 27 | Eigen::Matrix3d Ry(double rad = 0); 28 | Eigen::Matrix3d Rz(double rad = 0); 29 | Eigen::Matrix3d IdentifyMatrix(void); 30 | 31 | double sign(double arg = 0); // return 1 or -1 (argが>=0なら1, <0なら-1を返す) 32 | 33 | const float pi_ = 3.141593; // 四捨五入済み 34 | 35 | private: 36 | }; 37 | 38 | } 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /kinematics/include/kinematics/Jacobian.hpp: -------------------------------------------------------------------------------- 1 | #ifndef JACOBIAN_HPP 2 | #define JACOBIAN_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "pluginlib/class_loader.hpp" 6 | #include "robot_manager/control_plugin_bases/PluginBase_Jacobian.hpp" 7 | #include "robot_manager/control_plugin_bases/PluginBase_ForwardKinematics.hpp" 8 | 9 | #include "Eigen/Dense" 10 | 11 | namespace kinematics 12 | { 13 | pluginlib::ClassLoader fk_loader("robot_manager", "control_plugin_base::ForwardKinematics"); 14 | 15 | class Default_Jacobian : public control_plugin_base::Jacobian { 16 | public: 17 | Default_Jacobian(); 18 | ~Default_Jacobian(){} 19 | 20 | void jacobian( 21 | const std::shared_ptr leg_states_jac_ptr, 22 | Eigen::Matrix& leg_jacobian 23 | ) override; 24 | 25 | private: 26 | std::shared_ptr fk_; 27 | }; 28 | } 29 | 30 | #endif -------------------------------------------------------------------------------- /kinematics/include/kinematics/visibility_control.h: -------------------------------------------------------------------------------- 1 | #ifndef KINEMATICS__VISIBILITY_CONTROL_H_ 2 | #define KINEMATICS__VISIBILITY_CONTROL_H_ 3 | 4 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 5 | // https://gcc.gnu.org/wiki/Visibility 6 | 7 | #if defined _WIN32 || defined __CYGWIN__ 8 | #ifdef __GNUC__ 9 | #define KINEMATICS_EXPORT __attribute__ ((dllexport)) 10 | #define KINEMATICS_IMPORT __attribute__ ((dllimport)) 11 | #else 12 | #define KINEMATICS_EXPORT __declspec(dllexport) 13 | #define KINEMATICS_IMPORT __declspec(dllimport) 14 | #endif 15 | #ifdef KINEMATICS_BUILDING_LIBRARY 16 | #define KINEMATICS_PUBLIC KINEMATICS_EXPORT 17 | #else 18 | #define KINEMATICS_PUBLIC KINEMATICS_IMPORT 19 | #endif 20 | #define KINEMATICS_PUBLIC_TYPE KINEMATICS_PUBLIC 21 | #define KINEMATICS_LOCAL 22 | #else 23 | #define KINEMATICS_EXPORT __attribute__ ((visibility("default"))) 24 | #define KINEMATICS_IMPORT 25 | #if __GNUC__ >= 4 26 | #define KINEMATICS_PUBLIC __attribute__ ((visibility("default"))) 27 | #define KINEMATICS_LOCAL __attribute__ ((visibility("hidden"))) 28 | #else 29 | #define KINEMATICS_PUBLIC 30 | #define KINEMATICS_LOCAL 31 | #endif 32 | #define KINEMATICS_PUBLIC_TYPE 33 | #endif 34 | 35 | #endif // KINEMATICS__VISIBILITY_CONTROL_H_ 36 | -------------------------------------------------------------------------------- /kinematics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | kinematics 5 | 0.0.0 6 | TODO: Package description 7 | Yusuke Yamasaki 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | robot_manager 14 | pluginlib 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /kinematics/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Default forward_kinematics plugin. 4 | 5 | 6 | Default inverse_kinematics plugin. 7 | 8 | 9 | Default jacobian plugin. 10 | 11 | -------------------------------------------------------------------------------- /kinematics/src/ForwardKinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "kinematics/ForwardKinematics.hpp" 2 | 3 | using namespace Eigen; 4 | 5 | namespace kinematics 6 | { 7 | // 3D Rotation Matrix 8 | Matrix3d Default_ForwardKinematics::IdentifyMatrix() { 9 | Matrix3d I; 10 | I << 1, 0, 0, 11 | 0, 1, 0, 12 | 0, 0, 1; 13 | return(I); 14 | } 15 | Matrix3d Default_ForwardKinematics::Rx(double rad) { 16 | Matrix3d R_x; 17 | R_x << 1, 0, 0, 18 | 0, cos(rad), -sin(rad), 19 | 0, sin(rad), cos(rad); 20 | return(R_x); 21 | } 22 | Matrix3d Default_ForwardKinematics::Ry(double rad) { 23 | Matrix3d R_y; 24 | R_y << cos(rad), 0, sin(rad), 25 | 0, 1, 0, 26 | -sin(rad), 0, cos(rad); 27 | return(R_y); 28 | } 29 | Matrix3d Default_ForwardKinematics::Rz(double rad) { 30 | Matrix3d R_z; 31 | R_z << cos(rad), -sin(rad), 0, 32 | sin(rad), cos(rad), 0, 33 | 0, 0, 1; 34 | return(R_z); 35 | } 36 | 37 | // CHECKME: あってもなくても良い。オーバーヘッドの違い次第。 38 | std::array Default_ForwardKinematics::getR_leg( 39 | std::array Q_leg 40 | ) { 41 | return {Rz(Q_leg[0]), Rx(Q_leg[1]), Ry(Q_leg[2]), Ry(Q_leg[3]), Ry(Q_leg[4]), Rx(Q_leg[5])}; 42 | } 43 | 44 | void Default_ForwardKinematics::forward_kinematics( 45 | std::shared_ptr leg_states_ptr, 46 | Eigen::Vector3d& end_eff_pos_ptr 47 | ) { 48 | Default_ForwardKinematics::forward_kinematics( 49 | leg_states_ptr, 50 | 6, 51 | end_eff_pos_ptr 52 | ); 53 | } 54 | 55 | // 関数のオーバーライドをして、joint_pointを入れずに導出できるようにする。 or forで再帰的に回す。 56 | void Default_ForwardKinematics::forward_kinematics( 57 | std::shared_ptr leg_states_ptr, 58 | int joint_point, 59 | Eigen::Vector3d& end_eff_pos_ptr 60 | ) { 61 | std::array joint_rot = Default_ForwardKinematics::getR_leg(leg_states_ptr->joint_ang); 62 | 63 | // std::cout << "Here is default forward kinematics class." << std::endl; 64 | 65 | // CHECKME: 全て値を参照しに行っているが、値のコピーとのオーバーヘッドを比較するべき。参照のオーバーヘッドが溜まってバカにならないかも。 66 | switch(joint_point) { 67 | case 0: 68 | end_eff_pos_ptr = 69 | leg_states_ptr->link_len[0]; 70 | break; 71 | case 1: 72 | end_eff_pos_ptr = 73 | joint_rot[0] * leg_states_ptr->link_len[1] 74 | + leg_states_ptr->link_len[0]; 75 | break; 76 | case 2: 77 | end_eff_pos_ptr = 78 | joint_rot[0] * joint_rot[1] * leg_states_ptr->link_len[2] 79 | + joint_rot[0] * leg_states_ptr->link_len[1] 80 | + leg_states_ptr->link_len[0]; 81 | break; 82 | case 3: 83 | end_eff_pos_ptr = 84 | joint_rot[0] * joint_rot[1] * joint_rot[2] * leg_states_ptr->link_len[3] 85 | + joint_rot[0] * joint_rot[1] * leg_states_ptr->link_len[2] 86 | + joint_rot[0] * leg_states_ptr->link_len[1] 87 | + leg_states_ptr->link_len[0]; 88 | break; 89 | case 4: 90 | end_eff_pos_ptr = 91 | joint_rot[0] * joint_rot[1] * joint_rot[2] * joint_rot[3] * leg_states_ptr->link_len[4] 92 | + joint_rot[0] * joint_rot[1] * joint_rot[2] * leg_states_ptr->link_len[3] 93 | + joint_rot[0] * joint_rot[1] * leg_states_ptr->link_len[2] 94 | + joint_rot[0] * leg_states_ptr->link_len[1] 95 | + leg_states_ptr->link_len[0]; 96 | break; 97 | case 5: 98 | end_eff_pos_ptr = 99 | joint_rot[0] * joint_rot[1] * joint_rot[2] * joint_rot[3] * joint_rot[4] * leg_states_ptr->link_len[5] 100 | + joint_rot[0] * joint_rot[1] * joint_rot[2] * joint_rot[3] * leg_states_ptr->link_len[4] 101 | + joint_rot[0] * joint_rot[1] * joint_rot[2] * leg_states_ptr->link_len[3] 102 | + joint_rot[0] * joint_rot[1] * leg_states_ptr->link_len[2] 103 | + joint_rot[0] * leg_states_ptr->link_len[1] 104 | + leg_states_ptr->link_len[0]; 105 | break; 106 | case 6: 107 | end_eff_pos_ptr = 108 | joint_rot[0] * joint_rot[1] * joint_rot[2] * joint_rot[3] * joint_rot[4] * joint_rot[5] * leg_states_ptr->link_len[6] 109 | + joint_rot[0] * joint_rot[1] * joint_rot[2] * joint_rot[3] * joint_rot[4] * leg_states_ptr->link_len[5] 110 | + joint_rot[0] * joint_rot[1] * joint_rot[2] * joint_rot[3] * leg_states_ptr->link_len[4] 111 | + joint_rot[0] * joint_rot[1] * joint_rot[2] * leg_states_ptr->link_len[3] 112 | + joint_rot[0] * joint_rot[1] * leg_states_ptr->link_len[2] 113 | + joint_rot[0] * leg_states_ptr->link_len[1] 114 | + leg_states_ptr->link_len[0]; 115 | break; 116 | } 117 | } 118 | } 119 | 120 | #include 121 | 122 | PLUGINLIB_EXPORT_CLASS(kinematics::Default_ForwardKinematics, control_plugin_base::ForwardKinematics) -------------------------------------------------------------------------------- /kinematics/src/InverseKinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "kinematics/InverseKinematics.hpp" 2 | 3 | using namespace Eigen; 4 | 5 | namespace kinematics 6 | { 7 | Vector3d Default_InverseKinematics::Array2Vector(std::array array) { 8 | return {array[0], array[1], array[2]}; 9 | } 10 | Matrix3d Default_InverseKinematics::Array2Matrix(std::array array) { 11 | Matrix3d R; 12 | R << array[0], array[1], array[2], 13 | array[3], array[4], array[5], 14 | array[6], array[7], array[8]; 15 | return R; 16 | } 17 | 18 | Matrix3d Default_InverseKinematics::IdentifyMatrix() { 19 | Matrix3d I; 20 | I << 1, 0, 0, 21 | 0, 1, 0, 22 | 0, 0, 1; 23 | return(I); 24 | } 25 | Matrix3d Default_InverseKinematics::Rx(double rad) { 26 | Matrix3d R_x; 27 | R_x << 1, 0, 0, 28 | 0, cos(rad), -sin(rad), 29 | 0, sin(rad), cos(rad); 30 | return(R_x); 31 | } 32 | Matrix3d Default_InverseKinematics::Ry(double rad) { 33 | Matrix3d R_y; 34 | R_y << cos(rad), 0, sin(rad), 35 | 0, 1, 0, 36 | -sin(rad), 0, cos(rad); 37 | return(R_y); 38 | } 39 | Matrix3d Default_InverseKinematics::Rz(double rad) { 40 | Matrix3d R_z; 41 | R_z << cos(rad), -sin(rad), 0, 42 | sin(rad), cos(rad), 0, 43 | 0, 0, 1; 44 | return(R_z); 45 | } 46 | 47 | double Default_InverseKinematics::sign(double arg) { 48 | return((arg >= 0) - (arg < 0)); // result 1 or -1 (true == 1, false == 0) 49 | } 50 | 51 | // IK (ROBOTIS-OP2's Leg only. analytical method) 52 | void Default_InverseKinematics::inverse_kinematics( 53 | const std::shared_ptr leg_states_ptr, 54 | std::array& joint_ang_ptr 55 | ) { 56 | std::array Q; 57 | Vector3d P_target_leg_zhip2end; // 股関節(z軸関節)から末端まで 58 | P_target_leg_zhip2end = leg_states_ptr->end_eff_rot.transpose() * (leg_states_ptr->link_len[0] - leg_states_ptr->end_eff_pos); 59 | 60 | double a, b, c; 61 | a = std::abs(leg_states_ptr->link_len[3](2)); 62 | b = std::abs(leg_states_ptr->link_len[4](2)); 63 | c = std::sqrt(std::pow(P_target_leg_zhip2end(0), 2) + std::pow(P_target_leg_zhip2end(1), 2) + std::pow(P_target_leg_zhip2end(2), 2)); // pow(A, B) == A^B =- AのB乗 64 | 65 | Q[3] = -1 * std::acos((std::pow(a, 2) + std::pow(b, 2) - std::pow(c, 2)) / (2 * a * b)) + pi_; 66 | 67 | double d; 68 | d = std::asin((a * sin(pi_ - Q[3])) / c); 69 | 70 | Q[4] = -1 * std::atan2(P_target_leg_zhip2end(0), sign(P_target_leg_zhip2end(2)) * std::sqrt(std::pow(P_target_leg_zhip2end(1), 2) + std::pow(P_target_leg_zhip2end(2), 2))) - d; 71 | Q[5] = std::atan2(P_target_leg_zhip2end(1), P_target_leg_zhip2end(2)); 72 | 73 | Matrix3d R_target_leg_begin2yhip; // 基準から股関節(y軸関節)まで 74 | R_target_leg_begin2yhip = leg_states_ptr->end_eff_rot * Rx(Q[5]).inverse() * Ry(Q[3]+Q[4]).inverse(); 75 | // std::cout << R_target_leg_begin2yhip << std::endl; 76 | // std::cout << R_target_leg_begin2yhip(0, 1) << std::endl; 77 | Q[0] = std::atan2(-R_target_leg_begin2yhip(0, 1), R_target_leg_begin2yhip(1, 1)); 78 | Q[1] = std::atan2(R_target_leg_begin2yhip(2, 1), -R_target_leg_begin2yhip(0, 1) * std::sin(Q[0]) + R_target_leg_begin2yhip(1, 1) * std::cos(Q[0])); 79 | Q[2] = std::atan2(-R_target_leg_begin2yhip(2, 0), R_target_leg_begin2yhip(2, 2)); 80 | 81 | joint_ang_ptr = Q; 82 | } 83 | } 84 | 85 | #include 86 | 87 | PLUGINLIB_EXPORT_CLASS(kinematics::Default_InverseKinematics, control_plugin_base::InverseKinematics) -------------------------------------------------------------------------------- /kinematics/src/Jacobian.cpp: -------------------------------------------------------------------------------- 1 | #include "kinematics/Jacobian.hpp" 2 | 3 | using namespace Eigen; 4 | 5 | namespace kinematics 6 | { 7 | void Default_Jacobian::jacobian( 8 | const std::shared_ptr leg_states_jac_ptr, 9 | Eigen::Matrix& leg_jacobian_ptr 10 | ) { 11 | leg_jacobian_ptr = MatrixXd::Zero(6, leg_states_jac_ptr->unit_vec.max_size()); 12 | 13 | std::array P_FK_leg; 14 | Vector3d end_eff_pos = {0, 0, 0}; 15 | std::shared_ptr leg_states_fk_ptr = std::make_shared(); 16 | leg_states_fk_ptr->joint_ang = leg_states_jac_ptr->joint_ang; 17 | leg_states_fk_ptr->link_len = leg_states_jac_ptr->link_len; 18 | for(int joint_point = 0; joint_point < int(leg_states_jac_ptr->unit_vec.max_size()); joint_point++) { 19 | fk_->forward_kinematics(leg_states_fk_ptr, joint_point, end_eff_pos); 20 | P_FK_leg[joint_point] = end_eff_pos; 21 | // std::cout << P_FK_leg[joint_point].transpose() << std::endl; 22 | } 23 | 24 | Vector3d mat_leg = Vector3d::Zero(3); 25 | Vector3d pt_P_leg = Vector3d::Zero(3); 26 | for(int tag = 0; tag < int(leg_states_jac_ptr->unit_vec.max_size()); tag++) { 27 | if(tag == int(leg_states_jac_ptr->unit_vec.max_size()-1)) { 28 | mat_leg = Vector3d::Zero(3); 29 | } 30 | else { 31 | pt_P_leg = P_FK_leg[int(leg_states_jac_ptr->unit_vec.max_size())-1] - P_FK_leg[tag]; 32 | // std::cout << "pt_P_leg: " << pt_P_leg.transpose() << ", " << P_FK_leg[int(leg_states_jac_ptr->unit_vec.max_size())-1].transpose() << ", " << P_FK_leg[tag].transpose() << std::endl; 33 | mat_leg = leg_states_jac_ptr->unit_vec[tag].cross(pt_P_leg); 34 | } 35 | 36 | for(int i = 0; i < 3; i++) { 37 | leg_jacobian_ptr(i, tag) = mat_leg[i]; 38 | leg_jacobian_ptr(i+3, tag) = leg_states_jac_ptr->unit_vec[tag][i]; 39 | } 40 | } 41 | } 42 | 43 | Default_Jacobian::Default_Jacobian() { 44 | // std::cout << "Init Jac" << std::endl; 45 | fk_ = fk_loader.createSharedInstance("kinematics::Default_ForwardKinematics"); 46 | } 47 | } 48 | 49 | #include 50 | 51 | PLUGINLIB_EXPORT_CLASS(kinematics::Default_Jacobian, control_plugin_base::Jacobian) -------------------------------------------------------------------------------- /robot_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_bringup) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | 21 | add_executable(robot_parameter_server 22 | src/ParameterServer.cpp 23 | ) 24 | target_include_directories(robot_parameter_server 25 | PUBLIC 26 | $ 27 | $ 28 | ) 29 | target_compile_features(robot_parameter_server 30 | PUBLIC 31 | c_std_99 32 | cxx_std_17 33 | ) 34 | ament_target_dependencies(robot_parameter_server 35 | rclcpp 36 | ) 37 | install( 38 | TARGETS robot_parameter_server 39 | DESTINATION lib/${PROJECT_NAME} 40 | ) 41 | 42 | install(DIRECTORY 43 | launch 44 | config 45 | DESTINATION share/${PROJECT_NAME}/ 46 | ) 47 | 48 | if(BUILD_TESTING) 49 | find_package(ament_lint_auto REQUIRED) 50 | # the following line skips the linter which checks for copyrights 51 | # comment the line when a copyright and license is added to all source files 52 | set(ament_cmake_copyright_FOUND TRUE) 53 | # the following line skips cpplint (only works in a git repo) 54 | # comment the line when this package is in a git repo and when 55 | # a copyright and license is added to all source files 56 | set(ament_cmake_cpplint_FOUND TRUE) 57 | ament_lint_auto_find_test_dependencies() 58 | endif() 59 | 60 | ament_package() 61 | -------------------------------------------------------------------------------- /robot_bringup/config/param_control.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | control_constant: 4 | waist_pos_z: 0.171856 # [m] 5 | height_leg_lift: 0.025 6 | control_times: 7 | control_cycle: 0.01 # [s] 8 | walking_cycle: 0.8 9 | both_leg_support_period: 0.5 10 | single_leg_support_period: 0.3 -------------------------------------------------------------------------------- /robot_bringup/config/param_debug_mode.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | setting_debug_mode: 4 | # setting visualizer 5 | using_rviz: false 6 | rviz_file: display.rviz 7 | using_rqt: false 8 | # setting recorder 9 | using_feedback_recorder: false 10 | using_footStep_recorder: false 11 | using_walkingPattern_recorder: false 12 | using_walkingStabilization_recorder: false 13 | using_jointState_recorder: false 14 | using_rosbag2: false 15 | -------------------------------------------------------------------------------- /robot_bringup/config/param_mode_switch.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | mode_switch: 4 | on_or_offline_pattern_generate: false 5 | debug_mode: true 6 | using_simulator: true 7 | -------------------------------------------------------------------------------- /robot_bringup/config/param_robot_description.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | robot_description: 4 | robot_name: robotis_op2 5 | xacro_file_path: urdf/robotis_op2.urdf.xacro 6 | urdf_file_path: urdf/robotis_op2.urdf 7 | -------------------------------------------------------------------------------- /robot_bringup/launch/robot_bringup_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import yaml 3 | from time import sleep 4 | 5 | import launch 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | from launch.launch_description_sources import PythonLaunchDescriptionSource 9 | 10 | def generate_launch_description(): 11 | launch_description = launch.LaunchDescription() 12 | 13 | mode_switch_yaml_path = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_mode_switch.yaml") 14 | with open(mode_switch_yaml_path, "r") as f: 15 | mode_switch_yaml = yaml.safe_load(f)["/**"]["ros__parameters"]["mode_switch"] 16 | robot_description_yaml_path = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_robot_description.yaml") 17 | with open(robot_description_yaml_path, "r") as f: 18 | robot_description_yaml = yaml.safe_load(f)["/**"]["ros__parameters"]["robot_description"] 19 | 20 | # load param_file 21 | param_mode_switch_yaml = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_mode_switch.yaml") 22 | param_control_yaml = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_control.yaml") 23 | param_robot_description_yaml = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_robot_description.yaml") 24 | limb_yaml = os.path.join(get_package_share_directory("robot_description"), "config", robot_description_yaml["robot_name"], "param_"+robot_description_yaml["robot_name"]+"_limb.yaml") 25 | name_list_yaml = os.path.join(get_package_share_directory("robot_description"), "config", robot_description_yaml["robot_name"], "param_"+robot_description_yaml["robot_name"]+"_name_lists.yaml") 26 | 27 | # load param server 28 | parameter_server_node = Node( 29 | package = "robot_bringup", 30 | executable = "robot_parameter_server", 31 | output = "screen", 32 | parameters = [ 33 | param_mode_switch_yaml, 34 | param_control_yaml, 35 | param_robot_description_yaml, 36 | limb_yaml, 37 | name_list_yaml 38 | ] 39 | ) 40 | launch_description.add_action(parameter_server_node) 41 | # sleep(2) 42 | 43 | # load robot_manager node 44 | # TODO: RMの起動など、制御プログラムは他launchファイルに記述して、制御実行時にそのlaunchファイルを起動する手順を取るべき。 45 | # そうしないと、rosbagを使って再現するときに干渉しちゃうし、RMの起動を阻止しないと行けない。 46 | robot_manager = Node( 47 | package = "robot_manager", 48 | executable = "robot_manager", 49 | output = "screen" 50 | ) 51 | launch_description.add_action(robot_manager) 52 | # sleep(1) 53 | 54 | # Load visualizer launch file & recorder launch file 55 | if mode_switch_yaml["debug_mode"] == True: 56 | 57 | record_launch = launch.actions.IncludeLaunchDescription( 58 | PythonLaunchDescriptionSource([ 59 | os.path.join(get_package_share_directory("robot_recorder"), "launch"), 60 | "/robot_recorder.launch.py" 61 | ]) 62 | ) 63 | launch_description.add_action(record_launch) 64 | 65 | visual_launch = launch.actions.IncludeLaunchDescription( 66 | PythonLaunchDescriptionSource([ 67 | os.path.join(get_package_share_directory("robot_visualizer"), "launch"), 68 | "/robot_visualizer.launch.py" 69 | ]) 70 | ) 71 | launch_description.add_action(visual_launch) 72 | 73 | # load simulation launch file 74 | if mode_switch_yaml["using_simulator"] == True: 75 | sim_launch = launch.actions.IncludeLaunchDescription( 76 | PythonLaunchDescriptionSource([ 77 | os.path.join(get_package_share_directory("webots_robot_handler"), "launch"), 78 | "/start_launch.py" 79 | ]) 80 | ) 81 | launch_description.add_action(sim_launch) 82 | 83 | 84 | # Execution 85 | return launch_description -------------------------------------------------------------------------------- /robot_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_bringup 5 | 0.0.0 6 | TODO: Package description 7 | ubu22 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | rclcpp 16 | 17 | 18 | ament_cmake 19 | 20 | 21 | -------------------------------------------------------------------------------- /robot_bringup/src/ParameterServer.cpp: -------------------------------------------------------------------------------- 1 | // declare & get all_parameters 2 | // Plugin用。PluginはLaunchで起動しないので、YAMLをパラメータとして受け取れない。ので、Serverを用意して、コンストラクタ内でパラメータを通信によって受け取ってもらう。 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | class ParameterServer : public rclcpp::Node { 7 | public: 8 | ParameterServer( 9 | const rclcpp::NodeOptions &options 10 | ) : Node("RobotParameterServer", options) { 11 | 12 | // mode_switch 13 | on_or_offline_pattern_generate_ = get_parameter("mode_switch.on_or_offline_pattern_generate").as_bool(); 14 | debug_mode_ = get_parameter("mode_switch.debug_mode").as_bool(); 15 | using_simulator_ = get_parameter("mode_switch.using_simulator").as_bool(); 16 | 17 | //robot_description 18 | robot_name_ = get_parameter("robot_description.robot_name").as_string(); 19 | xacro_file_path_ = get_parameter("robot_description.xacro_file_path").as_string(); 20 | urdf_file_path_ = get_parameter("robot_description.urdf_file_path").as_string(); 21 | 22 | //control 23 | waist_pos_z_ = get_parameter("control_constant.waist_pos_z").as_double(); 24 | height_leg_lift_ = get_parameter("control_constant.height_leg_lift").as_double(); 25 | control_cycle_ = get_parameter("control_times.control_cycle").as_double(); 26 | walking_cycle_ = get_parameter("control_times.walking_cycle").as_double(); 27 | both_leg_support_period_ = get_parameter("control_times.both_leg_support_period").as_double(); 28 | single_leg_support_period_ = get_parameter("control_times.single_leg_support_period").as_double(); 29 | 30 | // limb 31 | left_leg_name_ = get_parameter(robot_name_+"_limb.limb_names.left_leg").as_string(); 32 | right_leg_name_ = get_parameter(robot_name_+"_limb.limb_names.right_leg").as_string(); 33 | left_leg_joint_names_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+left_leg_name_+".joint_names").as_string_array(); 34 | right_leg_joint_names_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+right_leg_name_+".joint_names").as_string_array(); 35 | left_leg_joint_numbers_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+left_leg_name_+".joint_numbers").as_integer_array(); 36 | right_leg_joint_numbers_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+right_leg_name_+".joint_numbers").as_integer_array(); 37 | left_leg_joint_unit_vector_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+left_leg_name_+".joint_unit_vector").as_string_array(); // TODO: 多重配列は、Paramで読めない。ので、lengthは合計しちゃって良いと思われ。それか、文字列として扱うか。ほしいのは脚の始端から末端までの座標だし。それをparam_robot_description.yamlに書き込みたい。 38 | right_leg_joint_unit_vector_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+right_leg_name_+".joint_unit_vector").as_string_array(); // TODO: 単位ベクトルは、軸方向の文字で良いのではないだろうか。それか、文字列として扱うか。 39 | left_leg_link_length_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+left_leg_name_+".link_length").as_string_array(); 40 | right_leg_link_length_ = get_parameter(robot_name_+"_limb.limb_without_fixed_joints."+right_leg_name_+".link_length").as_string_array(); 41 | 42 | // name_lists 43 | all_joint_names_without_fixed_ = get_parameter(robot_name_+"_name_lists.all_names_without_fixed_joints.all_joint_names").as_string_array(); 44 | 45 | RCLCPP_INFO(this->get_logger(), "Start Up RobotParameterServer"); 46 | } 47 | 48 | 49 | private: 50 | // param_fileに対応した変数 51 | 52 | // mode_switch 53 | bool on_or_offline_pattern_generate_; 54 | bool debug_mode_; 55 | bool using_simulator_; 56 | 57 | // robot_description 58 | std::string robot_name_; 59 | std::string xacro_file_path_; 60 | std::string urdf_file_path_; 61 | 62 | // control 63 | double waist_pos_z_; 64 | double height_leg_lift_; 65 | double control_cycle_; 66 | double walking_cycle_; 67 | double both_leg_support_period_; 68 | double single_leg_support_period_; 69 | 70 | // limb 71 | std::string left_leg_name_; 72 | std::vector left_leg_joint_names_; 73 | std::vector left_leg_joint_numbers_; 74 | std::vector left_leg_joint_unit_vector_; 75 | std::vector left_leg_link_length_; 76 | std::string right_leg_name_; 77 | std::vector right_leg_joint_names_; 78 | std::vector right_leg_joint_numbers_; 79 | std::vector right_leg_joint_unit_vector_; 80 | std::vector right_leg_link_length_; 81 | 82 | // name_lists 83 | std::vector all_joint_names_without_fixed_; 84 | 85 | 86 | }; 87 | 88 | int main(int argc, char* argv[]) { 89 | rclcpp::init(argc, argv); 90 | 91 | rclcpp::NodeOptions opt; 92 | opt.automatically_declare_parameters_from_overrides(true); 93 | 94 | rclcpp::spin(std::make_shared(opt)); 95 | 96 | rclcpp::shutdown(); 97 | 98 | return 0; 99 | } -------------------------------------------------------------------------------- /robot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | install(DIRECTORY 15 | models 16 | config 17 | DESTINATION share/${PROJECT_NAME}/ 18 | ) 19 | 20 | if(BUILD_TESTING) 21 | find_package(ament_lint_auto REQUIRED) 22 | # the following line skips the linter which checks for copyrights 23 | # comment the line when a copyright and license is added to all source files 24 | set(ament_cmake_copyright_FOUND TRUE) 25 | # the following line skips cpplint (only works in a git repo) 26 | # comment the line when this package is in a git repo and when 27 | # a copyright and license is added to all source files 28 | set(ament_cmake_cpplint_FOUND TRUE) 29 | ament_lint_auto_find_test_dependencies() 30 | endif() 31 | 32 | ament_package() 33 | -------------------------------------------------------------------------------- /robot_description/config/rdc_robot_v1/param_rdc_robot_v1_joints.yaml: -------------------------------------------------------------------------------- 1 | # Source URDF file path : ../../robot_description/models/rdc_robot_v1/urdf/simple_model.urdf 2 | # Generated by robot_tools/robot_tools/model_parameter_generator.py 3 | 4 | # joints 5 | 6 | /**: 7 | ros__parameters: 8 | rdc_robot_v1_joints: 9 | CPG: 10 | origin: 11 | xyz: 0 0 0.333 12 | rpy: 0 0 0 13 | luggage: 14 | origin: 15 | xyz: -0.091 0 0 16 | rpy: 0 0 0 17 | r_waist_roll: 18 | position_sensor_name: r_waist_roll_sensor 19 | origin: 20 | xyz: -0.091 -0.075 0 21 | rpy: 0 0 0 22 | axis: 1 0 0 23 | limit: 24 | effort: 7.2 25 | lower: -6.28 26 | upper: 6.28 27 | velocity: 11.519173063 28 | r_waist_pitch: 29 | position_sensor_name: r_waist_pitch_sensor 30 | origin: 31 | xyz: 0.091 -0.055 0.00 32 | rpy: 0 0 0 33 | axis: 0 1 0 34 | limit: 35 | effort: 7.2 36 | lower: -6.28 37 | upper: 6.28 38 | velocity: 11.519173063 39 | r_knee_pitch: 40 | position_sensor_name: r_knee_pitch_sensor 41 | origin: 42 | xyz: 0 0 -0.16 43 | rpy: 0 0 0 44 | axis: 0 1 0 45 | limit: 46 | effort: 7.2 47 | lower: -6.28 48 | upper: 6.28 49 | velocity: 11.519173063 50 | r_ankle: 51 | origin: 52 | xyz: 0 0 -0.149335 53 | rpy: 0 0 0 54 | l_waist_roll: 55 | position_sensor_name: l_waist_roll_sensor 56 | origin: 57 | xyz: -0.091 0.075 0 58 | rpy: 0 0 0 59 | axis: 1 0 0 60 | limit: 61 | effort: 7.2 62 | lower: -6.28 63 | upper: 6.28 64 | velocity: 11.519173063 65 | l_waist_pitch: 66 | position_sensor_name: l_waist_pitch_sensor 67 | origin: 68 | xyz: 0.091 0.055 0.00 69 | rpy: 0 0 0 70 | axis: 0 1 0 71 | limit: 72 | effort: 7.2 73 | lower: -6.28 74 | upper: 6.28 75 | velocity: 11.519173063 76 | l_knee_pitch: 77 | position_sensor_name: l_knee_pitch_sensor 78 | origin: 79 | xyz: 0 0 -0.16 80 | rpy: 0 0 0 81 | axis: 0 1 0 82 | limit: 83 | effort: 7.2 84 | lower: -6.28 85 | upper: 6.28 86 | velocity: 11.519173063 87 | l_ankle: 88 | origin: 89 | xyz: 0 0 -0.149335 90 | rpy: 0 0 0 91 | -------------------------------------------------------------------------------- /robot_description/config/rdc_robot_v1/param_rdc_robot_v1_limb.yaml: -------------------------------------------------------------------------------- 1 | # Source URDF file path : ../../robot_description/models/rdc_robot_v1/urdf/simple_model.urdf 2 | # Generated by robot_tools/robot_tools/model_parameter_generator.py 3 | 4 | # limb 5 | 6 | /**: 7 | ros__parameters: 8 | rdc_robot_v1_limb: 9 | limb_names: 10 | left_leg: l_leg 11 | right_leg: r_leg 12 | limb_without_fixed_joints: 13 | cpg: 14 | joint_names: null 15 | position_sensor_names: 'null' 16 | joint_numbers: null 17 | joint_unit_vector: null 18 | link_length: null 19 | joint_posture: null 20 | body: 21 | joint_names: null 22 | position_sensor_names: 'null' 23 | joint_numbers: null 24 | joint_unit_vector: null 25 | link_length: null 26 | joint_posture: null 27 | r_leg: 28 | joint_names: 29 | - r_waist_pitch 30 | - r_knee_pitch 31 | position_sensor_names: 32 | - r_waist_pitch_sensor 33 | - r_knee_pitch_sensor 34 | joint_numbers: 35 | - 0 36 | - 1 37 | joint_unit_vector: 38 | - 0 1 0 39 | - 0 1 0 40 | link_length: 41 | - 0.091 -0.055 0.00 42 | - 0 0 -0.16 43 | joint_posture: 44 | - 0 0 0 45 | - 0 0 0 46 | l_leg: 47 | joint_names: 48 | - l_waist_pitch 49 | - l_knee_pitch 50 | position_sensor_names: 51 | - l_waist_pitch_sensor 52 | - l_knee_pitch_sensor 53 | joint_numbers: 54 | - 2 55 | - 3 56 | joint_unit_vector: 57 | - 0 1 0 58 | - 0 1 0 59 | link_length: 60 | - 0.091 0.055 0.00 61 | - 0 0 -0.16 62 | joint_posture: 63 | - 0 0 0 64 | - 0 0 0 65 | limb_with_fixed_joints: 66 | cpg: 67 | joint_names: 68 | - CPG 69 | link_length: 70 | - 0 0 0.333 71 | joint_posture: 72 | - 0 0 0 73 | body: 74 | joint_names: 75 | - luggage 76 | link_length: 77 | - -0.091 0 0 78 | joint_posture: 79 | - 0 0 0 80 | r_leg: 81 | joint_names: 82 | - r_waist_roll 83 | - r_waist_pitch 84 | - r_knee_pitch 85 | - r_ankle 86 | link_length: 87 | - -0.091 -0.075 0 88 | - 0.091 -0.055 0.00 89 | - 0 0 -0.16 90 | - 0 0 -0.149335 91 | joint_posture: 92 | - 0 0 0 93 | - 0 0 0 94 | - 0 0 0 95 | - 0 0 0 96 | l_leg: 97 | joint_names: 98 | - l_waist_roll 99 | - l_waist_pitch 100 | - l_knee_pitch 101 | - l_ankle 102 | link_length: 103 | - -0.091 0.075 0 104 | - 0.091 0.055 0.00 105 | - 0 0 -0.16 106 | - 0 0 -0.149335 107 | joint_posture: 108 | - 0 0 0 109 | - 0 0 0 110 | - 0 0 0 111 | - 0 0 0 112 | -------------------------------------------------------------------------------- /robot_description/config/rdc_robot_v1/param_rdc_robot_v1_name_lists.yaml: -------------------------------------------------------------------------------- 1 | # Source URDF file path : ../../robot_description/models/rdc_robot_v1/urdf/simple_model.urdf 2 | # Generated by robot_tools/robot_tools/model_parameter_generator.py 3 | 4 | # name_lists 5 | 6 | /**: 7 | ros__parameters: 8 | rdc_robot_v1_name_lists: 9 | all_names_without_fixed_joints: 10 | all_joint_names: 11 | - r_waist_roll 12 | - r_waist_pitch 13 | - r_knee_pitch 14 | - l_waist_roll 15 | - l_waist_pitch 16 | - l_knee_pitch 17 | all_position_sensor_names: 18 | - r_waist_roll_sensor 19 | - r_waist_pitch_sensor 20 | - r_knee_pitch_sensor 21 | - l_waist_roll_sensor 22 | - l_waist_pitch_sensor 23 | - l_knee_pitch_sensor 24 | all_names_with_fixed_joints: 25 | all_joint_names: 26 | - CPG 27 | - luggage 28 | - r_waist_roll 29 | - r_waist_pitch 30 | - r_knee_pitch 31 | - r_ankle 32 | - l_waist_roll 33 | - l_waist_pitch 34 | - l_knee_pitch 35 | - l_ankle 36 | all_position_sensor_names: 37 | - null 38 | - null 39 | - r_waist_roll_sensor 40 | - r_waist_pitch_sensor 41 | - r_knee_pitch_sensor 42 | - null 43 | - l_waist_roll_sensor 44 | - l_waist_pitch_sensor 45 | - l_knee_pitch_sensor 46 | - null 47 | -------------------------------------------------------------------------------- /robot_description/config/robotis_op2/README.md: -------------------------------------------------------------------------------- 1 | # Model Parameter Generation 2 | 3 | * Generated by robot_tools/robot_tools/urdf_to_yaml.py
4 | * Source file : robot_description/models/robotis_op2/urdf/robotis_op2.urdf
-------------------------------------------------------------------------------- /robot_description/config/robotis_op2/param_robotis_op2_joints.yaml: -------------------------------------------------------------------------------- 1 | # Source URDF file path : ../../robot_description/models/robotis_op2/urdf/robotis_op2.urdf 2 | # Generated by robot_tools/robot_tools/urdf_to_yaml.py 3 | 4 | # joints 5 | 6 | /**: 7 | ros__parameters: 8 | robotis_op2_joints: 9 | base_joint: 10 | origin: 11 | xyz: 12 | - 0.0, 0.0, 0.0 13 | rpy: 14 | - 0.0, 0.0, 0.0 15 | head_pan: 16 | position_sensor_name: head_pan_sensor 17 | origin: 18 | xyz: 0 0 0.0205 19 | rpy: 0 0 0 20 | axis: 0 0 1 21 | limit: 22 | effort: 2.8 23 | lower: -2.6179939 24 | upper: 2.6179939 25 | velocity: 5.6548668 26 | head_tilt: 27 | position_sensor_name: head_tilt_sensor 28 | origin: 29 | xyz: 0 0 0.03 30 | rpy: 0 0.5759586531581288 0 31 | axis: 0 -1 0 32 | limit: 33 | effort: 2.8 34 | lower: -2.6179939 35 | upper: 2.6179939 36 | velocity: 5.6548668 37 | head_cam: 38 | origin: 39 | xyz: 0.0332 0 0.0344 40 | rpy: 0 0 0 41 | l_sho_pitch: 42 | position_sensor_name: l_sho_pitch_sensor 43 | origin: 44 | xyz: 0 0.0575 0 45 | rpy: 0 0 0 46 | axis: 0 1 0 47 | limit: 48 | effort: 2.8 49 | lower: -2.6179939 50 | upper: 2.6179939 51 | velocity: 5.6548668 52 | l_sho_roll: 53 | position_sensor_name: l_sho_roll_sensor 54 | origin: 55 | xyz: 0 0.0245 -0.016 56 | rpy: 0.7853981633974483 0 0 57 | axis: -1 0 0 58 | limit: 59 | effort: 2.8 60 | lower: -2.6179939 61 | upper: 2.6179939 62 | velocity: 5.648668 63 | l_el: 64 | position_sensor_name: l_el_sensor 65 | origin: 66 | xyz: 0.016 0 -0.06 67 | rpy: 0 -1.5707963267948966 0 68 | axis: 0 1 0 69 | limit: 70 | effort: 2.8 71 | lower: -2.6179939 72 | upper: 2.6179939 73 | velocity: 5.6548668 74 | r_sho_pitch: 75 | position_sensor_name: r_sho_pitch_sensor 76 | origin: 77 | xyz: 0 -0.0575 0 78 | rpy: 0 0 0 79 | axis: 0 -1 0 80 | limit: 81 | effort: 2.8 82 | lower: -2.6179939 83 | upper: 2.6179939 84 | velocity: 5.6548668 85 | r_sho_roll: 86 | position_sensor_name: r_sho_roll_sensor 87 | origin: 88 | xyz: 0 -0.0245 -0.016 89 | rpy: -0.7853981633974483 0 0 90 | axis: -1 0 0 91 | limit: 92 | effort: 2.8 93 | lower: -2.6179939 94 | upper: 2.6179939 95 | velocity: 5.648668 96 | r_el: 97 | position_sensor_name: r_el_sensor 98 | origin: 99 | xyz: 0.016 0 -0.06 100 | rpy: 0 -1.5707963267948966 0 101 | axis: 0 -1 0 102 | limit: 103 | effort: 2.8 104 | lower: -2.6179939 105 | upper: 2.6179939 106 | velocity: 5.6548668 107 | l_hip_yaw: 108 | position_sensor_name: l_hip_yaw_sensor 109 | origin: 110 | xyz: -0.005 0.037 -0.0907 111 | rpy: 0 0 0 112 | axis: 0 0 -1 113 | limit: 114 | effort: 2.8 115 | lower: -2.6179939 116 | upper: 2.6179939 117 | velocity: 5.6548668 118 | l_hip_roll: 119 | position_sensor_name: l_hip_roll_sensor 120 | origin: 121 | xyz: 0 0 -0.0315 122 | rpy: 0 0 0 123 | axis: -1 0 0 124 | limit: 125 | effort: 2.8 126 | lower: -2.6179939 127 | upper: 2.6179939 128 | velocity: 5.6548668 129 | l_hip_pitch: 130 | position_sensor_name: l_hip_pitch_sensor 131 | origin: 132 | xyz: 0 0 0 133 | rpy: 0 0 0 134 | axis: 0 -1 0 135 | limit: 136 | effort: 2.8 137 | lower: -2.6179939 138 | upper: 2.6179939 139 | velocity: 5.6548668 140 | l_knee: 141 | position_sensor_name: l_knee_sensor 142 | origin: 143 | xyz: 0 0 -0.093 144 | rpy: 0 0 0 145 | axis: 0 -1 0 146 | limit: 147 | effort: 2.8 148 | lower: -2.6179939 149 | upper: 2.6179939 150 | velocity: 5.6548668 151 | l_ank_pitch: 152 | position_sensor_name: l_ank_pitch_sensor 153 | origin: 154 | xyz: 0 0 -0.093 155 | rpy: 0 0 0 156 | axis: 0 1 0 157 | limit: 158 | effort: 2.8 159 | lower: -2.6179939 160 | upper: 2.6179939 161 | velocity: 5.6548668 162 | l_ank_roll: 163 | position_sensor_name: l_ank_roll_sensor 164 | origin: 165 | xyz: 0 0 0 166 | rpy: 0 0 0 167 | axis: 1 0 0 168 | limit: 169 | effort: 2.8 170 | lower: -2.6179939 171 | upper: 2.6179939 172 | velocity: 5.6548668 173 | r_hip_yaw: 174 | position_sensor_name: r_hip_yaw_sensor 175 | origin: 176 | xyz: -0.005 -0.037 -0.0907 177 | rpy: 0 0 0 178 | axis: 0 0 -1 179 | limit: 180 | effort: 2.8 181 | lower: -2.6179939 182 | upper: 2.6179939 183 | velocity: 5.6548668 184 | r_hip_roll: 185 | position_sensor_name: r_hip_roll_sensor 186 | origin: 187 | xyz: 0 0 -0.0315 188 | rpy: 0 0 0 189 | axis: -1 0 0 190 | limit: 191 | effort: 2.8 192 | lower: -2.6179939 193 | upper: 2.6179939 194 | velocity: 5.6548668 195 | r_hip_pitch: 196 | position_sensor_name: r_hip_pitch_sensor 197 | origin: 198 | xyz: 0 0 0 199 | rpy: 0 0 0 200 | axis: 0 1 0 201 | limit: 202 | effort: 2.8 203 | lower: -2.6179939 204 | upper: 2.6179939 205 | velocity: 5.6548668 206 | r_knee: 207 | position_sensor_name: r_knee_sensor 208 | origin: 209 | xyz: 0 0 -0.093 210 | rpy: 0 0 0 211 | axis: 0 1 0 212 | limit: 213 | effort: 2.8 214 | lower: -2.6179939 215 | upper: 2.6179939 216 | velocity: 5.6548668 217 | r_ank_pitch: 218 | position_sensor_name: r_ank_pitch_sensor 219 | origin: 220 | xyz: 0 0 -0.093 221 | rpy: 0 0 0 222 | axis: 0 -1 0 223 | limit: 224 | effort: 2.8 225 | lower: -2.6179939 226 | upper: 2.6179939 227 | velocity: 5.6548668 228 | r_ank_roll: 229 | position_sensor_name: r_ank_roll_sensor 230 | origin: 231 | xyz: 0 0 0 232 | rpy: 0 0 0 233 | axis: 1 0 0 234 | limit: 235 | effort: 2.8 236 | lower: -2.6179939 237 | upper: 2.6179939 238 | velocity: 5.6548668 239 | -------------------------------------------------------------------------------- /robot_description/config/robotis_op2/param_robotis_op2_limb.yaml: -------------------------------------------------------------------------------- 1 | # Source URDF file path : ../../robot_description/models/robotis_op2/urdf/robotis_op2.urdf 2 | # Generated by robot_tools/robot_tools/urdf_to_yaml.py 3 | 4 | # limb 5 | 6 | /**: 7 | ros__parameters: 8 | robotis_op2_limb: 9 | limb_names: 10 | left_leg: l_leg 11 | right_leg: r_leg 12 | limb_without_fixed_joints: 13 | base: 14 | joint_names: null 15 | position_sensor_names: 'null' 16 | joint_numbers: null 17 | joint_unit_vector: null 18 | link_length: null 19 | joint_posture: null 20 | head: 21 | joint_names: 22 | - head_pan 23 | - head_tilt 24 | position_sensor_names: 25 | - head_pan_sensor 26 | - head_tilt_sensor 27 | joint_numbers: 28 | - 0 29 | - 1 30 | joint_unit_vector: 31 | - 0 0 1 32 | - 0 -1 0 33 | link_length: 34 | - 0 0 0.0205 35 | - 0 0 0.03 36 | joint_posture: 37 | - 0 0 0 38 | - 0 0.5759586531581288 0 39 | l_arm: 40 | joint_names: 41 | - l_sho_pitch 42 | - l_sho_roll 43 | - l_el 44 | position_sensor_names: 45 | - l_sho_pitch_sensor 46 | - l_sho_roll_sensor 47 | - l_el_sensor 48 | joint_numbers: 49 | - 2 50 | - 3 51 | - 4 52 | joint_unit_vector: 53 | - 0 1 0 54 | - -1 0 0 55 | - 0 1 0 56 | link_length: 57 | - 0 0.0575 0 58 | - 0 0.0245 -0.016 59 | - 0.016 0 -0.06 60 | joint_posture: 61 | - 0 0 0 62 | - 0.7853981633974483 0 0 63 | - 0 -1.5707963267948966 0 64 | r_arm: 65 | joint_names: 66 | - r_sho_pitch 67 | - r_sho_roll 68 | - r_el 69 | position_sensor_names: 70 | - r_sho_pitch_sensor 71 | - r_sho_roll_sensor 72 | - r_el_sensor 73 | joint_numbers: 74 | - 5 75 | - 6 76 | - 7 77 | joint_unit_vector: 78 | - 0 -1 0 79 | - -1 0 0 80 | - 0 -1 0 81 | link_length: 82 | - 0 -0.0575 0 83 | - 0 -0.0245 -0.016 84 | - 0.016 0 -0.06 85 | joint_posture: 86 | - 0 0 0 87 | - -0.7853981633974483 0 0 88 | - 0 -1.5707963267948966 0 89 | l_leg: 90 | joint_names: 91 | - l_hip_yaw 92 | - l_hip_roll 93 | - l_hip_pitch 94 | - l_knee 95 | - l_ank_pitch 96 | - l_ank_roll 97 | position_sensor_names: 98 | - l_hip_yaw_sensor 99 | - l_hip_roll_sensor 100 | - l_hip_pitch_sensor 101 | - l_knee_sensor 102 | - l_ank_pitch_sensor 103 | - l_ank_roll_sensor 104 | joint_numbers: 105 | - 8 106 | - 9 107 | - 10 108 | - 11 109 | - 12 110 | - 13 111 | joint_unit_vector: 112 | - 0 0 -1 113 | - -1 0 0 114 | - 0 -1 0 115 | - 0 -1 0 116 | - 0 1 0 117 | - 1 0 0 118 | link_length: 119 | - -0.005 0.037 -0.0907 120 | - 0 0 -0.0315 121 | - 0 0 0 122 | - 0 0 -0.093 123 | - 0 0 -0.093 124 | - 0 0 0 125 | joint_posture: 126 | - 0 0 0 127 | - 0 0 0 128 | - 0 0 0 129 | - 0 0 0 130 | - 0 0 0 131 | - 0 0 0 132 | r_leg: 133 | joint_names: 134 | - r_hip_yaw 135 | - r_hip_roll 136 | - r_hip_pitch 137 | - r_knee 138 | - r_ank_pitch 139 | - r_ank_roll 140 | position_sensor_names: 141 | - r_hip_yaw_sensor 142 | - r_hip_roll_sensor 143 | - r_hip_pitch_sensor 144 | - r_knee_sensor 145 | - r_ank_pitch_sensor 146 | - r_ank_roll_sensor 147 | joint_numbers: 148 | - 14 149 | - 15 150 | - 16 151 | - 17 152 | - 18 153 | - 19 154 | joint_unit_vector: 155 | - 0 0 -1 156 | - -1 0 0 157 | - 0 1 0 158 | - 0 1 0 159 | - 0 -1 0 160 | - 1 0 0 161 | link_length: 162 | - -0.005 -0.037 -0.0907 163 | - 0 0 -0.0315 164 | - 0 0 0 165 | - 0 0 -0.093 166 | - 0 0 -0.093 167 | - 0 0 0 168 | joint_posture: 169 | - 0 0 0 170 | - 0 0 0 171 | - 0 0 0 172 | - 0 0 0 173 | - 0 0 0 174 | - 0 0 0 175 | limb_with_fixed_joints: 176 | base: 177 | joint_names: 178 | - base_joint 179 | link_length: 180 | - 0.0, 0.0, 0.0 181 | joint_posture: 182 | - 0.0, 0.0, 0.0 183 | head: 184 | joint_names: 185 | - head_pan 186 | - head_tilt 187 | - head_cam 188 | link_length: 189 | - 0 0 0.0205 190 | - 0 0 0.03 191 | - 0.0332 0 0.0344 192 | joint_posture: 193 | - 0 0 0 194 | - 0 0.5759586531581288 0 195 | - 0 0 0 196 | l_arm: 197 | joint_names: 198 | - l_sho_pitch 199 | - l_sho_roll 200 | - l_el 201 | link_length: 202 | - 0 0.0575 0 203 | - 0 0.0245 -0.016 204 | - 0.016 0 -0.06 205 | joint_posture: 206 | - 0 0 0 207 | - 0.7853981633974483 0 0 208 | - 0 -1.5707963267948966 0 209 | r_arm: 210 | joint_names: 211 | - r_sho_pitch 212 | - r_sho_roll 213 | - r_el 214 | link_length: 215 | - 0 -0.0575 0 216 | - 0 -0.0245 -0.016 217 | - 0.016 0 -0.06 218 | joint_posture: 219 | - 0 0 0 220 | - -0.7853981633974483 0 0 221 | - 0 -1.5707963267948966 0 222 | l_leg: 223 | joint_names: 224 | - l_hip_yaw 225 | - l_hip_roll 226 | - l_hip_pitch 227 | - l_knee 228 | - l_ank_pitch 229 | - l_ank_roll 230 | link_length: 231 | - -0.005 0.037 -0.0907 232 | - 0 0 -0.0315 233 | - 0 0 0 234 | - 0 0 -0.093 235 | - 0 0 -0.093 236 | - 0 0 0 237 | joint_posture: 238 | - 0 0 0 239 | - 0 0 0 240 | - 0 0 0 241 | - 0 0 0 242 | - 0 0 0 243 | - 0 0 0 244 | r_leg: 245 | joint_names: 246 | - r_hip_yaw 247 | - r_hip_roll 248 | - r_hip_pitch 249 | - r_knee 250 | - r_ank_pitch 251 | - r_ank_roll 252 | link_length: 253 | - -0.005 -0.037 -0.0907 254 | - 0 0 -0.0315 255 | - 0 0 0 256 | - 0 0 -0.093 257 | - 0 0 -0.093 258 | - 0 0 0 259 | joint_posture: 260 | - 0 0 0 261 | - 0 0 0 262 | - 0 0 0 263 | - 0 0 0 264 | - 0 0 0 265 | - 0 0 0 266 | -------------------------------------------------------------------------------- /robot_description/config/robotis_op2/param_robotis_op2_name_lists.yaml: -------------------------------------------------------------------------------- 1 | # Source URDF file path : ../../robot_description/models/robotis_op2/urdf/robotis_op2.urdf 2 | # Generated by robot_tools/robot_tools/urdf_to_yaml.py 3 | 4 | # name_lists 5 | 6 | /**: 7 | ros__parameters: 8 | robotis_op2_name_lists: 9 | all_names_without_fixed_joints: 10 | all_joint_names: 11 | - head_pan 12 | - head_tilt 13 | - l_sho_pitch 14 | - l_sho_roll 15 | - l_el 16 | - r_sho_pitch 17 | - r_sho_roll 18 | - r_el 19 | - l_hip_yaw 20 | - l_hip_roll 21 | - l_hip_pitch 22 | - l_knee 23 | - l_ank_pitch 24 | - l_ank_roll 25 | - r_hip_yaw 26 | - r_hip_roll 27 | - r_hip_pitch 28 | - r_knee 29 | - r_ank_pitch 30 | - r_ank_roll 31 | all_position_sensor_names: 32 | - head_pan_sensor 33 | - head_tilt_sensor 34 | - l_sho_pitch_sensor 35 | - l_sho_roll_sensor 36 | - l_el_sensor 37 | - r_sho_pitch_sensor 38 | - r_sho_roll_sensor 39 | - r_el_sensor 40 | - l_hip_yaw_sensor 41 | - l_hip_roll_sensor 42 | - l_hip_pitch_sensor 43 | - l_knee_sensor 44 | - l_ank_pitch_sensor 45 | - l_ank_roll_sensor 46 | - r_hip_yaw_sensor 47 | - r_hip_roll_sensor 48 | - r_hip_pitch_sensor 49 | - r_knee_sensor 50 | - r_ank_pitch_sensor 51 | - r_ank_roll_sensor 52 | all_names_with_fixed_joints: 53 | all_joint_names: 54 | - base_joint 55 | - head_pan 56 | - head_tilt 57 | - head_cam 58 | - l_sho_pitch 59 | - l_sho_roll 60 | - l_el 61 | - r_sho_pitch 62 | - r_sho_roll 63 | - r_el 64 | - l_hip_yaw 65 | - l_hip_roll 66 | - l_hip_pitch 67 | - l_knee 68 | - l_ank_pitch 69 | - l_ank_roll 70 | - r_hip_yaw 71 | - r_hip_roll 72 | - r_hip_pitch 73 | - r_knee 74 | - r_ank_pitch 75 | - r_ank_roll 76 | all_position_sensor_names: 77 | - null 78 | - head_pan_sensor 79 | - head_tilt_sensor 80 | - null 81 | - l_sho_pitch_sensor 82 | - l_sho_roll_sensor 83 | - l_el_sensor 84 | - r_sho_pitch_sensor 85 | - r_sho_roll_sensor 86 | - r_el_sensor 87 | - l_hip_yaw_sensor 88 | - l_hip_roll_sensor 89 | - l_hip_pitch_sensor 90 | - l_knee_sensor 91 | - l_ank_pitch_sensor 92 | - l_ank_roll_sensor 93 | - r_hip_yaw_sensor 94 | - r_hip_roll_sensor 95 | - r_hip_pitch_sensor 96 | - r_knee_sensor 97 | - r_ank_pitch_sensor 98 | - r_ank_roll_sensor 99 | -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/README.md: -------------------------------------------------------------------------------- 1 | # RDC_Bipedal_Robot 2 | (version1: 2025/5/10) 3 | 4 | For details, see [https://github.com/open-rdc/Bipedal_Robot_Hardware/](https://github.com/open-rdc/Bipedal_Robot_Hardware/) 5 | 6 | ## License, etc 7 | 8 | `./proto/simple_model.proto` was made using [cyberbotics/urdf2webots](https://github.com/cyberbotics/urdf2webots) -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/meshes/body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/rdc_robot_v1/meshes/body.stl -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/meshes/body_luggage.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/rdc_robot_v1/meshes/body_luggage.stl -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/meshes/link_heel2end.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/rdc_robot_v1/meshes/link_heel2end.stl -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/meshes/link_kp2heel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/rdc_robot_v1/meshes/link_kp2heel.stl -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/meshes/link_wp2kp.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/rdc_robot_v1/meshes/link_wp2kp.stl -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/meshes/link_wr2wp_l.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/rdc_robot_v1/meshes/link_wr2wp_l.stl -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/meshes/link_wr2wp_r.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/rdc_robot_v1/meshes/link_wr2wp_r.stl -------------------------------------------------------------------------------- /robot_description/models/rdc_robot_v1/parts_properties.md: -------------------------------------------------------------------------------- 1 | ### BR2_parts_09.ipt (body) 2 | 物理プロパティ BR2_parts_09 3 | 一般的なプロパティ: 4 | 材料: {アルミニウム 6061} 5 | 密度: 2.700 g/cm^3 6 | 質量: 1128.279 g (相対誤差 = 0.006896%) 7 | 領域: 180817.438 mm^2 (相対誤差 = 0.000000%) 8 | 体積: 417881.111 mm^3 (相対誤差 = 0.006896%) 9 | 重心: 10 | X: 165.000 mm (相対誤差 = 0.006896%) 11 | Y: 129.349 mm (相対誤差 = 0.006896%) 12 | Z: 44.429 mm (相対誤差 = 0.006896%) 13 | 重心に関する慣性の質量モーメント(負の値を使って計算) 14 | Ixx 7256487.589 g mm^2 (相対誤差 = 0.006896%) 15 | Iyx Iyy -0.000 g mm^2 (相対誤差 = 0.006896%) 12763191.197 g mm^2 (相対誤差 = 0.006896%) 16 | Izx Izy Izz -0.000 g mm^2 (相対誤差 = 0.006896%) -2040679.720 g mm^2 (相対誤差 = 0.006896%) 12655750.700 g mm^2 (相対誤差 = 0.006896%) 17 | グローバルに関する慣性の質量モーメント(負の値を使って計算) 18 | Ixx 28361118.173 g mm^2 (相対誤差 = 0.006896%) 19 | Iyx Iyy -24080427.514 g mm^2 (相対誤差 = 0.006896%) 45707738.684 g mm^2 (相対誤差 = 0.006896%) 20 | Izx Izy Izz -8271172.884 g mm^2 (相対誤差 = 0.006896%) -8524737.829 g mm^2 (相対誤差 = 0.006896%) 62250625.327 g mm^2 (相対誤差 = 0.006896%) 21 | 重心に関する慣性の基本モーメント 22 | I1: 7256487.589 g mm^2 (相対誤差 = 0.006896%) 23 | I2: 14750857.631 g mm^2 (相対誤差 = 0.006896%) 24 | I3: 10668084.267 g mm^2 (相対誤差 = 0.006896%) 25 | グローバルから焦点への回転 26 | Rx: 44.25 deg (相対誤差 = 0.006896%) 27 | Ry: 0.00 deg (相対誤差 = 0.006896%) 28 | Rz: 0.00 deg (相対誤差 = 0.006896%) 29 | 30 | ### BR3_box.ipt (body_luggage) 31 | 物理プロパティ BR3_box 32 | 一般的なプロパティ: 33 | 材料: {ABS プラスチック} 34 | 密度: 1.060 g/cm^3 35 | 質量: 6002.284 g (相対誤差 = 0.000001%) 36 | 領域: 253635.742 mm^2 (相対誤差 = 0.000000%) 37 | 体積: 5662532.355 mm^3 (相対誤差 = 0.000001%) 38 | 重心: 39 | X: 150.000 mm (相対誤差 = 0.000001%) 40 | Y: 64.260 mm (相対誤差 = 0.000001%) 41 | Z: 75.000 mm (相対誤差 = 0.000001%) 42 | 重心に関する慣性の質量モーメント(負の値を使って計算) 43 | Ixx 21219922.175 g mm^2 (相対誤差 = 0.000001%) 44 | Iyx Iyy 0.000 g mm^2 (相対誤差 = 0.000001%) 59412361.786 g mm^2 (相対誤差 = 0.000001%) 45 | Izx Izy Izz -0.000 g mm^2 (相対誤差 = 0.000001%) 0.000 g mm^2 (相対誤差 = 0.000001%) 56129347.719 g mm^2 (相対誤差 = 0.000001%) 46 | グローバルに関する慣性の質量モーメント(負の値を使って計算) 47 | Ixx 79768565.902 g mm^2 (相対誤差 = 0.000001%) 48 | Iyx Iyy -57856340.818 g mm^2 (相対誤差 = 0.000001%) 228226607.627 g mm^2 (相対誤差 = 0.000001%) 49 | Izx Izy Izz -67525698.336 g mm^2 (相対誤差 = 0.000001%) -28928170.409 g mm^2 (相対誤差 = 0.000001%) 215966538.951 g mm^2 (相対誤差 = 0.000001%) 50 | 重心に関する慣性の基本モーメント 51 | I1: 21219922.175 g mm^2 (相対誤差 = 0.000001%) 52 | I2: 59412361.786 g mm^2 (相対誤差 = 0.000001%) 53 | I3: 56129347.719 g mm^2 (相対誤差 = 0.000001%) 54 | グローバルから焦点への回転 55 | Rx: 0.00 deg (相対誤差 = 0.000001%) 56 | Ry: 0.00 deg (相対誤差 = 0.000001%) 57 | Rz: 0.00 deg (相対誤差 = 0.000001%) 58 | 59 | ### A_hip(R).iam (一部パーツ不足)（wr2wp） 60 | 物理プロパティ A_hip(R) 61 | 一般的なプロパティ: 62 | 材料: {} 63 | 密度: 2.865 g/cm^3 64 | *質量: 1463.019 g (相対誤差 = 0.502991%) 65 | 領域: 209892.372 mm^2 (相対誤差 = 0.001242%) 66 | 体積: 510693.470 mm^3 (相対誤差 = 0.502991%) 67 | **重心: 68 | X: 2.577 mm (相対誤差 = 0.502991%) 69 | Y: 17.925 mm (相対誤差 = 0.502991%) 70 | Z: -39.004 mm (相対誤差 = 0.502991%) 71 | **重心に関する慣性の質量モーメント(負の値を使って計算) 72 | Ixx 1834107.432 g mm^2 (相対誤差 = 0.502991%) 73 | Iyx Iyy 6118.591 g mm^2 (相対誤差 = 0.502991%) 3223868.349 g mm^2 (相対誤差 = 0.502991%) 74 | Izx Izy Izz -347126.339 g mm^2 (相対誤差 = 0.502991%) 6132.139 g mm^2 (相対誤差 = 0.502991%) 3178256.399 g mm^2 (相対誤差 = 0.502991%) 75 | **グローバルに関する慣性の質量モーメント(負の値を使って計算) 76 | Ixx 4529948.932 g mm^2 (相対誤差 = 0.502991%) 77 | Iyx Iyy -61472.250 g mm^2 (相対誤差 = 0.502991%) 5459339.296 g mm^2 (相対誤差 = 0.502991%) 78 | Izx Izy Izz -200052.219 g mm^2 (相対誤差 = 0.502991%) 1029021.047 g mm^2 (相対誤差 = 0.502991%) 3658063.790 g mm^2 (相対誤差 = 0.502991%) 79 | **重心に関する慣性の基本モーメント 80 | I1: 1749718.494 g mm^2 (相対誤差 = 0.502991%) 81 | I2: 3223385.949 g mm^2 (相対誤差 = 0.502991%) 82 | I3: 3263127.737 g mm^2 (相対誤差 = 0.502991%) 83 | **グローバルから焦点への回転 84 | Rx: 6.75 deg (相対誤差 = 0.502991%) 85 | Ry: 13.53 deg (相対誤差 = 0.502991%) 86 | Rz: 1.88 deg (相対誤差 = 0.502991%) 87 | *計算結果はユーザが上書きした値に基づいています 88 | **この値は、ユーザがオーバーライドした質量や体積を反映しません 89 | 90 | 91 | ### A_calf(L).iam (一部パーツ不足)（wp2kp） 92 | 物理プロパティ A_calf(L) 93 | 一般的なプロパティ: 94 | 材料: {} 95 | 密度: 2.040 g/cm^3 96 | *質量: 573.746 g (相対誤差 = 0.008410%) 97 | 領域: 169876.121 mm^2 (相対誤差 = 0.000000%) 98 | *体積: 281180.153 mm^3 (相対誤差 = 0.008410%) 99 | **重心: 100 | X: 146.545 mm (相対誤差 = 0.008410%) 101 | Y: -30.766 mm (相対誤差 = 0.008410%) 102 | Z: 98.569 mm (相対誤差 = 0.008410%) 103 | **重心に関する慣性の質量モーメント(負の値を使って計算) 104 | Ixx 2806720.657 g mm^2 (相対誤差 = 0.008410%) 105 | Iyx Iyy -48377.919 g mm^2 (相対誤差 = 0.008410%) 3314673.129 g mm^2 (相対誤差 = 0.008410%) 106 | Izx Izy Izz -541781.155 g mm^2 (相対誤差 = 0.008410%) -42839.916 g mm^2 (相対誤差 = 0.008410%) 704027.535 g mm^2 (相対誤差 = 0.008410%) 107 | **グローバルに関する慣性の質量モーメント(負の値を使って計算) 108 | Ixx 8924230.058 g mm^2 (相対誤差 = 0.008410%) 109 | Iyx Iyy 2538443.383 g mm^2 (相対誤差 = 0.008410%) 21210477.606 g mm^2 (相対誤差 = 0.008410%) 110 | Izx Izy Izz -8829395.754 g mm^2 (相対誤差 = 0.008410%) 1697108.531 g mm^2 (相対誤差 = 0.008410%) 13568506.253 g mm^2 (相対誤差 = 0.008410%) 111 | **重心に関する慣性の基本モーメント 112 | I1: 2934532.383 g mm^2 (相対誤差 = 0.008410%) 113 | I2: 3319273.043 g mm^2 (相対誤差 = 0.008410%) 114 | I3: 571615.895 g mm^2 (相対誤差 = 0.008410%) 115 | **グローバルから焦点への回転 116 | Rx: 1.14 deg (相対誤差 = 0.008410%) 117 | Ry: -13.65 deg (相対誤差 = 0.008410%) 118 | Rz: -5.80 deg (相対誤差 = 0.008410%) 119 | *計算結果はユーザが上書きした値に基づいています 120 | **この値は、ユーザがオーバーライドした質量や体積を反映しません 121 | 122 | ### A_foot(L).iam (一部パーツ不足)（kp2heel & heel2end） 123 | 物理プロパティ A_foot(L) 124 | 一般的なプロパティ: 125 | 材料: {} 126 | 密度: 1.451 g/cm^3 127 | 質量: 199.496 g (相対誤差 = 0.035448%) 128 | 領域: 42421.170 mm^2 (相対誤差 = 0.002724%) 129 | 体積: 137498.674 mm^3 (相対誤差 = 0.035448%) 130 | 重心: 131 | X: 89.099 mm (相対誤差 = 0.035448%) 132 | Y: 34.755 mm (相対誤差 = 0.035448%) 133 | Z: 241.054 mm (相対誤差 = 0.035448%) 134 | 重心に関する慣性の質量モーメント(負の値を使って計算) 135 | Ixx 232792.167 g mm^2 (相対誤差 = 0.035448%) 136 | Iyx Iyy 43988.430 g mm^2 (相対誤差 = 0.035448%) 685815.781 g mm^2 (相対誤差 = 0.035448%) 137 | Izx Izy Izz 289070.271 g mm^2 (相対誤差 = 0.035448%) -27898.665 g mm^2 (相対誤差 = 0.035448%) 482483.748 g mm^2 (相対誤差 = 0.035448%) 138 | グローバルに関する慣性の質量モーメント(負の値を使って計算) 139 | Ixx 12065936.553 g mm^2 (相対誤差 = 0.035448%) 140 | Iyx Iyy -573783.812 g mm^2 (相対誤差 = 0.035448%) 13861710.587 g mm^2 (相対誤差 = 0.035448%) 141 | Izx Izy Izz -3995651.070 g mm^2 (相対誤差 = 0.035448%) -1699260.334 g mm^2 (相対誤差 = 0.035448%) 2307188.892 g mm^2 (相対誤差 = 0.035448%) 142 | 重心に関する慣性の基本モーメント 143 | I1: 38569.077 g mm^2 (相対誤差 = 0.035448%) 144 | I2: 690047.835 g mm^2 (相対誤差 = 0.035448%) 145 | I3: 672474.784 g mm^2 (相対誤差 = 0.035448%) 146 | グローバルから焦点への回転 147 | Rx: -3.30 deg (相対誤差 = 0.035448%) 148 | Ry: -33.05 deg (相対誤差 = 0.035448%) 149 | Rz: 6.40 deg (相対誤差 = 0.035448%) 150 | 151 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for robot_description/models/robotis_op2 directory 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.1 (2023-12-17) 6 | ------------------ 7 | * add robotis_op2.urdf generated by ``ros2 run xacro xacro``. 8 | * disable the code below in all files in the urdf directory. 9 | ```` 10 | * change xacro:include file path & mesh file path. 11 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/README.md: -------------------------------------------------------------------------------- 1 | # ROBOTIS OP2 2 | 3 | This directory contains parts of [ROBOTIS-GIT/ROBOTIS-OP2-Common](https://github.com/ROBOTIS-GIT/ROBOTIS-OP2-Common).
4 | 5 | Specifically, it includes the following directories and files:
6 | (in [ROBOTIS-GIT/ROBOTIS-OP2-Common/robotis_op2_description](https://github.com/ROBOTIS-GIT/ROBOTIS-OP2-Common/tree/master/robotis_op2_description))
7 | 8 | ```bash 9 | meshes 10 | |-- all files 11 | 12 | urdf 13 | |-- all files 14 | ``` 15 | 16 | These are in the directory: 17 | 18 | * All files in original meshes directory are located here in meshes directory.
19 | * All files in original urdf directory are located here in urdf directory.
20 | 21 | All other chages and additions are listed in CHANGELOG.rst.
22 | 23 | ## Original License & Auther 24 | 25 | License: Apache-2.0 license
26 | 27 | Author: 28 | 29 | * ROBOTIS-OP2-Common
30 | * author: Kayman
31 | * maintainer: Pyo
32 | * ROBOTIS-OP2-Common/robotis_op2_description
33 | * author: Changhyun Sung
34 | 35 | ## This Directory License & Maintainer 36 | 37 | License: Apache-2.0 license
38 | 39 | Maintainer: Yusuke-yamasaki-555
40 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_body.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_head.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_head.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_ankle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_ankle.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_foot.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_hip-roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_hip-roll.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_hip-yaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_hip-yaw.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_lower-arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_lower-arm.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_shin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_shin.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_shoulder.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_thigh.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_thigh.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_left_upper-arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_left_upper-arm.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_neck.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_neck.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_ankle.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_ankle.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_foot.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_foot.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_hip-roll.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_hip-roll.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_hip-yaw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_hip-yaw.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_lower-arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_lower-arm.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_shin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_shin.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_shoulder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_shoulder.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_thigh.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_thigh.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/meshes/geo_op_right_upper-arm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_description/models/robotis_op2/meshes/geo_op_right_upper-arm.stl -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/urdf/robotis_op2.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | /robotis_op2 7 | gazebo_ros_control/DefaultRobotHWSim 8 | 9 | 10 | 11 | 12 | 13 | map 14 | base_link 15 | odom 16 | 100.0 17 | 18 | 19 | 20 | 21 | false 22 | 23 | 24 | 25 | 26 | 0.5 27 | 0.5 28 | false 29 | 30 | 31 | 32 | 33 | 0.5 34 | 0.5 35 | false 36 | 37 | 38 | 39 | 0.5 40 | 0.5 41 | false 42 | 43 | 44 | 45 | false 46 | 47 | 48 | 49 | 50 | false 51 | 52 | 53 | 54 | 0.5 55 | 0.5 56 | false 57 | 58 | 59 | 60 | 0.5 61 | 0.5 62 | false 63 | 64 | 65 | 66 | 0.5 67 | 0.5 68 | false 69 | 70 | 71 | 72 | 73 | false 74 | 75 | 76 | 77 | 0.5 78 | 0.5 79 | false 80 | 81 | 82 | 83 | 0.5 84 | 0.5 85 | false 86 | 87 | 88 | 89 | 0.5 90 | 0.5 91 | false 92 | 93 | 94 | 95 | 96 | 0.5 97 | 0.5 98 | false 99 | 100 | 101 | 102 | 0.5 103 | 0.5 104 | false 105 | 106 | 107 | 108 | 0.5 109 | 0.5 110 | false 111 | 112 | 113 | 114 | 0.5 115 | 0.5 116 | false 117 | 118 | 119 | 120 | 0.5 121 | 0.5 122 | false 123 | 124 | 125 | 126 | 9000 127 | 9000 128 | 1000000.0 129 | 10.0 130 | 0.001 131 | 1 132 | false 133 | 134 | 135 | 136 | 137 | 0.5 138 | 0.5 139 | false 140 | 141 | 142 | 143 | 0.5 144 | 0.5 145 | false 146 | 147 | 148 | 149 | 0.5 150 | 0.5 151 | false 152 | 153 | 154 | 155 | 0.5 156 | 0.5 157 | false 158 | 159 | 160 | 161 | 0.5 162 | 0.5 163 | false 164 | 165 | 166 | 167 | 9000 168 | 9000 169 | 1000000.0 170 | 10.0 171 | 0.001 172 | 1 173 | false 174 | 175 | 176 | 177 | 178 | 179 | -0.050000 -0.010000 0.000000 -1.5708 0.000000 -3.141592 180 | 181 | 1.012300 182 | 183 | 320 184 | 200 185 | 186 | 187 | 0.001000 188 | 100.000000 189 | 190 | 191 | 1 192 | 30.000000 193 | 1 194 | 195 | true 196 | 30 197 | robotis_op2/camera 198 | image_raw 199 | camera_info 200 | camera_link 201 | 0.07 202 | 0.0 203 | 0.0 204 | 0.0 205 | 0.0 206 | 0.0 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | body_link 215 | 50 216 | /robotis_op2/imu_service 217 | /robotis_op2/imu 218 | 0 219 | 0 0 0 220 | 0 0 -0.703220730592 221 | 222 | 223 | 224 | 225 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/urdf/robotis_op2.inertia.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/urdf/robotis_op2.structure.arm.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/urdf/robotis_op2.structure.head.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/urdf/robotis_op2.transmissions.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | transmission_interface/SimpleTransmission 7 | 8 | hardware_interface/EffortJointInterface 9 | 10 | 11 | 12 | hardware_interface/EffortJointInterface 13 | 1 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/urdf/robotis_op2.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /robot_description/models/robotis_op2/urdf/robotis_op2.visuals_simple.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | -------------------------------------------------------------------------------- /robot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_description 5 | 0.0.0 6 | TODO: Package description 7 | Yusuke-Yamasaki-555 8 | Apache-2.0 license 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /robot_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_manager) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | # find dependencies 11 | find_package(ament_cmake REQUIRED) 12 | find_package(pluginlib REQUIRED) 13 | find_package(Eigen3 REQUIRED) 14 | find_package(rclcpp REQUIRED) 15 | find_package(sensor_msgs REQUIRED) 16 | find_package(robot_messages REQUIRED) 17 | 18 | add_executable(robot_manager 19 | src/robot_manager.cpp 20 | src/robot_manager_main.cpp 21 | ) 22 | target_include_directories(robot_manager 23 | PUBLIC 24 | $ 25 | $ 26 | $ 27 | # PRIVATE 28 | # include 29 | ) 30 | target_compile_features(robot_manager 31 | PUBLIC 32 | c_std_99 33 | cxx_std_17 34 | ) 35 | ament_target_dependencies(robot_manager 36 | rclcpp 37 | pluginlib 38 | sensor_msgs 39 | robot_messages 40 | ) 41 | 42 | ament_export_include_directories( 43 | include 44 | ${EIGEN3_INCLUDE_DIR} 45 | ) 46 | install( 47 | DIRECTORY include/ 48 | DESTINATION include 49 | ) 50 | 51 | install(TARGETS robot_manager 52 | DESTINATION lib/${PROJECT_NAME} 53 | ) 54 | 55 | if(BUILD_TESTING) 56 | find_package(ament_lint_auto REQUIRED) 57 | # the following line skips the linter which checks for copyrights 58 | # comment the line when a copyright and license is added to all source files 59 | set(ament_cmake_copyright_FOUND TRUE) 60 | # the following line skips cpplint (only works in a git repo) 61 | # comment the line when this package is in a git repo and when 62 | # a copyright and license is added to all source files 63 | set(ament_cmake_cpplint_FOUND TRUE) 64 | ament_lint_auto_find_test_dependencies() 65 | endif() 66 | 67 | ament_package() 68 | -------------------------------------------------------------------------------- /robot_manager/include/robot_manager/control_plugin_bases/PluginBase_ConvertToJointStates.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_MANAGER_PLUGIN_BASE_CONVERT_TO_JOINT_STATES_HPP 2 | #define ROBOT_MANAGER_PLUGIN_BASE_CONVERT_TO_JOINT_STATES_HPP 3 | 4 | #include 5 | #include 6 | #include "robot_manager/control_plugin_bases/PluginBase_WalkingStabilizationController.hpp" 7 | #include "robot_manager/control_plugin_bases/PluginBase_FootStepPlanner.hpp" 8 | 9 | namespace control_plugin_base 10 | { 11 | struct LegJointStatesPattern { // TODO: 実装にもよるが、片脚分だけで十分にできそう。LegStatesは片脚なので、合わせたい。 12 | std::array joint_ang_pat_legR; // 6自由度未満、冗長にも対応したいので、可変長 in 可変長. 13 | std::array joint_ang_pat_legL; // このファイル内で、URDFから脚の関節数を参照して固定長の長さに適用するのも面白そう。 14 | std::array joint_vel_pat_legR; 15 | std::array joint_vel_pat_legL; 16 | }; 17 | 18 | class ConvertToJointStates { 19 | public: 20 | virtual std::unique_ptr convert_to_joint_states( 21 | const std::shared_ptr walking_stabilization_ptr, 22 | const std::shared_ptr foot_step_ptr, 23 | uint32_t walking_step, 24 | uint32_t control_step, 25 | float t 26 | ) = 0; 27 | virtual ~ConvertToJointStates(){} 28 | 29 | protected: 30 | ConvertToJointStates(){} 31 | }; 32 | } 33 | 34 | #endif -------------------------------------------------------------------------------- /robot_manager/include/robot_manager/control_plugin_bases/PluginBase_FootStepPlanner.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_MANAGER_PLUGIN_BASE_FOOT_STEP_PLANNER_HPP 2 | #define ROBOT_MANAGER_PLUGIN_BASE_FOOT_STEP_PLANNER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace control_plugin_base 8 | { 9 | struct FootStep { 10 | std::vector> foot_pos; 11 | double waist_height; 12 | double walking_step_time; 13 | }; 14 | 15 | class FootStepPlanner { 16 | public: 17 | virtual std::unique_ptr foot_step_planner(void) = 0; 18 | virtual ~FootStepPlanner(){} 19 | 20 | protected: 21 | FootStepPlanner(){} 22 | }; 23 | } 24 | 25 | #endif -------------------------------------------------------------------------------- /robot_manager/include/robot_manager/control_plugin_bases/PluginBase_ForwardKinematics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_MANAGER_PLUGIN_BASE_FORWARD_KINEMATICS_HPP 2 | #define ROBOT_MANAGER_PLUGIN_BASE_FORWARD_KINEMTAICS_HPP 3 | 4 | #include "Eigen/Dense" 5 | 6 | // TODO: control_plugin_baseではなく、kinematics_plugin_baseとして、別pkgとして管理するべきかも 7 | namespace control_plugin_base 8 | { 9 | // TODO: 型は全て一箇所にまとめたい。ライブラリとか。 10 | struct LegStates_ToFK { 11 | std::array link_len; 12 | std::array joint_ang; 13 | }; 14 | 15 | class ForwardKinematics { 16 | public: 17 | virtual void forward_kinematics( 18 | std::shared_ptr leg_states_ptr, 19 | Eigen::Vector3d& end_eff_pos_ptr 20 | ) = 0; 21 | virtual void forward_kinematics( // overload. jacobian計算時に用いる、始端の関節から特定の関節（joint_point）までの順運動学計算 22 | std::shared_ptr leg_states_ptr, 23 | int joint_point, 24 | Eigen::Vector3d& end_eff_pos_ptr 25 | ) = 0; 26 | virtual ~ForwardKinematics(){} 27 | 28 | protected: 29 | ForwardKinematics(){} 30 | }; 31 | } 32 | 33 | #endif -------------------------------------------------------------------------------- /robot_manager/include/robot_manager/control_plugin_bases/PluginBase_InverseKinematics.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_MANAGER_PLUGIN_BASE_INVERSE_KINEMATICS_HPP 2 | #define ROBOT_MANAGER_PLUGIN_BASE_INVERSE_KINEMATICS_HPP 3 | 4 | #include "Eigen/Dense" 5 | 6 | namespace control_plugin_base 7 | { 8 | // TODO: 型は全て一箇所にまとめたい。ライブラリとか。 9 | // 他プラグインと型を共有するときも、PluginBase内で各々で定義する必要がある。 10 | struct LegStates_ToIK { 11 | Eigen::Vector3d end_eff_pos; 12 | Eigen::Matrix3d end_eff_rot; 13 | std::array link_len; 14 | }; 15 | 16 | class InverseKinematics { 17 | public: 18 | virtual void inverse_kinematics( 19 | const std::shared_ptr leg_states_ptr, 20 | std::array& joint_ang_ptr 21 | ) = 0; 22 | virtual ~InverseKinematics(){} 23 | 24 | protected: 25 | InverseKinematics(){} 26 | }; 27 | } 28 | 29 | #endif -------------------------------------------------------------------------------- /robot_manager/include/robot_manager/control_plugin_bases/PluginBase_Jacobian.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_MANAGER_PLUGIN_BASE_JACOBIAN_HPP 2 | #define ROBOT_MANAGER_PLUGIN_BASE_JACOBIAN_HPP 3 | 4 | #include "Eigen/Dense" 5 | 6 | namespace control_plugin_base 7 | { 8 | // TODO: 型は全て一箇所にまとめたい。ライブラリとか。 9 | // Kinematicsは様々な箇所で用いられるので、構造体ではなく１つの変数のunique_ptrを返す形でいく。 10 | // 基本的にPluginの引数は、独自の構造体のConstとする。 11 | struct LegStates_ToJac { 12 | std::array link_len; 13 | std::array unit_vec; 14 | std::array joint_ang; 15 | }; 16 | 17 | class Jacobian { 18 | public: 19 | virtual void jacobian( 20 | const std::shared_ptr leg_states_jac_ptr, 21 | Eigen::Matrix& leg_jacobian 22 | ) = 0; 23 | virtual ~Jacobian(){} 24 | 25 | protected: 26 | Jacobian(){} 27 | }; 28 | } 29 | 30 | #endif -------------------------------------------------------------------------------- /robot_manager/include/robot_manager/control_plugin_bases/PluginBase_WalkingPatternGenerator.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_MANAGER_PLUGIN_BASE_WALKING_PATTERN_GENERATOR_HPP 2 | #define ROBOT_MANAGER_PLUGIN_BASE_WALKING_PATTERN_GENERATOR_HPP 3 | 4 | #include 5 | #include 6 | #include "robot_manager/control_plugin_bases/PluginBase_FootStepPlanner.hpp" 7 | 8 | #include "Eigen/Dense" 9 | 10 | namespace control_plugin_base 11 | { 12 | struct WalkingPattern { 13 | std::vector> cc_cog_pos_ref; // 重心位置 14 | std::vector> cc_cog_vel_ref; // 重心速度 15 | // std::vector> cc_foot_sup_pos_ref; // 支持脚足先位置 16 | // std::vector> cc_foot_swing_pos_ref; // 遊脚足先位置 17 | std::vector> wc_foot_land_pos_ref; // 着地位置 18 | }; 19 | 20 | class WalkingPatternGenerator { 21 | public: 22 | virtual std::unique_ptr walking_pattern_generator( 23 | const std::shared_ptr foot_step_ptr // output from foot_step_planner 24 | ) = 0; 25 | virtual ~WalkingPatternGenerator(){} 26 | 27 | protected: 28 | WalkingPatternGenerator(){} 29 | }; 30 | } 31 | 32 | #endif -------------------------------------------------------------------------------- /robot_manager/include/robot_manager/control_plugin_bases/PluginBase_WalkingStabilizationController.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_MANAGER_PLUGIN_BASE_WALKING_STABILIZATION_CONTROLLER_HPP 2 | #define ROBOT_MANAGER_PLUGIN_BASE_WALKING_STABILIZATION_CONTROLLER_HPP 3 | 4 | #include 5 | #include 6 | #include "robot_manager/control_plugin_bases/PluginBase_WalkingPatternGenerator.hpp" 7 | 8 | namespace control_plugin_base 9 | { 10 | struct WalkingStabilization { 11 | std::vector> cog_pos_fix; 12 | std::vector> cog_vel_fix; 13 | std::vector> zmp_pos_fix; 14 | }; 15 | 16 | class WalkingStabilizationController { 17 | public: 18 | virtual std::unique_ptr walking_stabilization_controller( 19 | const std::shared_ptr walking_pattern_ptr 20 | ) = 0; 21 | virtual ~WalkingStabilizationController(){} 22 | 23 | protected: 24 | WalkingStabilizationController(){} 25 | }; 26 | // struct WalkingStabilization { 27 | // std::array cog_pos_fix; 28 | // std::array cog_vel_fix; 29 | // std::array zmp_pos_fix; 30 | // }; 31 | 32 | // class WalkingStabilizationController { 33 | // public: 34 | // virtual std::unique_ptr walking_stabilization_controller( 35 | // const std::shared_ptr walking_pattern_ptr, 36 | // const uint32_t control_step 37 | // ) = 0; 38 | // virtual ~WalkingStabilizationController(){} 39 | 40 | // protected: 41 | // WalkingStabilizationController(){} 42 | // }; 43 | } 44 | 45 | #endif -------------------------------------------------------------------------------- /robot_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_manager 5 | 0.0.0 6 | TODO: Package description 7 | ubu22 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | pluginlib 13 | rclcpp 14 | sensor_msgs 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /robot_manager/src/robot_manager_main.cpp: -------------------------------------------------------------------------------- 1 | #include "robot_manager/robot_manager.hpp" 2 | 3 | int main(int argc, char* argv[]) { 4 | rclcpp::init(argc, argv); 5 | rclcpp::NodeOptions opt; 6 | opt.automatically_declare_parameters_from_overrides(true); 7 | rclcpp::spin(std::make_shared(opt)); 8 | rclcpp::shutdown(); 9 | 10 | return 0; 11 | } 12 | 13 | // int main(int argc, char* argv[]) { 14 | // rclcpp::init(argc, argv); 15 | // auto node = std::make_shared(); 16 | // rclcpp::spin(node->get_node_base_interface()); 17 | // rclcpp::shutdown(); 18 | 19 | // return 0; 20 | // } -------------------------------------------------------------------------------- /robot_messages/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(robot_messages) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rosidl_default_generators REQUIRED) 21 | find_package(sensor_msgs REQUIRED) 22 | 23 | rosidl_generate_interfaces(${PROJECT_NAME} 24 | "msg/FootStepRecord.msg" 25 | "msg/WalkingPatternRecord.msg" 26 | "msg/WalkingStabilizationRecord.msg" 27 | "msg/JointStateRecord.msg" 28 | "msg/Feedback.msg" 29 | DEPENDENCIES sensor_msgs 30 | ) 31 | 32 | if(BUILD_TESTING) 33 | find_package(ament_lint_auto REQUIRED) 34 | # the following line skips the linter which checks for copyrights 35 | # uncomment the line when a copyright and license is not present in all source files 36 | #set(ament_cmake_copyright_FOUND TRUE) 37 | # the following line skips cpplint (only works in a git repo) 38 | # uncomment the line when this package is not in a git repo 39 | #set(ament_cmake_cpplint_FOUND TRUE) 40 | ament_lint_auto_find_test_dependencies() 41 | endif() 42 | 43 | ament_package() 44 | -------------------------------------------------------------------------------- /robot_messages/msg/Feedback.msg: -------------------------------------------------------------------------------- 1 | int32 step_count 2 | float64[6] q_now_leg_r 3 | float64[6] q_now_leg_l 4 | float64[3] accelerometer_now 5 | float64[3] gyro_now -------------------------------------------------------------------------------- /robot_messages/msg/FootStepRecord.msg: -------------------------------------------------------------------------------- 1 | int32 step_count 2 | float64[2] foot_step_pos 3 | float64 waist_height 4 | float64 walking_step_time -------------------------------------------------------------------------------- /robot_messages/msg/JointStateRecord.msg: -------------------------------------------------------------------------------- 1 | int32 step_count 2 | float64[6] joint_ang_leg_l 3 | float64[6] joint_ang_leg_r 4 | float64[6] joint_vel_leg_l 5 | float64[6] joint_vel_leg_r 6 | #sensor_msgs/JointState joint_state -------------------------------------------------------------------------------- /robot_messages/msg/WalkingPatternRecord.msg: -------------------------------------------------------------------------------- 1 | int32 step_count 2 | float64[3] cc_cog_pos_ref # cc: control_cycle, wc: walking_cycle 3 | float64[3] cc_cog_vel_ref 4 | float64[2] wc_foot_land_pos_ref -------------------------------------------------------------------------------- /robot_messages/msg/WalkingStabilizationRecord.msg: -------------------------------------------------------------------------------- 1 | int32 step_count 2 | float64[3] cog_pos_fix 3 | float64[3] cog_vel_fix 4 | float64[2] zmp_pos_fix -------------------------------------------------------------------------------- /robot_messages/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_messages 5 | 0.0.0 6 | Message_file definition 7 | Yusuke Yamasaki 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | rosidl_default_generators 13 | rosidl_default_runtime 14 | rosidl_interface_packages 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | sensor_msgs 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /robot_recorder/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_recorder) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rclcpp REQUIRED) 11 | find_package(robot_messages REQUIRED) 12 | 13 | add_executable(feedback_recorder 14 | src/robot_feedback_recorder.cpp 15 | ) 16 | target_include_directories(feedback_recorder 17 | PUBLIC 18 | $ 19 | $) 20 | target_compile_features(feedback_recorder 21 | PUBLIC 22 | c_std_99 23 | cxx_std_17 24 | ) # Require C99 and C++17 25 | ament_target_dependencies(feedback_recorder 26 | rclcpp 27 | robot_messages 28 | ) 29 | 30 | add_executable(footStep_recorder 31 | src/robot_footStepPlanner_recorder.cpp 32 | ) 33 | target_include_directories(footStep_recorder 34 | PUBLIC 35 | $ 36 | $) 37 | target_compile_features(footStep_recorder 38 | PUBLIC 39 | c_std_99 40 | cxx_std_17 41 | ) # Require C99 and C++17 42 | ament_target_dependencies(footStep_recorder 43 | rclcpp 44 | robot_messages 45 | ) 46 | 47 | add_executable(walkingPattern_recorder 48 | src/robot_walkingPatternGenerator_recorder.cpp 49 | ) 50 | target_include_directories(walkingPattern_recorder 51 | PUBLIC 52 | $ 53 | $) 54 | target_compile_features(walkingPattern_recorder 55 | PUBLIC 56 | c_std_99 57 | cxx_std_17 58 | ) # Require C99 and C++17 59 | ament_target_dependencies(walkingPattern_recorder 60 | rclcpp 61 | robot_messages 62 | ) 63 | 64 | add_executable(walkingStabilization_recorder 65 | src/robot_walkingStabilizationController_recorder.cpp 66 | ) 67 | target_include_directories(walkingStabilization_recorder 68 | PUBLIC 69 | $ 70 | $) 71 | target_compile_features(walkingStabilization_recorder 72 | PUBLIC 73 | c_std_99 74 | cxx_std_17 75 | ) # Require C99 and C++17 76 | ament_target_dependencies(walkingStabilization_recorder 77 | rclcpp 78 | robot_messages 79 | ) 80 | 81 | add_executable(jointState_recorder 82 | src/robot_jointStates_recorder.cpp 83 | ) 84 | target_include_directories(jointState_recorder 85 | PUBLIC 86 | $ 87 | $) 88 | target_compile_features(jointState_recorder 89 | PUBLIC 90 | c_std_99 91 | cxx_std_17 92 | ) # Require C99 and C++17 93 | ament_target_dependencies(jointState_recorder 94 | rclcpp 95 | robot_messages 96 | ) 97 | 98 | install(TARGETS feedback_recorder 99 | DESTINATION lib/${PROJECT_NAME}) 100 | install(TARGETS footStep_recorder 101 | DESTINATION lib/${PROJECT_NAME}) 102 | install(TARGETS walkingPattern_recorder 103 | DESTINATION lib/${PROJECT_NAME}) 104 | install(TARGETS walkingStabilization_recorder 105 | DESTINATION lib/${PROJECT_NAME}) 106 | install(TARGETS jointState_recorder 107 | DESTINATION lib/${PROJECT_NAME}) 108 | 109 | install(DIRECTORY 110 | launch 111 | DESTINATION share/${PROJECT_NAME}/ 112 | ) 113 | 114 | if(BUILD_TESTING) 115 | find_package(ament_lint_auto REQUIRED) 116 | # the following line skips the linter which checks for copyrights 117 | # comment the line when a copyright and license is added to all source files 118 | set(ament_cmake_copyright_FOUND TRUE) 119 | # the following line skips cpplint (only works in a git repo) 120 | # comment the line when this package is in a git repo and when 121 | # a copyright and license is added to all source files 122 | set(ament_cmake_cpplint_FOUND TRUE) 123 | ament_lint_auto_find_test_dependencies() 124 | endif() 125 | 126 | ament_package() 127 | -------------------------------------------------------------------------------- /robot_recorder/launch/robot_recorder.launch.py: -------------------------------------------------------------------------------- 1 | # Logger launch file 2 | 3 | # TODO: rosbag2の仕様確認と、実行 4 | # 開発ソフトウェアに関わるTopicをrecord。他はいらん。recordするTopic名をconfig fileから読み取るか？User側で増やしたくなるかもしれんし、Configいじれば良いんだな！という思考の流れを持つだろうから。 5 | 6 | import os 7 | from time import sleep 8 | import datetime 9 | import yaml 10 | 11 | from launch import LaunchDescription 12 | from ament_index_python.packages import get_package_share_directory 13 | from launch.actions import DeclareLaunchArgument 14 | from launch.substitutions import LaunchConfiguration 15 | from launch_ros.actions import Node 16 | 17 | 18 | def generate_launch_description(): 19 | launch_description = LaunchDescription() 20 | 21 | debug_mode_yaml_path = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_debug_mode.yaml") 22 | with open(debug_mode_yaml_path, "r") as f: 23 | debug_mode_yaml = yaml.safe_load(f)["/**"]["ros__parameters"]["setting_debug_mode"] 24 | 25 | if debug_mode_yaml["using_feedback_recorder"] + \ 26 | debug_mode_yaml["using_footStep_recorder"] + \ 27 | debug_mode_yaml["using_walkingPattern_recorder"] + \ 28 | debug_mode_yaml["using_walkingStabilization_recorder"] + \ 29 | debug_mode_yaml["using_jointState_recorder"] + \ 30 | debug_mode_yaml["using_rosbag2"] > 0: # 何もRecordしないなら，Dirを作らない処理．TODO: これで良いのか？User側を必要最低限にしたくてこうなっている． 31 | launch_datetime = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") 32 | 33 | #record_dir_path = "./src/ROS2_Walking_Pattern_Generator/Record/" + launch_datetime 34 | record_dir_path = "./src/Records/" + launch_datetime # DEBUG 35 | 36 | try: 37 | os.mkdir(record_dir_path) 38 | except FileExistsError: 39 | pass 40 | except: 41 | print("Could not make record dir.") 42 | quit() 43 | 44 | record_dir_path = record_dir_path + "/" 45 | 46 | recorder_params = { 47 | "record_dir_path": record_dir_path, 48 | "launch_datetime": launch_datetime 49 | } 50 | 51 | if debug_mode_yaml["using_feedback_recorder"] == True: 52 | feedback_recorder_node = Node( 53 | package = "robot_recorder", 54 | executable = "feedback_recorder", 55 | parameters = [recorder_params], 56 | ) 57 | launch_description.add_action(feedback_recorder_node) 58 | 59 | if debug_mode_yaml["using_footStep_recorder"] == True: 60 | footStep_recorder_node = Node( 61 | package = "robot_recorder", 62 | executable = "footStep_recorder", 63 | parameters = [recorder_params], 64 | ) 65 | launch_description.add_action(footStep_recorder_node) 66 | 67 | if debug_mode_yaml["using_walkingPattern_recorder"] == True: 68 | walkingPattern_recorder_node = Node( 69 | package = "robot_recorder", 70 | executable = "walkingPattern_recorder", 71 | parameters = [recorder_params], 72 | ) 73 | launch_description.add_action(walkingPattern_recorder_node) 74 | 75 | if debug_mode_yaml["using_walkingStabilization_recorder"] == True: 76 | walkingStabilization_recorder_node = Node( 77 | package = "robot_recorder", 78 | executable = "walkingStabilization_recorder", 79 | parameters = [recorder_params], 80 | ) 81 | launch_description.add_action(walkingStabilization_recorder_node) 82 | 83 | if debug_mode_yaml["using_jointState_recorder"] == True: 84 | jointState_recorder_node = Node( 85 | package = "robot_recorder", 86 | executable = "jointState_recorder", 87 | parameters = [recorder_params], 88 | ) 89 | launch_description.add_action(jointState_recorder_node) 90 | 91 | if debug_mode_yaml["using_rosbag2"] == True: 92 | None 93 | 94 | # sleep(4) 95 | return launch_description -------------------------------------------------------------------------------- /robot_recorder/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_recorder 5 | 0.0.0 6 | robot_recorder package 7 | Yusuke Yamasaki 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | msgs_package 14 | 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /robot_recorder/src/robot_feedback_recorder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // #include 3 | // #include 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | // #include 7 | #include "robot_messages/msg/feedback.hpp" 8 | 9 | #include 10 | 11 | namespace Recorder { 12 | // static const rmw_qos_profile_t custom_qos_profile = 13 | // { 14 | // RMW_QOS_POLICY_HISTORY_KEEP_LAST, // History: keep_last or keep_all 15 | // 1, // History(keep_last) Depth 16 | // RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, // Reliability: best_effort or reliable 17 | // RMW_QOS_POLICY_DURABILITY_VOLATILE, // Durability: transient_local or volatile 18 | // RMW_QOS_DEADLINE_DEFAULT, // Deadline: default or number 19 | // RMW_QOS_LIFESPAN_DEFAULT, // Lifespan: default or number 20 | // RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, // Liveliness: automatic or manual_by_topic 21 | // RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, // Liveliness_LeaseDuration: default or number 22 | // false // avoid_ros_namespace_conventions 23 | // }; 24 | 25 | class RobotFeedbackRecorder : public rclcpp::Node { 26 | public: 27 | RobotFeedbackRecorder( 28 | const rclcpp::NodeOptions &options = rclcpp::NodeOptions() 29 | ) : Node("RobotFeedbackRecorder", options) { 30 | // auto custom_QoS = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos_profile)); 31 | 32 | std::string record_dir_path = get_parameter("record_dir_path").as_string(); 33 | std::string launch_datetime = get_parameter("launch_datetime").as_string(); 34 | 35 | // file_feedback_acc_path = record_dir_path + launch_datetime + "__feedback-acceleration.dat"; 36 | // file_feedback_gyro_path = record_dir_path + launch_datetime + "__feedback-gyro.dat"; 37 | file_feedback_path = record_dir_path + launch_datetime + "__feedback.dat"; 38 | 39 | // file_feedback_acc.open(file_feedback_acc_path, std::ios::out); 40 | // file_feedback_gyro.open(file_feedback_gyro_path, std::ios::out); 41 | file_feedback.open(file_feedback_path, std::ios::out); 42 | 43 | file_feedback << "# record data: step_count | acceleration (x y z) | gyro (x y z)" << std::endl; 44 | 45 | using namespace std::placeholders; 46 | sub_feedback_ = this->create_subscription("feedback", 10, std::bind(&RobotFeedbackRecorder::Feedback_Callback, this, _1)); 47 | } 48 | 49 | ~RobotFeedbackRecorder() { 50 | // file_feedback_acc.close(); 51 | // file_feedback_gyro.close(); 52 | file_feedback.close(); 53 | } 54 | 55 | private: 56 | void Feedback_Callback(const robot_messages::msg::Feedback::SharedPtr callback_data) { 57 | /* Accelerometer & Gyro. Darwin-op.proto 仕様 58 | source: https://github.com/cyberbotics/webots/blob/master/projects/robots/robotis/darwin-op/protos/Darwin-op.proto 59 | 60 | Accelerometer { 61 | translation -0.01 0 -0.068 62 | rotation 0 0 1.0 -1.5708 # z軸基準に座標を-90°回転 (x -> -y, y -> x, z -> z) 63 | name "Accelerometer" 64 | lookupTable [ 65 | -39.24 0 0 39.24 1024 0 66 | ] 67 | } 68 | Gyro { 69 | translation 0.01 0 -0.068 70 | rotation 0 0 1.0 -3.1416 # z軸基準に座標を-180°回転 (x -> -x, y -> -y, z -> z) 71 | name "Gyro" 72 | lookupTable [ 73 | -27.925 0 0 27.925 1024 0 74 | ] 75 | } 76 | 77 | Offset (センサデータ出力値より推測) 78 | Acce 79 | x: 512 80 | y: 512 81 | z: 640 82 | Gyro 83 | x: 512 84 | y: 512 85 | z: 512 86 | 87 | reference: 88 | acce: https://github.com/cyberbotics/webots/blob/master/docs/reference/accelerometer.md 89 | gyro: https://github.com/cyberbotics/webots/blob/master/docs/reference/gyro.md 90 | */ 91 | // データ落ちに対処 92 | // 落ちたデータの箇所は、今の最新と同値で埋める。 93 | // CHECKME: データ落ちの箇所は記録しておいたほうが良い？ 94 | diff = callback_data->step_count - counter_old_; 95 | if(1 != diff) { 96 | // RCLCPP_WARN(this->get_Recorder(), "Feedback Data Loss!!: loss count [ %d ], loss data step number [ %d ]", loss_count_, counter_old_+1); 97 | for(int loss_step = 1; loss_step < diff; loss_step++) { 98 | // TODO: datファイルへの書き込みは最後に一括して行いたい。step_count data　って感じで。 99 | feedback_step_count.push_back(-999); // loss dataなので、エラー値。いや、単にカウント値を入れるのとエラー値は別にしたほうが良いか？Plotする時を考えると。 100 | feedback_acceleration.push_back(feedback_acceleration.back()); 101 | feedback_gyro.push_back(feedback_gyro.back()); 102 | // file_feedback_acc << counter_old_+loss_step << " "; 103 | // file_feedback_gyro << counter_old_+loss_step << " "; 104 | file_feedback << -999 << " "; 105 | 106 | for(double acce : feedback_acceleration.back()) { 107 | file_feedback << acce << " "; 108 | } 109 | for(double gyro : feedback_gyro.back()) { 110 | file_feedback << gyro << " "; 111 | } 112 | file_feedback << std::endl; 113 | } 114 | } 115 | // record 116 | feedback_step_count.push_back(callback_data->step_count); 117 | feedback_acceleration.push_back(callback_data->accelerometer_now); 118 | feedback_gyro.push_back(callback_data->gyro_now); 119 | // file_feedback_acc << callback_data->step_count << " "; 120 | // file_feedback_gyro << callback_data->step_count << " "; 121 | file_feedback << callback_data->step_count << " "; 122 | 123 | for(double acce : callback_data->accelerometer_now) { 124 | file_feedback << acce << " "; 125 | } 126 | for(double gyro : callback_data->gyro_now) { 127 | file_feedback << gyro << " "; 128 | } 129 | file_feedback << std::endl; 130 | 131 | counter_old_ = callback_data->step_count; 132 | 133 | } 134 | 135 | rclcpp::Subscription::SharedPtr sub_feedback_; 136 | 137 | // std::ofstream file_feedback_acc; 138 | // std::ofstream file_feedback_gyro; 139 | // std::string file_feedback_acc_path; 140 | // std::string file_feedback_gyro_path; 141 | std::ofstream file_feedback; 142 | std::string file_feedback_path; 143 | 144 | int loss_count_ = 0; 145 | int counter_old_ = 0; 146 | int diff = 0; 147 | 148 | std::vector> feedback_acceleration; 149 | std::vector> feedback_gyro; 150 | std::vector feedback_step_count; 151 | }; 152 | } 153 | 154 | 155 | int main(int argc, char *argv[]) { 156 | rclcpp::init(argc, argv); 157 | 158 | rclcpp::NodeOptions node_option; 159 | node_option.allow_undeclared_parameters(true); 160 | node_option.automatically_declare_parameters_from_overrides(true); 161 | 162 | rclcpp::spin(std::make_shared(node_option)); 163 | rclcpp::shutdown(); 164 | 165 | return 0; 166 | } -------------------------------------------------------------------------------- /robot_recorder/src/robot_footStepPlanner_recorder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // #include 3 | // #include 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | // #include 7 | #include "robot_messages/msg/foot_step_record.hpp" 8 | 9 | #include 10 | 11 | namespace Recorder { 12 | // static const rmw_qos_profile_t custom_qos_profile = 13 | // { 14 | // RMW_QOS_POLICY_HISTORY_KEEP_LAST, // History: keep_last or keep_all 15 | // 1, // History(keep_last) Depth 16 | // RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, // Reliability: best_effort or reliable 17 | // RMW_QOS_POLICY_DURABILITY_VOLATILE, // Durability: transient_local or volatile 18 | // RMW_QOS_DEADLINE_DEFAULT, // Deadline: default or number 19 | // RMW_QOS_LIFESPAN_DEFAULT, // Lifespan: default or number 20 | // RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, // Liveliness: automatic or manual_by_topic 21 | // RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, // Liveliness_LeaseDuration: default or number 22 | // false // avoid_ros_namespace_conventions 23 | // }; 24 | 25 | class RobotFootStepRecorder : public rclcpp::Node { 26 | public: 27 | RobotFootStepRecorder( 28 | const rclcpp::NodeOptions &options = rclcpp::NodeOptions() 29 | ) : Node("RobotFootStepRecorder", options) { 30 | // auto custom_QoS = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos_profile)); 31 | 32 | // ファイルの作成。ファイル名先頭に日付時間を付与 33 | // auto time_now = std::chrono::system_clock::now(); 34 | // std::time_t datetime = std::chrono::system_clock::to_time_t(time_now); 35 | // std::string datetime_str = std::ctime(&datetime); 36 | 37 | std::string record_dir_path = get_parameter("record_dir_path").as_string(); 38 | std::string launch_datetime = get_parameter("launch_datetime").as_string(); 39 | 40 | file_footStep_path = record_dir_path + launch_datetime + "__foot_step.dat"; 41 | 42 | file_footStep.open(file_footStep_path, std::ios::out); 43 | 44 | file_footStep << "# record data: step_count | foot_step (x y)" << std::endl; 45 | 46 | using namespace std::placeholders; 47 | sub_footStep_ = this->create_subscription("foot_step", 10, std::bind(&RobotFootStepRecorder::FootStep_Callback, this, _1)); 48 | } 49 | 50 | ~RobotFootStepRecorder() { 51 | file_footStep.close(); 52 | } 53 | 54 | private: 55 | void FootStep_Callback(const robot_messages::msg::FootStepRecord::SharedPtr callback_data) { 56 | 57 | // データ落ちに対処 58 | // 落ちたデータの箇所は、今の最新と同値で埋める。 59 | // CHECKME: データ落ちの箇所は記録しておいたほうが良い？ 60 | diff = callback_data->step_count - counter_old_; 61 | if(1 != diff) { 62 | for(int loss_step = 1; loss_step < diff; loss_step++) { 63 | // TODO: datファイルへの書き込みは最後に一括して行いたい。step_count data　って感じで。 64 | footStep_step_count.push_back(-999); // loss dataなので、エラー値。いや、単にカウント値を入れるのとエラー値は別にしたほうが良いか？Plotする時を考えると。 65 | footStep.push_back(footStep.back()); 66 | file_footStep << counter_old_+loss_step << " "; 67 | 68 | for(double foot : footStep.back()) { 69 | file_footStep << foot << " "; 70 | } 71 | file_footStep << std::endl; 72 | } 73 | } 74 | // record 75 | footStep_step_count.push_back(callback_data->step_count); 76 | footStep.push_back(callback_data->foot_step_pos); 77 | file_footStep << callback_data->step_count << " "; 78 | 79 | for(double foot : callback_data->foot_step_pos) { 80 | file_footStep << foot << " "; 81 | } 82 | file_footStep << std::endl; 83 | 84 | counter_old_ = callback_data->step_count; 85 | 86 | } 87 | 88 | rclcpp::Subscription::SharedPtr sub_footStep_; 89 | 90 | // TODO: ファイル名を生成する。../data/内に記録するようにする（../表記が行けるか？無理ならこのフルパスをゲットして記録するか？） 91 | std::ofstream file_footStep; 92 | std::string file_footStep_path; 93 | 94 | int loss_count_ = 0; 95 | int counter_old_ = 0; 96 | int diff = 0; 97 | 98 | std::vector> footStep; 99 | std::vector footStep_step_count; 100 | }; 101 | } 102 | 103 | int main(int argc, char *argv[]) { 104 | rclcpp::init(argc, argv); 105 | 106 | rclcpp::NodeOptions node_option; 107 | node_option.allow_undeclared_parameters(true); 108 | node_option.automatically_declare_parameters_from_overrides(true); 109 | 110 | rclcpp::spin(std::make_shared(node_option)); 111 | rclcpp::shutdown(); 112 | 113 | return 0; 114 | } -------------------------------------------------------------------------------- /robot_recorder/src/robot_walkingPatternGenerator_recorder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // #include 3 | // #include 4 | 5 | #include "rclcpp/rclcpp.hpp" 6 | // #include 7 | #include "robot_messages/msg/walking_pattern_record.hpp" 8 | 9 | #include 10 | 11 | namespace Recorder { 12 | // static const rmw_qos_profile_t custom_qos_profile = 13 | // { 14 | // RMW_QOS_POLICY_HISTORY_KEEP_LAST, // History: keep_last or keep_all 15 | // 1, // History(keep_last) Depth 16 | // RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, // Reliability: best_effort or reliable 17 | // RMW_QOS_POLICY_DURABILITY_VOLATILE, // Durability: transient_local or volatile 18 | // RMW_QOS_DEADLINE_DEFAULT, // Deadline: default or number 19 | // RMW_QOS_LIFESPAN_DEFAULT, // Lifespan: default or number 20 | // RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, // Liveliness: automatic or manual_by_topic 21 | // RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, // Liveliness_LeaseDuration: default or number 22 | // false // avoid_ros_namespace_conventions 23 | // }; 24 | 25 | class RobotWalkingPatternRecorder : public rclcpp::Node { 26 | public: 27 | RobotWalkingPatternRecorder( 28 | const rclcpp::NodeOptions &options = rclcpp::NodeOptions() 29 | ) : Node("RobotWalkingPatternRecorder", options) { 30 | // auto custom_QoS = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos_profile)); 31 | 32 | // ファイルの作成。ファイル名先頭に日付時間を付与 33 | // auto time_now = std::chrono::system_clock::now(); 34 | // std::time_t datetime = std::chrono::system_clock::to_time_t(time_now); 35 | // std::string datetime_str = std::ctime(&datetime); 36 | 37 | std::string record_dir_path = get_parameter("record_dir_path").as_string(); 38 | std::string launch_datetime = get_parameter("launch_datetime").as_string(); 39 | 40 | // file_walkingPattern_pos_path = record_dir_path + launch_datetime + "__walking_pattern-position.dat"; 41 | // file_walkingPattern_vel_path = record_dir_path + launch_datetime + "__walking_pattern-velocity.dat"; 42 | file_walkingPattern_path = record_dir_path + launch_datetime + "__walking_pattern.dat"; 43 | 44 | // file_walkingPattern_pos.open(file_walkingPattern_pos_path, std::ios::out); 45 | // file_walkingPattern_vel.open(file_walkingPattern_pos_path, std::ios::out); 46 | file_walkingPattern.open(file_walkingPattern_path, std::ios::out); 47 | 48 | file_walkingPattern << "# record data: step_count | walking_pattern_position (x y z) | walking_pattern_velocity (x y z) | walking_pattern_zmp (x y)" << std::endl; 49 | 50 | using namespace std::placeholders; 51 | sub_walkingPattern_ = this->create_subscription("walking_pattern", 10, std::bind(&RobotWalkingPatternRecorder::WalkingPattern_Callback, this, _1)); 52 | } 53 | 54 | ~RobotWalkingPatternRecorder() { 55 | // file_walkingPattern_pos.close(); 56 | // file_walkingPattern_vel.close(); 57 | file_walkingPattern.close(); 58 | } 59 | 60 | private: 61 | void WalkingPattern_Callback(const robot_messages::msg::WalkingPatternRecord::SharedPtr callback_data) { 62 | 63 | // データ落ちに対処 64 | // 落ちたデータの箇所は、今の最新と同値で埋める。 65 | // CHECKME: データ落ちの箇所は記録しておいたほうが良い？ 66 | diff = callback_data->step_count - counter_old_; 67 | if(1 != diff) { 68 | for(int loss_step = 1; loss_step < diff; loss_step++) { 69 | // TODO: datファイルへの書き込みは最後に一括して行いたい。step_count data　って感じで。 70 | walkingPattern_step_count.push_back(-999); // loss dataなので、エラー値。いや、単にカウント値を入れるのとエラー値は別にしたほうが良いか？Plotする時を考えると。 71 | walkingPattern_pos.push_back(walkingPattern_pos.back()); 72 | walkingPattern_vel.push_back(walkingPattern_vel.back()); 73 | walkingPattern_zmp_pos.push_back(walkingPattern_zmp_pos.back()); 74 | 75 | // file_walkingPattern_pos << counter_old_+loss_step << " "; 76 | // file_walkingPattern_vel << counter_old_+loss_step << " "; 77 | file_walkingPattern << counter_old_+loss_step << " "; 78 | for(double pos : walkingPattern_pos.back()) { 79 | file_walkingPattern << pos << " "; 80 | } 81 | for(double vel : walkingPattern_vel.back()) { 82 | file_walkingPattern << vel << " "; 83 | } 84 | for(double zmp : walkingPattern_zmp_pos.back()) { 85 | file_walkingPattern << zmp << " "; 86 | } 87 | file_walkingPattern << std::endl; 88 | } 89 | } 90 | // record 91 | walkingPattern_step_count.push_back(callback_data->step_count); 92 | walkingPattern_pos.push_back(callback_data->cc_cog_pos_ref); 93 | walkingPattern_vel.push_back(callback_data->cc_cog_vel_ref); 94 | walkingPattern_zmp_pos.push_back(callback_data->wc_foot_land_pos_ref); 95 | 96 | // file_walkingPattern_pos << callback_data->step_count << " "; 97 | // file_walkingPattern_vel << callback_data->step_count << " "; 98 | file_walkingPattern << callback_data->step_count << " "; 99 | for(double pos : callback_data->cc_cog_pos_ref) { 100 | file_walkingPattern << pos << " "; 101 | } 102 | for(double vel : callback_data->cc_cog_vel_ref) { 103 | file_walkingPattern << vel << " "; 104 | } 105 | for(double zmp : callback_data->wc_foot_land_pos_ref) { 106 | file_walkingPattern << zmp << " "; 107 | } 108 | file_walkingPattern << std::endl; 109 | 110 | counter_old_ = callback_data->step_count; 111 | 112 | } 113 | 114 | rclcpp::Subscription::SharedPtr sub_walkingPattern_; 115 | 116 | // std::ofstream file_walkingPattern_pos; 117 | // std::ofstream file_walkingPattern_vel; 118 | // std::string file_walkingPattern_pos_path; 119 | // std::string file_walkingPattern_vel_path; 120 | std::ofstream file_walkingPattern; 121 | std::string file_walkingPattern_path; 122 | 123 | int loss_count_ = 0; 124 | int counter_old_ = 0; 125 | int diff = 0; 126 | 127 | std::vector> walkingPattern_pos; 128 | std::vector> walkingPattern_vel; 129 | std::vector> walkingPattern_zmp_pos; 130 | std::vector walkingPattern_step_count; 131 | }; 132 | } 133 | 134 | int main(int argc, char *argv[]) { 135 | rclcpp::init(argc, argv); 136 | 137 | rclcpp::NodeOptions node_option; 138 | node_option.allow_undeclared_parameters(true); 139 | node_option.automatically_declare_parameters_from_overrides(true); 140 | 141 | rclcpp::spin(std::make_shared(node_option)); 142 | rclcpp::shutdown(); 143 | 144 | return 0; 145 | } -------------------------------------------------------------------------------- /robot_tools/README.md: -------------------------------------------------------------------------------- 1 | > This README was created by translating the Japanese text using GitHub Copilot. 2 | --- 3 | # robot_tools 4 | 5 | (Writing in progress) 6 | 7 | ## model_parameter_generator.py 8 | 9 | A Python script to extract parameters for using a new robot with this software. 10 | 11 | ### Usage 12 | 13 | #### 1. Preparation 14 | 15 | - Place the directory and model files in the following structure under `robot_description/models/`: 16 | 17 | ``` 18 | models/ 19 | |-- / 20 | |-- meshes/ 21 | |-- 22 | |-- urdf/ 23 | |-- 24 | ``` 25 | 26 | - Modify the code around lines 27 and 28 in `robot_tools/robot_tools/model_parameter_generator.py` as follows: 27 | 28 | ```diff 29 | - robot_name = "rdc_robot_v1" 30 | - urdf_name = "simple_model.urdf" 31 | + robot_name = "" 32 | + urdf_name = ".urdf" 33 | ``` 34 | 35 | #### 2. Execution 36 | 37 | Run the Python script by entering the following commands: 38 | 39 | ```bash 40 | cd robot_tools/robot_tools/ 41 | python3 model_parameter_generator.py 42 | ``` 43 | 44 | During execution, you will be prompted to name each link. Follow the displayed instructions and provide input as needed. 45 | 46 | Below is an example of the output (the parts marked with ">" are where you input values): 47 | 48 | ``` 49 | [INFO] List of tree in URDF file. (with fixed joints) 50 | tree 0 : ['CPG'] 51 | tree 1 : ['luggage'] 52 | tree 2 : ['r_waist_roll', 'r_waist_pitch', 'r_knee_pitch', 'r_ankle'] 53 | tree 3 : ['l_waist_roll', 'l_waist_pitch', 'l_knee_pitch', 'l_ankle'] 54 | 55 | [INFO] Please input free joint_tree_name. 56 | [INFO] If nothing is inputted, joint_tree_name is 'names_group_'. 57 | [INFO] tree 0 name? > 58 | [INFO] tree 1 name? > 59 | [INFO] tree 2 name? > 60 | [INFO] tree 3 name? > 61 | 62 | name's number | joint_group_name | joint_group_value 63 | --------------|------------------|------------------ 64 | 0 | names_group_0 | ['CPG'] 65 | 1 | names_group_1 | ['luggage'] 66 | 2 | names_group_2 | ['r_waist_roll', 'r_waist_pitch', 'r_knee_pitch', 'r_ankle'] 67 | 3 | names_group_3 | ['l_waist_roll', 'l_waist_pitch', 'l_knee_pitch', 'l_ankle'] 68 | 69 | [INFO] What's left_leg's name? (please input name's number) > 3 70 | [INFO] What's right_leg's name? (please input name's number) > 2 71 | [WARNING][CPG] Axis_tag is not defined. 72 | [WARNING][luggage] Axis_tag is not defined. 73 | [WARNING][r_ankle] Axis_tag is not defined. 74 | [WARNING][l_ankle] Axis_tag is not defined. 75 | ``` 76 | 77 | Once the script is complete, a directory with the robot model's name will be generated under robot_description/config/. Inside, three YAML files will be created, each containing parameters. 78 | 79 | (Writing in progress) 80 | 81 | -------------------------------------------------------------------------------- /robot_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_tools 5 | 0.0.0 6 | TODO: Package description 7 | ubu22 8 | Apache-2.0 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /robot_tools/resource/robot_tools: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_tools/resource/robot_tools -------------------------------------------------------------------------------- /robot_tools/robot_tools/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/robot_tools/robot_tools/__init__.py -------------------------------------------------------------------------------- /robot_tools/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/robot_tools 3 | [install] 4 | install_scripts=$base/lib/robot_tools 5 | -------------------------------------------------------------------------------- /robot_tools/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = 'robot_tools' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=find_packages(exclude=['test']), 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools', 'xml', 'yaml'], 15 | zip_safe=True, 16 | maintainer='Yusuke-Yamasaki-555', 17 | maintainer_email='yuu5364yuu@gmail.com', 18 | description='Tools', 19 | license='Apache-2.0', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | ], 24 | }, 25 | ) 26 | -------------------------------------------------------------------------------- /robot_tools/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /robot_tools/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /robot_tools/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /robot_visualizer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(robot_visualizer) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(rviz2 REQUIRED) 11 | find_package(xacro REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | 14 | install(DIRECTORY 15 | rviz 16 | launch 17 | DESTINATION share/${PROJECT_NAME}/ 18 | ) 19 | 20 | if(BUILD_TESTING) 21 | find_package(ament_lint_auto REQUIRED) 22 | # the following line skips the linter which checks for copyrights 23 | # comment the line when a copyright and license is added to all source files 24 | set(ament_cmake_copyright_FOUND TRUE) 25 | # the following line skips cpplint (only works in a git repo) 26 | # comment the line when this package is in a git repo and when 27 | # a copyright and license is added to all source files 28 | set(ament_cmake_cpplint_FOUND TRUE) 29 | ament_lint_auto_find_test_dependencies() 30 | endif() 31 | 32 | ament_package() 33 | -------------------------------------------------------------------------------- /robot_visualizer/launch/robot_visualizer.launch.py: -------------------------------------------------------------------------------- 1 | # Visualizer launch file 2 | import os 3 | from time import sleep 4 | import yaml 5 | 6 | import launch 7 | from launch.actions import TimerAction 8 | from launch_ros.actions import Node 9 | from ament_index_python.packages import get_package_share_directory 10 | from launch.launch_description_sources import PythonLaunchDescriptionSource 11 | 12 | def generate_launch_description(): 13 | launch_description = launch.LaunchDescription() 14 | 15 | debug_mode_yaml_path = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_debug_mode.yaml") 16 | with open(debug_mode_yaml_path, "r") as f: 17 | debug_mode_yaml = yaml.safe_load(f)["/**"]["ros__parameters"]["setting_debug_mode"] 18 | 19 | # Rviz2のlaunch 20 | if debug_mode_yaml["using_rviz"] == True: 21 | rviz_launch = launch.actions.IncludeLaunchDescription( 22 | PythonLaunchDescriptionSource([ 23 | os.path.join(get_package_share_directory("robot_visualizer"), "launch"), 24 | "/rviz.launch.py" 25 | ]) 26 | ) 27 | launch_description.add_action(rviz_launch) 28 | 29 | # Loggerとの違いは、リアルタイムに表示をするだけ　｜　記録を取る 30 | 31 | # TODO: リアルタイムのモニタリングNodeとかの実行（作れれば。rqt、imgui,etc?） 32 | # ros関連を詳しく出してくれるtopを実行するのも良さげ？ 33 | # これ：https://github.com/iwatake2222/rotop 34 | # TODO: 動作確認と、pythonソースから実行できるか確認（コマンドラインでも、pythonでも） 35 | 36 | # RQT Plotのlaunch 37 | if debug_mode_yaml["using_rqt"] == True: 38 | rqt_plot_launch = Node( 39 | package = "rqt_plot", 40 | executable = "rqt_plot", 41 | output = "screen", 42 | arguments = [ 43 | "/joint_states/position[9]", # DEBUG: ここをDEBUG用のPublish Topicを指定すれば良い．んで，これをparameterから指定するようにすれば良い． 44 | ] 45 | ) 46 | rqt_plot_launch_delay = TimerAction( # TODO: 時間でディレイするのはスマートな方法ではないと思われ．LifeCycleを突っ込んだほうが良い？ 47 | period = 1.0, 48 | actions = [rqt_plot_launch] 49 | ) 50 | launch_description.add_action(rqt_plot_launch_delay) 51 | 52 | # sleep(4) 53 | return launch_description -------------------------------------------------------------------------------- /robot_visualizer/launch/rviz.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import yaml 3 | 4 | from launch import LaunchDescription 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch.substitutions import Command, FindExecutable, PathJoinSubstitution 7 | from launch_ros.actions import Node 8 | from launch_ros.substitutions import FindPackageShare 9 | from launch_ros.parameter_descriptions import ParameterValue 10 | 11 | 12 | def generate_launch_description(): 13 | launch_description = LaunchDescription() 14 | 15 | debug_mode_yaml_path = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_debug_mode.yaml") 16 | with open(debug_mode_yaml_path, "r") as f: 17 | debug_mode_yaml = yaml.safe_load(f)["/**"]["ros__parameters"]["setting_debug_mode"] 18 | 19 | description_yaml_path = os.path.join(get_package_share_directory("robot_bringup"), "config", "param_robot_description.yaml") 20 | with open(description_yaml_path, "r") as f: 21 | description_yaml = yaml.safe_load(f)["/**"]["ros__parameters"]["robot_description"] 22 | 23 | # Get URDF via xacro 24 | robot_model = ParameterValue( 25 | Command([ 26 | PathJoinSubstitution([FindExecutable(name="xacro")]), 27 | " ", 28 | PathJoinSubstitution( 29 | [FindPackageShare("robot_description"), "models/" + description_yaml["robot_name"] + "/" + description_yaml["xacro_file_path"]] 30 | ), 31 | ]), value_type=str 32 | ) 33 | robot_model_param = {"robot_description": robot_model} 34 | 35 | rviz_config_file = PathJoinSubstitution([ 36 | FindPackageShare("robot_visualizer"), "rviz", debug_mode_yaml["rviz_file"] 37 | ]) 38 | 39 | robot_state_pub_node = Node( 40 | package="robot_state_publisher", 41 | executable="robot_state_publisher", 42 | parameters=[robot_model_param] 43 | ) 44 | launch_description.add_action(robot_state_pub_node) 45 | 46 | if debug_mode_yaml["using_rviz"] == True: 47 | rviz_node = Node( 48 | package="rviz2", 49 | executable="rviz2", 50 | arguments=["-d", rviz_config_file] 51 | ) 52 | launch_description.add_action(rviz_node) 53 | 54 | 55 | return launch_description -------------------------------------------------------------------------------- /robot_visualizer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | robot_visualizer 5 | 0.0.0 6 | TODO: Package description 7 | ubu22 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | rviz2 13 | xacro 14 | rclcpp 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /walking_pattern_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(walking_pattern_generator) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | find_package(ament_cmake REQUIRED) 19 | find_package(ament_cmake_ros REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | find_package(robot_manager REQUIRED) 22 | find_package(pluginlib REQUIRED) 23 | # find_package(msgs_package REQUIRED) 24 | # find_package(kinematics REQUIRED) 25 | # find_package(sensor_msgs REQUIRED) 26 | find_package(Eigen3 REQUIRED) 27 | 28 | # add_executable(walking_pattern_generator 29 | # src/WalkingPatternGenerator.cpp 30 | # src/WalkingPatternGenerator_main.cpp 31 | # ) 32 | # ament_target_dependencies(walking_pattern_generator 33 | # rclcpp 34 | # msgs_package 35 | # kinematics 36 | # sensor_msgs 37 | # ) 38 | # target_include_directories(walking_pattern_generator 39 | # PUBLIC 40 | # $ 41 | # $ 42 | # $ 43 | # PRIVATE 44 | # include 45 | # ) 46 | 47 | pluginlib_export_plugin_description_file(robot_manager plugins.xml) 48 | 49 | add_library(walking_pattern_generator 50 | src/LinearInvertedPendulumModel.cpp 51 | ) 52 | ament_target_dependencies(walking_pattern_generator 53 | robot_manager 54 | rclcpp 55 | pluginlib 56 | ) 57 | target_include_directories(walking_pattern_generator 58 | PUBLIC 59 | $ 60 | $ 61 | $ 62 | PRIVATE 63 | include 64 | ) 65 | 66 | target_compile_definitions(walking_pattern_generator 67 | PRIVATE 68 | "WALKING_PATTERN_GENERATOR_BUILDING_LIBRARY" 69 | ) 70 | 71 | install( 72 | DIRECTORY include/ 73 | DESTINATION include 74 | ) 75 | install(TARGETS walking_pattern_generator 76 | EXPORT export_${PROJECT_NAME} 77 | ARCHIVE DESTINATION lib 78 | LIBRARY DESTINATION lib 79 | RUNTIME DESTINATION bin 80 | ) 81 | 82 | ament_export_include_directories( 83 | include 84 | ) 85 | ament_export_libraries( 86 | walking_pattern_generator 87 | ) 88 | ament_export_targets( 89 | export_${PROJECT_NAME} 90 | ) 91 | 92 | # install(TARGETS 93 | # walking_pattern_generator 94 | # DESTINATION lib/${PROJECT_NAME} 95 | # ) 96 | 97 | if(BUILD_TESTING) 98 | find_package(ament_lint_auto REQUIRED) 99 | ament_lint_auto_find_test_dependencies() 100 | endif() 101 | 102 | ament_package() 103 | -------------------------------------------------------------------------------- /walking_pattern_generator/include/walking_pattern_generator/LinearInvertedPendulumModel.hpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "robot_manager/control_plugin_bases/PluginBase_WalkingPatternGenerator.hpp" 3 | #include "robot_manager/control_plugin_bases/PluginBase_FootStepPlanner.hpp" 4 | 5 | #include "Eigen/Dense" 6 | 7 | namespace walking_pattern_generator 8 | { 9 | class WPG_LinearInvertedPendulumModel : public control_plugin_base::WalkingPatternGenerator { 10 | public: 11 | WPG_LinearInvertedPendulumModel(); 12 | ~WPG_LinearInvertedPendulumModel(){} 13 | 14 | std::unique_ptr walking_pattern_generator( 15 | const std::shared_ptr foot_step_ptr 16 | ) override; 17 | 18 | private: 19 | rclcpp::Node::SharedPtr node_ptr_; 20 | std::shared_ptr client_param_; 21 | 22 | double CONTROL_CYCLE_ = 0; 23 | double WALKING_CYCLE_ = 0; 24 | double WAIST_POS_Z_ = 0; 25 | 26 | }; 27 | } -------------------------------------------------------------------------------- /walking_pattern_generator/include/walking_pattern_generator/WalkingPatternGenerator.hpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "msgs_package/msg/walking_pattern.hpp" // いずれ消える？ 3 | #include "msgs_package/msg/control_output.hpp" // DEBUG: 4 | #include "sensor_msgs/msg/joint_state.hpp" 5 | #include "kinematics/IK.hpp" 6 | #include "kinematics/FK.hpp" 7 | #include "kinematics/Jacobian.hpp" 8 | 9 | #include "Eigen/Dense" 10 | 11 | namespace walking_pattern_generator 12 | { 13 | class WalkingPatternGenerator : public rclcpp::Node { 14 | public: 15 | WalkingPatternGenerator(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); 16 | 17 | private: 18 | // publisher 19 | // TODO: managerが完成次第、ControlOutput -> WalkingPattern に変更するべき 20 | // TODO: 重要性からして、ここはServiceのほうがいい気がするんだ。 21 | // TODO: ここの型をJointStateにして、ros2_controlに対応させる。さすればRviz2との連携も可能。 22 | // rclcpp::Publisher::SharedPtr pub_walking_pattern_; 23 | 24 | // CHECKME 25 | rclcpp::Publisher::SharedPtr pub_walking_pattern_; 26 | 27 | // 共有ライブラリの実体化 28 | kinematics::FK FK_; 29 | kinematics::IK IK_; 30 | kinematics::Jacobian Jacobian_; 31 | 32 | // 動歩行パターン生成関数 33 | void WalkingPatternGenerate(void); 34 | 35 | // 歩行パターンの行列. 各関節の角度・回転速度を保存 36 | std::vector> WalkingPattern_Pos_legR_; 37 | std::vector> WalkingPattern_Pos_legL_; 38 | std::vector> WalkingPattern_Vel_legR_; 39 | std::vector> WalkingPattern_Vel_legL_; 40 | 41 | // ヤコビアン 42 | Eigen::Matrix Jacobi_legR_; 43 | Eigen::Matrix Jacobi_legL_; 44 | 45 | // 関節角度 46 | std::array Q_legR_; 47 | std::array Q_legL_; 48 | 49 | 50 | // TODO: Parameterとして扱いたい変数達 51 | void DEBUG_ParameterSetting(void); 52 | 53 | float weight_; // 重量[kg] 54 | float length_leg_; // 脚の長さ[m]. これが腰の高さとなる。 55 | 56 | // 歩行パラメータ 57 | std::vector> LandingPosition_; 58 | 59 | // 目標足先姿勢行列 60 | Eigen::Matrix R_target_leg; 61 | 62 | // 脚の単位行列 63 | std::array UnitVec_legR_; 64 | std::array UnitVec_legL_; 65 | 66 | // 脚のリンク長 67 | // proto準拠 68 | std::array P_legR_; 69 | std::array P_legL_; 70 | // 腰位置に合わせる 71 | std::array P_legR_waist_standard_; 72 | std::array P_legL_waist_standard_; 73 | }; 74 | } -------------------------------------------------------------------------------- /walking_pattern_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | walking_pattern_generator 5 | 0.0.0 6 | Humanoid robot's walking_pattern_generator definition 7 | Yusuke Yamasaki 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | 16 | robot_manager 17 | pluginlib 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /walking_pattern_generator/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Linear Inverted Pendulum Model plugin. 4 | 5 | -------------------------------------------------------------------------------- /walking_pattern_generator/src/WalkingPatternGenerator_main.cpp: -------------------------------------------------------------------------------- 1 | #include "rclcpp/rclcpp.hpp" 2 | #include "walking_pattern_generator/WalkingPatternGenerator.hpp" 3 | 4 | int main(int argc, char *argv[]) { 5 | rclcpp::init(argc, argv); 6 | rclcpp::spin(std::make_shared()); 7 | rclcpp::shutdown(); 8 | 9 | return 0; 10 | } -------------------------------------------------------------------------------- /walking_stabilization_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(walking_stabilization_controller) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(ament_cmake_ros REQUIRED) 21 | find_package(rclcpp REQUIRED) 22 | find_package(robot_manager REQUIRED) 23 | find_package(pluginlib REQUIRED) 24 | 25 | pluginlib_export_plugin_description_file(robot_manager plugins.xml) 26 | 27 | add_library(walking_stabilization_controller 28 | src/WalkingStabilizationController.cpp 29 | ) 30 | ament_target_dependencies(walking_stabilization_controller 31 | rclcpp 32 | robot_manager 33 | pluginlib 34 | ) 35 | target_include_directories(walking_stabilization_controller 36 | PUBLIC 37 | $ 38 | $ 39 | PRIVATE 40 | include 41 | ) 42 | 43 | target_compile_definitions(walking_stabilization_controller 44 | PRIVATE 45 | "WALKING_STABILIZATION_CONTROLLER_BUILDING_LIBRARY" 46 | ) 47 | 48 | install( 49 | DIRECTORY 50 | include/ 51 | DESTINATION 52 | include 53 | ) 54 | install(TARGETS walking_stabilization_controller 55 | EXPORT export_walking_stabilization_controller 56 | ARCHIVE DESTINATION lib 57 | LIBRARY DESTINATION lib 58 | RUNTIME DESTINATION bin 59 | ) 60 | 61 | ament_export_include_directories( 62 | include 63 | ) 64 | ament_export_libraries( 65 | walking_stabilization_controller 66 | ) 67 | ament_export_targets( 68 | export_walking_stabilization_controller 69 | ) 70 | 71 | if(BUILD_TESTING) 72 | find_package(ament_lint_auto REQUIRED) 73 | ament_lint_auto_find_test_dependencies() 74 | endif() 75 | 76 | ament_package() 77 | 78 | 79 | -------------------------------------------------------------------------------- /walking_stabilization_controller/include/walking_stabilization_controller/WalkingStabilizationController.hpp: -------------------------------------------------------------------------------- 1 | #ifndef WALKING_STABILIZATION_CONTROLLER_HPP 2 | #define WALKING_STABILIZATION_CONTROLLER_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | // #include "robot_manager/control_plugin_bases/PluginBase_WalkingPatternGenerator.hpp" 6 | #include "robot_manager/control_plugin_bases/PluginBase_WalkingStabilizationController.hpp" 7 | 8 | namespace walking_stabilization_controller 9 | { 10 | class Default_WalkingStabilizationController : public control_plugin_base::WalkingStabilizationController 11 | { 12 | public: 13 | Default_WalkingStabilizationController(); 14 | ~Default_WalkingStabilizationController(){} 15 | 16 | std::unique_ptr walking_stabilization_controller( 17 | std::shared_ptr walking_pattern_ptr 18 | ) override; 19 | 20 | private: 21 | rclcpp::Node::SharedPtr node_ptr_; 22 | std::shared_ptr client_param_; 23 | 24 | }; 25 | // class Default_WalkingStabilizationController : public control_plugin_base::WalkingStabilizationController 26 | // { 27 | // public: 28 | // Default_WalkingStabilizationController(){} 29 | // ~Default_WalkingStabilizationController(){} 30 | 31 | // std::unique_ptr walking_stabilization_controller( 32 | // std::shared_ptr walking_pattern_ptr, 33 | // const uint32_t control_step 34 | // ) override; 35 | 36 | // private: 37 | // uint32_t max_step = 0; 38 | // }; 39 | } 40 | 41 | #endif -------------------------------------------------------------------------------- /walking_stabilization_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | walking_stabilization_controller 5 | 0.0.0 6 | TODO: Package description 7 | Yusuke Yamasaki 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | rclcpp 13 | robot_manager 14 | pluginlib 15 | 16 | ament_lint_auto 17 | ament_lint_common 18 | 19 | 20 | ament_cmake 21 | 22 | 23 | -------------------------------------------------------------------------------- /walking_stabilization_controller/plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Default walking_stabilization_controller plugin. 4 | 5 | -------------------------------------------------------------------------------- /walking_stabilization_controller/src/WalkingStabilizationController.cpp: -------------------------------------------------------------------------------- 1 | #include "walking_stabilization_controller/WalkingStabilizationController.hpp" 2 | 3 | namespace walking_stabilization_controller 4 | { 5 | std::unique_ptr Default_WalkingStabilizationController::walking_stabilization_controller( 6 | const std::shared_ptr walking_pattern_ptr 7 | ) { 8 | auto walking_stabilization_ptr = std::make_unique(); 9 | walking_stabilization_ptr->cog_pos_fix = walking_pattern_ptr->cc_cog_pos_ref; 10 | walking_stabilization_ptr->cog_vel_fix = walking_pattern_ptr->cc_cog_vel_ref; 11 | walking_stabilization_ptr->zmp_pos_fix = walking_pattern_ptr->wc_foot_land_pos_ref; 12 | 13 | // std::cout << "Here is default walking stabilization controller class." << std::endl; 14 | 15 | return walking_stabilization_ptr; 16 | } 17 | 18 | Default_WalkingStabilizationController::Default_WalkingStabilizationController() { 19 | node_ptr_ = rclcpp::Node::make_shared("WalkingStabilizationController"); 20 | client_param_ = std::make_shared(node_ptr_, "RobotParameterServer"); 21 | 22 | RCLCPP_INFO(node_ptr_->get_logger(), "Start Up WalkingStabilizationController."); 23 | } 24 | } 25 | // std::unique_ptr Default_WalkingStabilizationController::walking_stabilization_controller( 26 | // const std::shared_ptr walking_pattern_ptr, 27 | // const uint32_t control_step 28 | // ) { 29 | // auto walking_stabilization_ptr = std::make_unique(); 30 | 31 | // if(control_step > walking_pattern_ptr->cog_pos_ref.size()-1) { 32 | // max_step = walking_pattern_ptr->cog_pos_ref.size()-1; 33 | // walking_stabilization_ptr->cog_pos_fix = walking_pattern_ptr->cog_pos_ref.at(max_step); 34 | // walking_stabilization_ptr->cog_vel_fix = walking_pattern_ptr->cog_vel_ref.at(max_step); 35 | // walking_stabilization_ptr->zmp_pos_fix = walking_pattern_ptr->foot_pos_ref.at(max_step); 36 | // } 37 | 38 | // walking_stabilization_ptr->cog_pos_fix = walking_pattern_ptr->cog_pos_ref.at(control_step); 39 | // walking_stabilization_ptr->cog_vel_fix = walking_pattern_ptr->cog_vel_ref.at(control_step); 40 | // walking_stabilization_ptr->zmp_pos_fix = walking_pattern_ptr->foot_pos_ref.at(control_step); 41 | 42 | // // std::cout << "Here is default walking stabilization controller class." << std::endl; 43 | 44 | // return walking_stabilization_ptr; 45 | // } 46 | 47 | 48 | #include 49 | 50 | PLUGINLIB_EXPORT_CLASS(walking_stabilization_controller::Default_WalkingStabilizationController, control_plugin_base::WalkingStabilizationController) -------------------------------------------------------------------------------- /webots_robot_handler/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(webots_robot_handler) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++17 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 17) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | 18 | # find dependencies 19 | find_package(ament_cmake REQUIRED) 20 | find_package(rclcpp REQUIRED) 21 | 22 | # find_package(msgs_package REQUIRED) 23 | # find_package(kinematics REQUIRED) 24 | find_package(robot_messages REQUIRED) 25 | find_package(sensor_msgs REQUIRED) 26 | 27 | find_package(Eigen3 REQUIRED) 28 | find_package(webots_ros2_driver REQUIRED) 29 | # find_package(webots_ros2_control REQUIRED) 30 | # find_package(controller_manager REQUIRED) 31 | find_package(pluginlib REQUIRED) 32 | 33 | pluginlib_export_plugin_description_file(webots_ros2_driver webots_robot_handler.xml) 34 | 35 | add_library(${PROJECT_NAME} 36 | SHARED 37 | src/WebotsRobotHander.cpp 38 | ) 39 | ament_target_dependencies(${PROJECT_NAME} 40 | pluginlib 41 | rclcpp 42 | webots_ros2_driver 43 | # kinematics 44 | sensor_msgs 45 | robot_messages 46 | # webots_ros2_control 47 | # controller_manager 48 | # msgs_package 49 | ) 50 | target_include_directories(${PROJECT_NAME} 51 | PUBLIC 52 | $ 53 | $ 54 | $ 55 | PRIVATE 56 | include 57 | ) 58 | install(TARGETS ${PROJECT_NAME} 59 | ARCHIVE DESTINATION lib 60 | LIBRARY DESTINATION lib 61 | RUNTIME DESTINATION bin 62 | ) 63 | ament_export_include_directories( 64 | include 65 | ${EIGEN3_INCLUDE_DIR} 66 | ) 67 | ament_export_libraries(${PROJECT_NAME}) 68 | 69 | install(DIRECTORY 70 | launch 71 | worlds 72 | resource 73 | DESTINATION share/${PROJECT_NAME}/ 74 | ) 75 | 76 | if(BUILD_TESTING) 77 | find_package(ament_lint_auto REQUIRED) 78 | ament_lint_auto_find_test_dependencies() 79 | endif() 80 | 81 | ament_package() 82 | -------------------------------------------------------------------------------- /webots_robot_handler/include/webots_robot_handler/WebotsRobotHandler.hpp: -------------------------------------------------------------------------------- 1 | #ifndef WEBOTS_ROBOT_HANDLER_HPP 2 | #define WEBOTS_ROBOT_HANDLER_HPP 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "robot_messages/msg/feedback.hpp" 6 | #include "sensor_msgs/msg/joint_state.hpp" 7 | 8 | #include "webots_ros2_driver/PluginInterface.hpp" 9 | #include "webots_ros2_driver/WebotsNode.hpp" 10 | 11 | namespace webots_robot_handler 12 | { 13 | class WebotsRobotHandler : public webots_ros2_driver::PluginInterface { 14 | public: 15 | void init( 16 | webots_ros2_driver::WebotsNode *node, 17 | std::unordered_map ¶meters 18 | ) override; 19 | 20 | void step() override; 21 | 22 | private: 23 | // マネージャからのCallback 24 | void JointStates_Callback(const sensor_msgs::msg::JointState::SharedPtr callback_data); 25 | 26 | // DEBUG: 1つ前のcounterを記憶。データ落ちが無いかの判定に用いる。 27 | int counter_old_ = -1; 28 | int loss_count_ = 0; 29 | 30 | // TODO: Parameterから読み取るべき 31 | // TODO: 生成に必要な変数 32 | float weight_ = 0; 33 | float length_leg_ = 0; 34 | 35 | // 歩行パターンの変数（行列） 36 | std::vector> WalkingPattern_Pos_legR_ = {{0, 0, 0, 0, 0, 0}}; 37 | std::vector> WalkingPattern_Vel_legR_ = {{0, 0, 0, 0, 0, 0}}; 38 | std::vector> WalkingPattern_Pos_legL_ = {{0, 0, 0, 0, 0, 0}}; 39 | std::vector> WalkingPattern_Vel_legL_ = {{0, 0, 0, 0, 0, 0}}; 40 | 41 | std::array Q_legR_ = {0, 0, 0, 0, 0, 0}; 42 | std::array Q_legL_ = {0, 0, 0, 0, 0, 0}; 43 | 44 | // == init() == 45 | 46 | // init関数以外でもrclcpp::Nodeを使えるようにするため。 47 | webots_ros2_driver::WebotsNode *node_; 48 | 49 | rclcpp::Publisher::SharedPtr pub_feedback_; 50 | std::shared_ptr pub_feedback_msg_ = std::make_shared(); 51 | rclcpp::Subscription::SharedPtr sub_joint_state_; 52 | 53 | // Webots内のロボットが持つデバイスのタグを持つ。このタグをもとに、Webotsの関数はデバイスを区別する。 54 | WbDeviceTag motorsTag_[20]; // 全モータ２０個 55 | WbDeviceTag positionSensorsTag_[20]; // 全モータの回転角度センサ２０個 56 | WbDeviceTag gyroTag_; // ジャイロセンサ 57 | WbDeviceTag accelerometerTag_; // 加速度センサ 58 | 59 | // 処理に役立つ配列 60 | std::array jointNum_legR_; // motorsTag[20]とpositionSensorTag[20]に対応する、モータ（とセンサ）の列番号を記憶（右足） 61 | std::array jointNum_legL_; // 上に同じ（左足） 62 | std::array jointAng_posi_or_nega_legR_; // モータの回転方向の系が、モータごとに違う。Kinematicsの方ではすべて右手系で計算している。ので、Webots内環境に合わせるための補正（正負の逆転）をかける。（右足） 63 | std::array jointAng_posi_or_nega_legL_; // 上に同じ（左足） 64 | 65 | // DEBUG:===/* 66 | void DEBUG_ParameterSetting(void); 67 | 68 | // TODO: Parameter serverやURDF、Protoから読み込みたい。 69 | std::array motors_name_; 70 | std::array initJointAng_; 71 | std::array initJointVel_; 72 | 73 | 74 | // == step() == 75 | 76 | double getJointAng_[20]; // Webots側から得た関節角度を記憶 77 | const double *accelerometerValue_ = 0; 78 | const double *gyroValue_ = 0; 79 | 80 | int wait_step; // DEBUG: 初期姿勢になるまで待機するstep数 81 | 82 | int control_step; // DEBUG: 83 | 84 | int simu_step = 0; // DEBUG: 85 | int walking_step = 0; 86 | }; 87 | } 88 | 89 | #endif -------------------------------------------------------------------------------- /webots_robot_handler/launch/__pycache__/robot_launch.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/open-rdc/ROS2_Walking_Pattern_Generator/273f808839c22885fbfb652248d8893025b52a8c/webots_robot_handler/launch/__pycache__/robot_launch.cpython-38.pyc -------------------------------------------------------------------------------- /webots_robot_handler/launch/start_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from time import sleep 3 | 4 | import pathlib 5 | import launch 6 | from launch_ros.actions import Node 7 | from ament_index_python.packages import get_package_share_directory 8 | from webots_ros2_driver.webots_launcher import WebotsLauncher 9 | from webots_ros2_driver.utils import controller_url_prefix 10 | 11 | 12 | def generate_launch_description(): 13 | package_dir = get_package_share_directory("webots_robot_handler") 14 | robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'webots_robotis_op2_description.urdf')).read_text() 15 | # webots world 16 | webots = WebotsLauncher( 17 | world = os.path.join(package_dir, "worlds", "webots_simple_world.wbt") 18 | ) 19 | 20 | url = controller_url_prefix() 21 | print("prefix: " + url) 22 | # webots_robot_handler (C++_plugin of webots_ros2_driver) 23 | # TODO: 書き方が古くてWarningが出てる。WebotsControllerを使う方法に書き換えるべき。 24 | # ref: https://github.com/cyberbotics/webots_ros2/tree/master/webots_ros2_mavic 25 | robotis_op2_driver = Node( 26 | package = "webots_ros2_driver", 27 | executable = "driver", 28 | output = "screen", 29 | #additional_env = {"WEBOTS_CONTROLLER_URL": "ipc://1234/ROBOTIS_OP2"}, # Linuxの場合 30 | additional_env = {"WEBOTS_CONTROLLER_URL": url + "ROBOTIS_OP2"}, # WSL2の場合 31 | parameters = [ 32 | {"robot_description": robot_description}, 33 | {'use_sim_time': True} 34 | ] 35 | ) 36 | 37 | # sleep(4) 38 | return launch.LaunchDescription([ 39 | webots, 40 | robotis_op2_driver, 41 | launch.actions.RegisterEventHandler( 42 | event_handler = launch.event_handlers.OnProcessExit( 43 | target_action = webots, 44 | on_exit = [launch.actions.EmitEvent(event = launch.events.Shutdown())], 45 | ) 46 | ) 47 | ]) 48 | 49 | 50 | """ WARNING LOG 51 | [driver-3] [WARN] [1693453176.235405393] [ROBOTIS_OP2]: Passing robot description as a string is deprecated. Provide the URDF or Xacro file path instead. 52 | [ドライバー-3] [警告] [1693453176.235405393] [ROBOTIS_OP2]: ロボットの説明を文字列として渡すことは非推奨です。 代わりに URDF または Xacro ファイル パスを指定します。 53 | 54 | [driver-3] [WARN] [1693453176.244747602] [ROBOTIS_OP2]: The direct declaration of the driver node in the launch file is deprecated. Please use the new WebotsController node instead. 55 | [ドライバー-3] [警告] [1693453176.244747602] [ROBOTIS_OP2]: 起動ファイル内のドライバー ノードの直接宣言は非推奨です。 代わりに新しい WebotsController ノードを使用してください。 56 | """ 57 | -------------------------------------------------------------------------------- /webots_robot_handler/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | webots_robot_handler 5 | 0.0.0 6 | webots_ros2 plugin(C++) definition 7 | Yusuke Yamasaki 8 | Apache License 2.0 9 | 10 | ament_cmake 11 | 12 | launch_ros 13 | 14 | rclcpp 15 | 16 | webots_ros2_driver 17 | kinematics 18 | sensor_msgs 19 | 23 | pluginlib 24 | 25 | 26 | ament_lint_auto 27 | ament_lint_common 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /webots_robot_handler/resource/robotis_op2_ros2_control.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 1 -------------------------------------------------------------------------------- /webots_robot_handler/resource/webots_robot_handler: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /webots_robot_handler/resource/webots_robotis_op2_description.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /webots_robot_handler/webots_robot_handler.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Controller for robot in Webots 4 | 5 | -------------------------------------------------------------------------------- /webots_robot_handler/worlds/.webots_simple_world.wbproj: -------------------------------------------------------------------------------- 1 | Webots Project File version R2025a 2 | perspectives: 000000ff00000000fd0000000300000000000001ca0000039ffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000000000010000023a0000025bfc0200000001fb0000001400540065007800740045006400690074006f007201000000170000025b0000003f00ffffff00000003000007800000012afc0100000002fb0000000e0043006f006e0073006f006c006501000001d0000005b00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000005440000025b00000004000000040000000100000008fc00000000 3 | simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000002010000000101 4 | sceneTreePerspectives: 000000ff00000001000000030000001d0000026e000000fa0100000002010000000201 5 | maximizedDockId: -1 6 | centralWidgetVisible: 1 7 | orthographicViewHeight: 1 8 | textFiles: -1 9 | consoles: Console:All:All 10 | renderingDevicePerspectives: ROBOTIS OP2 Hinge2:Camera;1;1;0;0 11 | renderingDevicePerspectives: ROBOTIS OP3:Camera;0;0.484259;0;0 12 | renderingDevicePerspectives: ROBOTIS_OP2:Camera;0;1;0;0 13 | -------------------------------------------------------------------------------- /webots_robot_handler/worlds/webots_simple_world.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2023b utf8 2 | 3 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackground.proto" 4 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" 5 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/floors/protos/Floor.proto" 6 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/robotis/darwin-op/protos/RobotisOp2.proto" 7 | #EXTERNPROTO "../../robot_description/models/rdc_robot_v1/proto/simple_model.proto" 8 | 9 | WorldInfo { 10 | basicTimeStep 10 11 | } 12 | Viewpoint { 13 | orientation -0.13419524520586362 -0.033585038522204805 0.990385622548914 3.561959280293347 14 | position 1.1874580812309212 0.5930131516182624 0.483741794041867 15 | } 16 | TexturedBackground { 17 | texture "stadium_dry" 18 | } 19 | TexturedBackgroundLight { 20 | texture "stadium_dry" 21 | } 22 | Floor { 23 | } 24 | DEF ROBOTIS_OP2 RobotisOp2 { 25 | name "ROBOTIS_OP2" 26 | controller "" 27 | supervisor TRUE 28 | } 29 | --------------------------------------------------------------------------------