├── .gitignore ├── thormang3_action_module ├── data │ └── motion_4096.bin ├── package.xml ├── CHANGELOG.rst ├── include │ └── thormang3_action_module │ │ ├── action_file_define.h │ │ └── action_module.h └── CMakeLists.txt ├── thormang3_mpc ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── thormang3_walking_module ├── config │ └── balance_offset.yaml ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt └── include │ └── thormang3_walking_module │ ├── walking_module.h │ └── thormang3_online_walking.h ├── thormang3_manager ├── config │ ├── ft_calibration_data.yaml │ ├── ft_data.yaml │ ├── offset.yaml │ └── THORMANG3.robot ├── package.xml ├── launch │ ├── thormang3_manager.launch │ └── thormang3_manager_gazebo.launch ├── CHANGELOG.rst ├── CMakeLists.txt └── src │ └── thormang3_manager.cpp ├── .travis.yml ├── thormang3_manipulation_module ├── config │ ├── ik_weight.yaml │ └── ini_pose.yaml ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt └── include │ └── thormang3_manipulation_module │ └── manipulation_module.h ├── thormang3_balance_control ├── package.xml ├── CHANGELOG.rst └── CMakeLists.txt ├── motion_module_tutorial ├── package.xml ├── CHANGELOG.rst ├── include │ └── motion_module_tutorial │ │ └── motion_module_tutorial.h ├── CMakeLists.txt └── src │ └── motion_module_tutorial │ └── motion_module_tutorial.cpp ├── sensor_module_tutorial ├── package.xml ├── CHANGELOG.rst ├── include │ └── sensor_module_tutorial │ │ └── sensor_module_tutorial.h ├── src │ └── sensor_module_tutorial │ │ └── sensor_module_tutorial.cpp └── CMakeLists.txt ├── thormang3_kinematics_dynamics ├── package.xml ├── CHANGELOG.rst ├── include │ └── thormang3_kinematics_dynamics │ │ ├── link_data.h │ │ ├── kinematics_dynamics_define.h │ │ └── kinematics_dynamics.h ├── src │ └── link_data.cpp └── CMakeLists.txt ├── ati_ft_sensor ├── include │ └── ati_ft_sensor │ │ ├── ati_force_torque_sensor.h │ │ └── ati_force_torque_sensor_twe.h ├── CHANGELOG.rst ├── package.xml ├── CMakeLists.txt └── src │ └── ati_force_torque_sensor_twe.cpp ├── thormang3_gripper_module ├── CHANGELOG.rst ├── package.xml ├── include │ └── thormang3_gripper_module │ │ └── gripper_module.h ├── CMakeLists.txt └── src │ └── gripper_module.cpp ├── thormang3_base_module ├── package.xml ├── CHANGELOG.rst ├── src │ └── base_module_state.cpp ├── include │ └── thormang3_base_module │ │ ├── base_module_state.h │ │ └── base_module.h ├── data │ └── ini_pose.yaml └── CMakeLists.txt ├── thormang3_head_control_module ├── package.xml ├── CHANGELOG.rst ├── CMakeLists.txt └── include │ └── thormang3_head_control_module │ └── head_control_module.h ├── thormang3_feet_ft_module ├── package.xml ├── CHANGELOG.rst ├── include │ └── thormang3_feet_ft_module │ │ └── feet_force_torque_sensor_module.h └── CMakeLists.txt ├── README.md └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | thormang3_manager/config/ft_calibration_data.yaml 2 | thormang3_manager/config/ft_data.yaml 3 | thormang3_manager/config/offset.yaml 4 | -------------------------------------------------------------------------------- /thormang3_action_module/data/motion_4096.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/HEAD/thormang3_action_module/data/motion_4096.bin -------------------------------------------------------------------------------- /thormang3_mpc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(thormang3_mpc) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /thormang3_walking_module/config/balance_offset.yaml: -------------------------------------------------------------------------------- 1 | # 1.0 <-> 1.0 [deg] 2 | 3 | r_leg_to_body_roll_gain : 0.5 # support r_leg 4 | l_leg_to_body_roll_gain : -0.5 # support l_leg 5 | 6 | r_leg_to_body_pitch_gain : 0.0 # support r_leg 7 | l_leg_to_body_pitch_gain : 0.0 # support l_leg 8 | -------------------------------------------------------------------------------- /thormang3_manager/config/ft_calibration_data.yaml: -------------------------------------------------------------------------------- 1 | ft_right_foot_air: 2 | - 0 3 | - 0 4 | - 0 5 | - 0 6 | - 0 7 | - 0 8 | ft_right_foot_gnd: 9 | - 0 10 | - 0 11 | - 210 12 | - 0 13 | - 0 14 | - 0 15 | ft_left_foot_air: 16 | - 0 17 | - 0 18 | - 0 19 | - 0 20 | - 0 21 | - 0 22 | ft_left_foot_gnd: 23 | - 0 24 | - 0 25 | - 210 26 | - 0 27 | - 0 28 | - 0 -------------------------------------------------------------------------------- /thormang3_manager/config/ft_data.yaml: -------------------------------------------------------------------------------- 1 | ft_right_foot_calibration_matrix: 2 | [0, 0, 0, 0, 0, 0, 3 | 0, 0, 0, 0, 0, 0, 4 | 0, 0, 0, 0, 0, 0, 5 | 0, 0, 0, 0, 0, 0, 6 | 0, 0, 0, 0, 0, 0, 7 | 0, 0, 0, 0, 0, 0 ] 8 | 9 | ft_right_foot_unload: [1.65, 1.65, 1.65, 1.65, 1.65, 1.65] 10 | 11 | 12 | ft_left_foot_calibration_matrix: 13 | [0, 0, 0, 0, 0, 0, 14 | 0, 0, 0, 0, 0, 0, 15 | 0, 0, 0, 0, 0, 0, 16 | 0, 0, 0, 0, 0, 0, 17 | 0, 0, 0, 0, 0, 0, 18 | 0, 0, 0, 0, 0, 0] 19 | 20 | ft_left_foot_unload: [1.65, 1.65, 1.65, 1.65, 1.65, 1.65] -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config file for Travis CI utilizes ros-industrial/industrial_ci package. 2 | # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst 3 | 4 | sudo: required 5 | dist: trusty 6 | services: 7 | - docker 8 | language: generic 9 | python: 10 | - "2.7" 11 | compiler: 12 | - gcc 13 | notifications: 14 | email: 15 | on_success: change 16 | on_failure: always 17 | recipients: 18 | - pyo@robotis.com 19 | env: 20 | matrix: 21 | - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian 22 | # - ROS_DISTRO=kinetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=debian OS_NAME=debian OS_CODE_NAME=jessie 23 | branches: 24 | only: 25 | - master 26 | - develop 27 | - kinetic-devel 28 | install: 29 | - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config 30 | script: 31 | - source .ci_config/travis.sh 32 | 33 | -------------------------------------------------------------------------------- /thormang3_manipulation_module/config/ik_weight.yaml: -------------------------------------------------------------------------------- 1 | # weight_value 2 | 3 | weight_value : 4 | 1 : 1.0 # r_arm_sh_p1 5 | 2 : 1.0 # l_arm_sh_p1 6 | 3 : 1.0 # r_arm_sh_r 7 | 4 : 1.0 # l_arm_sh_r 8 | 5 : 1.0 # r_arm_sh_p2 9 | 6 : 1.0 # l_arm_sh_p2 10 | 7 : 3.0 # r_arm_el_y 11 | 8 : 3.0 # l_arm_el_y 12 | 9 : 1.0 # r_arm_wr_r 13 | 10 : 1.0 # l_arm_wr_r 14 | 11 : 10.0 # r_arm_wr_y 15 | 12 : 10.0 # l_arm_wr_y 16 | 13 : 10.0 # r_arm_wr_p 17 | 14 : 10.0 # l_arm_wr_p 18 | 15 : 1.0 # r_leg_hip_y 19 | 16 : 1.0 # l_leg_hip_y 20 | 17 : 1.0 # r_leg_hip_r 21 | 18 : 1.0 # l_leg_hip_r 22 | 19 : 1.0 # r_leg_hip_p 23 | 20 : 1.0 # l_leg_hip_p 24 | 21 : 1.0 # r_leg_kn_p 25 | 22 : 1.0 # l_leg_kn_p 26 | 23 : 1.0 # r_leg_an_p 27 | 24 : 1.0 # l_leg_an_p 28 | 25 : 1.0 # r_leg_an_r 29 | 26 : 1.0 # l_leg_an_r 30 | 27 : 5.0 # torso_y 31 | 28 : 1.0 # head_y 32 | 29 : 1.0 # head_p 33 | 30 : 1.0 # l_arm_grip 34 | 31 : 1.0 # r_arm_grip 35 | 36 | -------------------------------------------------------------------------------- /thormang3_balance_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_balance_control 4 | 0.2.0 5 | 6 | The thormang3_balance_control package 7 | This balance control is based on PD control. 8 | 9 | Apache 2.0 10 | Jay Song 11 | Kayman 12 | Pyo 13 | http://wiki.ros.org/thormang3_balance_control 14 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 15 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 17 | catkin 18 | roscpp 19 | cmake_modules 20 | robotis_math 21 | eigen 22 | 23 | -------------------------------------------------------------------------------- /motion_module_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | motion_module_tutorial 4 | 0.2.0 5 | 6 | The motion_module_tutorial package 7 | 8 | Apache 2.0 9 | Zerom 10 | SCH 11 | Kayman 12 | Pyo 13 | http://wiki.ros.org/motion_module_tutorial 14 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 15 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 17 | catkin 18 | roscpp 19 | std_msgs 20 | robotis_framework_common 21 | boost 22 | 23 | -------------------------------------------------------------------------------- /sensor_module_tutorial/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sensor_module_tutorial 4 | 0.2.0 5 | 6 | The sensor_module_tutorial package 7 | 8 | Apache 2.0 9 | Zerom 10 | SCH 11 | Kayman 12 | Pyo 13 | http://wiki.ros.org/sensor_module_tutorial 14 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 15 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 17 | catkin 18 | roscpp 19 | std_msgs 20 | robotis_framework_common 21 | boost 22 | 23 | -------------------------------------------------------------------------------- /thormang3_kinematics_dynamics/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_kinematics_dynamics 4 | 0.2.0 5 | 6 | This package provides a kinematics and dynamics impletation for the thormang3. 7 | It can be used to calculate forward and inverse kinematics. 8 | 9 | Apache 2.0 10 | SCH 11 | Pyo 12 | http://wiki.ros.org/thormang3_kinematics_dynamics 13 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 14 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 15 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 16 | catkin 17 | roscpp 18 | cmake_modules 19 | robotis_math 20 | eigen 21 | 22 | -------------------------------------------------------------------------------- /ati_ft_sensor/include/ati_ft_sensor/ati_force_torque_sensor.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #ifndef ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_H_ 18 | #define ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_H_ 19 | 20 | #include "ati_force_torque_sensor_twe.h" 21 | 22 | #endif /* ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_H_ */ 23 | -------------------------------------------------------------------------------- /thormang3_manipulation_module/config/ini_pose.yaml: -------------------------------------------------------------------------------- 1 | # time parameter 2 | 3 | mov_time : 3.0 # movement time 4 | 5 | # target pose [deg] 6 | 7 | tar_pose : 8 | 1 : -32.60 # r_arm_sh_p1 9 | 2 : 32.60 # l_arm_sh_p1 10 | 3 : -59.11 # r_arm_sh_r 11 | 4 : 59.11 # l_arm_sh_r 12 | 5 : 6.61 # r_arm_sh_p2 13 | 6 : -6.61 # l_arm_sh_p2 14 | 7 : -28.33 # r_arm_el_y 15 | 8 : 28.33 # l_arm_el_y 16 | 9 : -59.65 # r_arm_wr_r 17 | 10 : 59.65 # l_arm_wr_r 18 | 11 : 9.02 # r_arm_wr_y 19 | 12 : -9.02 # l_arm_wr_y 20 | 13 : -4.88 # r_arm_wr_p 21 | 14 : 4.88 # l_arm_wr_p 22 | 15 : 0.0 # r_leg_hip_y 23 | 16 : 0.0 # l_leg_hip_y 24 | 17 : 0.0 # r_leg_hip_r 25 | 18 : 0.0 # l_leg_hip_r 26 | 19 : 0.0 # r_leg_hip_p 27 | 20 : 0.0 # l_leg_hip_p 28 | 21 : 0.0 # r_leg_kn_p 29 | 22 : 0.0 # l_leg_kn_p 30 | 23 : 0.0 # r_leg_an_p 31 | 24 : 0.0 # l_leg_an_p 32 | 25 : 0.0 # r_leg_an_r 33 | 26 : 0.0 # l_leg_an_r 34 | 27 : 0.0 # torso_y 35 | 28 : 0.0 # head_y 36 | 29 : 0.0 # head_p 37 | 30 : 3.0 # l_arm_grip 38 | 31 : 3.0 # r_arm_grip 39 | -------------------------------------------------------------------------------- /thormang3_gripper_module/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_gripper_module 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * gripper done msg added 21 | * manipulation & gripper module clean up 22 | * wholebody modified 23 | * wholebody inipose added 24 | * update thormang3_head_control_module : scan lidar using range 25 | * wholebody update 26 | * gripper debug 27 | * gripper module added 28 | * wholebody added 29 | * Contributors: Jay Song, Zerom, Kayman, SCH 30 | 31 | 0.1.1 (2016-08-19) 32 | ------------------ 33 | * none 34 | 35 | 0.1.0 (2016-08-18) 36 | ------------------ 37 | * none 38 | -------------------------------------------------------------------------------- /thormang3_action_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_action_module 4 | 0.2.0 5 | 6 | This package is a motion module that can play the saved action. 7 | This module is based on position control. 8 | 9 | Apache 2.0 10 | Kayman 11 | Jay Song 12 | Pyo 13 | http://wiki.ros.org/thormang3_action_module 14 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 15 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 17 | catkin 18 | roscpp 19 | roslib 20 | std_msgs 21 | robotis_controller_msgs 22 | thormang3_action_module_msgs 23 | robotis_framework_common 24 | boost 25 | 26 | -------------------------------------------------------------------------------- /thormang3_gripper_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_gripper_module 4 | 0.2.0 5 | 6 | The thormang3_gripper_module package 7 | 8 | Apache 2.0 9 | SCH 10 | Pyo 11 | http://wiki.ros.org/thormang3_gripper_module 12 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 13 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 14 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 15 | catkin 16 | roscpp 17 | roslib 18 | std_msgs 19 | sensor_msgs 20 | robotis_controller_msgs 21 | cmake_modules 22 | robotis_framework_common 23 | robotis_math 24 | boost 25 | eigen 26 | yaml-cpp 27 | 28 | -------------------------------------------------------------------------------- /ati_ft_sensor/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ati_ft_sensor 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * none 21 | 22 | 0.1.1 (2016-08-19) 23 | ------------------ 24 | * none 25 | 26 | 0.1.0 (2016-08-18) 27 | ------------------ 28 | * first public release for Kinetic 29 | * modified package information for release 30 | * ROS C++ coding style is applied 31 | * change namespace of ati_ft_sensor package 32 | * modified file name for ROS C++ coding style 33 | * ati_ft_sensor/ATIForceTorqueSensorTWE.h : RobotisMath.h -> robotis_math.h 34 | * ROS c++ coding stylie is applying, first thormang3_kinematics_dynamics complete 35 | * Contributors: Jay Song, Zerom, SCH, Pyo 36 | -------------------------------------------------------------------------------- /ati_ft_sensor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ati_ft_sensor 4 | 0.2.0 5 | 6 | This package is a library for using ATI's transducer. 7 | This package describes basic functions for sensing force and torque. 8 | We provide some functions for converting and scaling. 9 | 10 | Apache 2.0 11 | Kayman 12 | Jay Song 13 | Pyo 14 | http://wiki.ros.org/ati_ft_sensor 15 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 17 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 18 | catkin 19 | roscpp 20 | roslib 21 | geometry_msgs 22 | cmake_modules 23 | robotis_math 24 | boost 25 | eigen 26 | yaml-cpp 27 | 28 | -------------------------------------------------------------------------------- /thormang3_base_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_base_module 4 | 0.2.0 5 | 6 | The thormang3_base_module package 7 | 8 | Apache 2.0 9 | SCH 10 | Pyo 11 | http://wiki.ros.org/thormang3_base_module 12 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 13 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 14 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 15 | catkin 16 | roscpp 17 | roslib 18 | std_msgs 19 | geometry_msgs 20 | robotis_controller_msgs 21 | cmake_modules 22 | robotis_framework_common 23 | robotis_math 24 | thormang3_kinematics_dynamics 25 | boost 26 | eigen 27 | yaml-cpp 28 | 29 | -------------------------------------------------------------------------------- /sensor_module_tutorial/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sensor_module_tutorial 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * fixed package dependencies 21 | * optimized cpu usage by spin loop (by astumpf) 22 | * wholebody modified 23 | * Contributors: Jay Song, Zerom 24 | 25 | 0.1.1 (2016-08-19) 26 | ------------------ 27 | * none 28 | 29 | 0.1.0 (2016-08-18) 30 | ------------------ 31 | * first public release for Kinetic 32 | * modified package information for release 33 | * modified motion_module_tutorial / sensor_module_tutorial 34 | * ROS C++ Coding Style is applied to motion_module_tutorial/sensor_module_tutorial 35 | * Fixed high CPU consumption due to busy waits 36 | * added Singleton template 37 | * Contributors: Jay Song, Zerom, Pyo 38 | -------------------------------------------------------------------------------- /motion_module_tutorial/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package motion_module_tutorial 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * fixed package dependencies 21 | * optimized cpu usage by spin loop (by astumpf) 22 | * wholebody modified 23 | * Contributors: Jay Song, Zerom 24 | 25 | 0.1.1 (2016-08-19) 26 | ------------------ 27 | * none 28 | 29 | 0.1.0 (2016-08-18) 30 | ------------------ 31 | * first public release for Kinetic 32 | * modified package information for release 33 | * modified motion_module_tutorial / sensor_module_tutorial 34 | * ROS C++ Coding Style is applied to motion_module_tutorial/sensor_module_tutorial 35 | * Fixed high CPU consumption due to busy waits 36 | * added Singleton template 37 | * Contributors: Jay Song, Zerom, Pyo 38 | -------------------------------------------------------------------------------- /thormang3_balance_control/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_balance_control 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * upgrade walking algorithm 8 | * modified build option and dependencies configuration 9 | * modified cmake & package setting for yaml-cpp 10 | * changed depend pkg for catkin dependencies and package.xml format to v2 11 | * changed include path for dependencies and license to Apache v2 12 | * Contributors: SCH, Pyo 13 | 14 | 0.1.3 (2017-05-23) 15 | ------------------ 16 | * updated cmake file for ros install 17 | * Contributors: SCH 18 | 19 | 0.1.2 (2017-04-24) 20 | ------------------ 21 | * upgraded walking balance control 22 | * - fixed package dependencies 23 | * Contributors: Jay Song, Zerom 24 | 25 | 0.1.1 (2016-08-19) 26 | ------------------ 27 | * none 28 | 29 | 0.1.0 (2016-08-18) 30 | ------------------ 31 | * first public release for Kinetic 32 | * modified package information for release 33 | * removed walking_module_math & modify thormang3_balance_control 34 | added set enable function for each balance algorithm 35 | removed walking module math 36 | * Walking Module Update 37 | divide balance algorithm 38 | made thormang3_balance_control 39 | * Contributors: Jay Song, Pyo 40 | -------------------------------------------------------------------------------- /thormang3_head_control_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_head_control_module 4 | 0.2.0 5 | 6 | A module to control the head. 7 | This module is included in the Thormang3 Manager as a library. 8 | 9 | Apache 2.0 10 | Zerom 11 | SCH 12 | Kayman 13 | Pyo 14 | http://wiki.ros.org/thormang3_head_control_module 15 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 17 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 18 | catkin 19 | roscpp 20 | std_msgs 21 | sensor_msgs 22 | robotis_controller_msgs 23 | thormang3_head_control_module_msgs 24 | cmake_modules 25 | robotis_framework_common 26 | robotis_math 27 | boost 28 | eigen 29 | 30 | -------------------------------------------------------------------------------- /thormang3_feet_ft_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_feet_ft_module 4 | 0.2.0 5 | 6 | This package is a sensor module for force torque sensors on feet. 7 | 8 | Apache 2.0 9 | Zerom 10 | SCH 11 | Kayman 12 | Jay Song 13 | Pyo 14 | http://wiki.ros.org/thormang3_feet_ft_module 15 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 17 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 18 | catkin 19 | roscpp 20 | std_msgs 21 | robotis_controller_msgs 22 | thormang3_feet_ft_module_msgs 23 | cmake_modules 24 | robotis_framework_common 25 | robotis_math 26 | thormang3_kinematics_dynamics 27 | ati_ft_sensor 28 | boost 29 | eigen 30 | yaml-cpp 31 | 32 | -------------------------------------------------------------------------------- /thormang3_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_manager 4 | 0.2.0 5 | 6 | This package describes robot manager to execute THORMANG3's motion modules. 7 | 8 | Apache 2.0 9 | Zerom 10 | SCH 11 | Kayman 12 | Pyo 13 | http://wiki.ros.org/thormang3_manager 14 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 15 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 17 | catkin 18 | roscpp 19 | robotis_controller 20 | thormang3_feet_ft_module 21 | thormang3_base_module 22 | thormang3_action_module 23 | thormang3_head_control_module 24 | thormang3_manipulation_module 25 | thormang3_walking_module 26 | thormang3_gripper_module 27 | thormang3_description 28 | thormang3_imu_3dm_gx4 29 | 30 | -------------------------------------------------------------------------------- /thormang3_manager/config/offset.yaml: -------------------------------------------------------------------------------- 1 | offset: 2 | head_p: 0 3 | head_y: 0 4 | l_arm_el_y: 0 5 | l_arm_grip: 0 6 | l_arm_sh_p1: 0 7 | l_arm_sh_p2: 0 8 | l_arm_sh_r: 0 9 | l_arm_wr_p: 0 10 | l_arm_wr_r: 0 11 | l_arm_wr_y: 0 12 | l_leg_an_p: 0 13 | l_leg_an_r: 0 14 | l_leg_hip_p: 0 15 | l_leg_hip_r: 0 16 | l_leg_hip_y: 0 17 | l_leg_kn_p: 0 18 | r_arm_el_y: 0 19 | r_arm_grip: 0 20 | r_arm_sh_p1: 0 21 | r_arm_sh_p2: 0 22 | r_arm_sh_r: 0 23 | r_arm_wr_p: 0 24 | r_arm_wr_r: 0 25 | r_arm_wr_y: 0 26 | r_leg_an_p: 0 27 | r_leg_an_r: 0 28 | r_leg_hip_p: 0 29 | r_leg_hip_r: 0 30 | r_leg_hip_y: 0 31 | r_leg_kn_p: 0 32 | torso_y: 0 33 | init_pose_for_offset_tuner: 34 | head_p: 0 35 | head_y: 0 36 | l_arm_el_y: -1.570796326794897 37 | l_arm_grip: 0 38 | l_arm_sh_p1: 0 39 | l_arm_sh_p2: 0 40 | l_arm_sh_r: 0 41 | l_arm_wr_p: 0 42 | l_arm_wr_r: 0 43 | l_arm_wr_y: 0 44 | l_leg_an_p: -0.1047197551196598 45 | l_leg_an_r: 0 46 | l_leg_hip_p: 0.1047197551196598 47 | l_leg_hip_r: 0 48 | l_leg_hip_y: 0 49 | l_leg_kn_p: 0.1570796326794897 50 | r_arm_el_y: 1.570796326794897 51 | r_arm_grip: 0 52 | r_arm_sh_p1: 0 53 | r_arm_sh_p2: 0 54 | r_arm_sh_r: 0 55 | r_arm_wr_p: 0 56 | r_arm_wr_r: 0 57 | r_arm_wr_y: 0 58 | r_leg_an_p: 0.1047197551196598 59 | r_leg_an_r: 0 60 | r_leg_hip_p: -0.1047197551196598 61 | r_leg_hip_r: 0 62 | r_leg_hip_y: 0 63 | r_leg_kn_p: -0.1570796326794897 64 | torso_y: 0 65 | 66 | -------------------------------------------------------------------------------- /thormang3_manipulation_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_manipulation_module 4 | 0.2.0 5 | 6 | This package describes basic function to control THORMANG3's upper-body. 7 | This module is based on position control. 8 | We provide joint space and task space control (forward kinematics, inverse kinematics). 9 | 10 | Apache 2.0 11 | SCH 12 | Pyo 13 | http://wiki.ros.org/thormang3_manipulation_module 14 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 15 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 17 | catkin 18 | roscpp 19 | roslib 20 | std_msgs 21 | geometry_msgs 22 | robotis_controller_msgs 23 | thormang3_manipulation_module_msgs 24 | cmake_modules 25 | robotis_framework_common 26 | robotis_math 27 | thormang3_kinematics_dynamics 28 | boost 29 | eigen 30 | yaml-cpp 31 | 32 | -------------------------------------------------------------------------------- /thormang3_manager/launch/thormang3_manager.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /thormang3_manager/launch/thormang3_manager_gazebo.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /thormang3_base_module/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_base_module 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * added done msg to base_module 21 | * - fixed package dependencies 22 | * - optimized cpu usage by spin loop (by astumpf) 23 | * wholebody modified 24 | * - reduce CPU consumption 25 | * wholebody inipose added 26 | * update thormang3_head_control_module : scan lidar using range 27 | * change init pose 28 | * Contributors: Jay Song, Zerom, Kayman 29 | 30 | 0.1.1 (2016-08-19) 31 | ------------------ 32 | * none 33 | 34 | 0.1.0 (2016-08-18) 35 | ------------------ 36 | * first public release for Kinetic 37 | * modified package information for release 38 | * thormang3_base_module : rename RobotisState -> BaseModuleState 39 | * applying coding style 40 | * file name changed. 41 | * modified coding style.. 42 | * ROS c++ coding stylie is applying, first thormang3_kinematics_dynamics complete 43 | * fixed high CPU consumption due to busy waits 44 | * added Singleton template 45 | * Contributors: Alexander Stumpf, Zerom, SCH, Pyo 46 | -------------------------------------------------------------------------------- /thormang3_walking_module/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_walking_module 4 | 0.2.0 5 | 6 | This package is a motion module for thormang3 walking. 7 | This package provides a online walking pattern generator for the thormang3. 8 | 9 | Apache 2.0 10 | Zerom 11 | SCH 12 | Kayman 13 | Jay Song 14 | Pyo 15 | http://wiki.ros.org/thormang3_walking_module 16 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 17 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 18 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 19 | catkin 20 | roscpp 21 | roslib 22 | std_msgs 23 | sensor_msgs 24 | geometry_msgs 25 | robotis_controller_msgs 26 | thormang3_walking_module_msgs 27 | cmake_modules 28 | eigen_conversions 29 | robotis_framework_common 30 | robotis_math 31 | thormang3_kinematics_dynamics 32 | thormang3_balance_control 33 | boost 34 | eigen 35 | yaml-cpp 36 | 37 | -------------------------------------------------------------------------------- /thormang3_kinematics_dynamics/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_kinematics_dynamics 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * fixed incorrect parameter in thormang3_kinematics_dynamics 21 | * added calculation function for global position of each link's COM 22 | * range of motion updated 23 | * wholebody inipose added 24 | * updated thormang3_head_control_module : scan lidar using range 25 | * eigen test 26 | * wholebody debugging ing... 27 | * wholebody COM added 28 | * wholebody balance 29 | * wholebody balance added 30 | * wholebody added 31 | * Contributors: Jay Song, Zerom, Kayman, SCH 32 | 33 | 0.1.1 (2016-08-19) 34 | ------------------ 35 | * none 36 | 37 | 0.1.0 (2016-08-18) 38 | ------------------ 39 | * first public release for Kinetic 40 | * modified package information for release 41 | * Walking Module Update 42 | divide balance algorithm 43 | made thormang3_balance_control 44 | * ROS C++ coding style is applied 45 | * modified ROS c++ coding style is applied 46 | * ROS c++ coding stylie is applying, first thormang3_kinematics_dynamics complete 47 | * Contributors: Jay Song, Zerom, Pyo, SCH 48 | -------------------------------------------------------------------------------- /thormang3_mpc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | thormang3_mpc 4 | 0.2.0 5 | 6 | ROS packages for the thormang3_mpc (meta package) 7 | 8 | Apache 2.0 9 | Zerom 10 | Kayman 11 | SCH 12 | Jay Song 13 | Pyo 14 | http://wiki.ros.org/thormang3_mpc 15 | http://emanual.robotis.com/docs/en/platform/thormang3/introduction/ 16 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC 17 | https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC/issues 18 | catkin 19 | ati_ft_sensor 20 | motion_module_tutorial 21 | sensor_module_tutorial 22 | thormang3_action_module 23 | thormang3_balance_control 24 | thormang3_base_module 25 | thormang3_feet_ft_module 26 | thormang3_gripper_module 27 | thormang3_head_control_module 28 | thormang3_kinematics_dynamics 29 | thormang3_manager 30 | thormang3_manipulation_module 31 | thormang3_walking_module 32 | 33 | 34 | -------------------------------------------------------------------------------- /thormang3_manipulation_module/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_manipulation_module 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * manipulation & gripper module clean up 21 | * merged develop with feature_walking_upgrade 22 | * - fixed package dependencies 23 | * - optimized cpu usage by spin loop (by astumpf) 24 | * manipulation module modified 25 | * manipulation & wholebody module modified 26 | * wholebody modified 27 | * - reduce CPU consumption 28 | * wholebody inipose added 29 | * update thormang3_head_control_module : scan lidar using range 30 | * wholebody IK bug fixed 31 | * manipulation module grip deleted 32 | * Contributors: Jay Song, Zerom, Kayman, SCH 33 | 34 | 0.1.1 (2016-08-19) 35 | ------------------ 36 | * none 37 | 38 | 0.1.0 (2016-08-18) 39 | ------------------ 40 | * first public release for Kinetic 41 | * modified package information for release 42 | * ROS C++ coding style is applied 43 | * file name changed. 44 | * thormang3_manipulation_module : appling coding style 45 | * ROS c++ coding stylie is applying, first thormang3_kinematics_dynamics complete 46 | * fixed high CPU consumption due to busy waits 47 | * Contributors: Alexander Stumpf, Jay Song, Zerom, SCH, Pyo 48 | -------------------------------------------------------------------------------- /thormang3_feet_ft_module/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_feet_ft_module 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * merged develop with feature_walking_upgrade 21 | * - fixed package dependencies 22 | * - optimized cpu usage by spin loop (by astumpf) 23 | * bug fix : calculate ft_scale 24 | * wholebody modified 25 | * - reduce CPU consumption 26 | * wholebody inipose added 27 | * update thormang3_head_control_module : scan lidar using range 28 | * Contributors: Jay Song, Zerom, Kayman 29 | 30 | 0.1.1 (2016-08-19) 31 | ------------------ 32 | * none 33 | 34 | 0.1.0 (2016-08-18) 35 | ------------------ 36 | * first public release for Kinetic 37 | * modified package information for release 38 | * bug fixed 39 | change ft calib command 40 | * modified include guard 41 | * ROS C++ coding style is applied 42 | * file name changed. 43 | * thormang3_feet_ft_module : coding style is applied. 44 | * ROS c++ coding stylie is applying, first thormang3_kinematics_dynamics complete 45 | * fixed a bug at thormang3_walking_module 46 | * fixed high CPU consumption due to busy waits 47 | * added Singleton template 48 | * SensorModule->Process() function argument changed. 49 | * Contributors: Alexander Stumpf, Jay Song, Zerom, SCH, Pyo 50 | -------------------------------------------------------------------------------- /thormang3_mpc/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_mpc 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * changed node setting 12 | * changed gazebo mode to false 13 | * upgrade walking algorism 14 | * Contributors: Kayman, SCH, Pyo 15 | 16 | 0.1.3 (2017-05-23) 17 | ------------------ 18 | * updated cmake file for ros install 19 | * Contributors: SCH 20 | 21 | 0.1.2 (2017-04-24) 22 | ------------------ 23 | * removed error of negative cut_off_freq 24 | * bug fix : balance param and joint feedback gain update 25 | * bug fix : joint feedback gain update 26 | * when balance turned off, walking can be started 27 | * modified done msg in walking module and action module 28 | * upgrade walking balance control 29 | * - fixed package dependencies 30 | * - optimized cpu usage by spin loop (by astumpf) 31 | * Merge branch 'whole-body' into walking_upgrade 32 | * bug fix : incorrect target force for balance 33 | * Merge branch 'whole-body' into walking_upgrade 34 | * wholebody modified 35 | * - reduce CPU consumption 36 | * wholebody inipose added 37 | * update thormang3_head_control_module : scan lidar using range 38 | * Contributors: Jay Song, Zerom, Kayman 39 | 40 | 0.1.1 (2016-08-19) 41 | ------------------ 42 | * package name changed 43 | imu_3dm_gx4 -> thormang3_imu_3dm_gx4 44 | * Contributors: Jay Song 45 | 46 | 0.1.0 (2016-08-18) 47 | ------------------ 48 | * first public release for Kinetic 49 | * modified package information for release 50 | * Contributors: Jay Song, Zerom, Kayman, SCH, Pyo 51 | -------------------------------------------------------------------------------- /thormang3_base_module/src/base_module_state.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * base_module_state.cpp 19 | * 20 | * Created on: Jan 11, 2016 21 | * Author: SCH 22 | */ 23 | 24 | #include "thormang3_base_module/base_module_state.h" 25 | 26 | using namespace thormang3; 27 | 28 | BaseModuleState::BaseModuleState() 29 | { 30 | is_moving_ = false; 31 | 32 | cnt_ = 0; 33 | 34 | mov_time_ = 1.0; 35 | smp_time_ = 0.008; 36 | all_time_steps_ = int(mov_time_ / smp_time_) + 1; 37 | 38 | calc_joint_tra_ = Eigen::MatrixXd::Zero(all_time_steps_, MAX_JOINT_ID + 1); 39 | 40 | joint_ini_pose_ = Eigen::MatrixXd::Zero( MAX_JOINT_ID + 1, 1); 41 | joint_pose_ = Eigen::MatrixXd::Zero( MAX_JOINT_ID + 1, 1); 42 | 43 | via_num_ = 1; 44 | 45 | joint_via_pose_ = Eigen::MatrixXd::Zero(via_num_, MAX_JOINT_ID + 1); 46 | joint_via_dpose_ = Eigen::MatrixXd::Zero(via_num_, MAX_JOINT_ID + 1); 47 | joint_via_ddpose_ = Eigen::MatrixXd::Zero(via_num_, MAX_JOINT_ID + 1); 48 | 49 | via_time_ = Eigen::MatrixXd::Zero(via_num_, 1); 50 | } 51 | 52 | BaseModuleState::~BaseModuleState() 53 | { 54 | } 55 | 56 | -------------------------------------------------------------------------------- /thormang3_base_module/include/thormang3_base_module/base_module_state.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #ifndef THORMANG3_BASE_MODULE_BASE_MODULE_STATE_H_ 18 | #define THORMANG3_BASE_MODULE_BASE_MODULE_STATE_H_ 19 | 20 | #include 21 | #include "robotis_math/robotis_math.h" 22 | #include "thormang3_kinematics_dynamics/kinematics_dynamics.h" 23 | 24 | namespace thormang3 25 | { 26 | 27 | class BaseModuleState 28 | { 29 | public: 30 | BaseModuleState(); 31 | ~BaseModuleState(); 32 | 33 | bool is_moving_; 34 | 35 | int cnt_; // counter number 36 | 37 | double mov_time_; // movement time 38 | double smp_time_; // sampling time 39 | 40 | int all_time_steps_; // all time steps of movement time 41 | 42 | Eigen::MatrixXd calc_joint_tra_; // calculated joint trajectory 43 | 44 | Eigen::MatrixXd joint_ini_pose_; 45 | Eigen::MatrixXd joint_pose_; 46 | 47 | int via_num_; 48 | 49 | Eigen::MatrixXd joint_via_pose_; 50 | Eigen::MatrixXd joint_via_dpose_; 51 | Eigen::MatrixXd joint_via_ddpose_; 52 | 53 | Eigen::MatrixXd via_time_; 54 | }; 55 | 56 | } 57 | 58 | #endif /* THORMANG3_BASE_MODULE_BASE_MODULE_STATE_H_ */ 59 | -------------------------------------------------------------------------------- /thormang3_head_control_module/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_head_control_module 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * changed depend pkg for catkin dependencies and package.xml format to v2 9 | * changed include path for dependencies and license to Apache v2 10 | * Contributors: SCH, Pyo 11 | 12 | 0.1.3 (2017-05-23) 13 | ------------------ 14 | * updated cmake file for ros install 15 | * Contributors: SCH 16 | 17 | 0.1.2 (2017-04-24) 18 | ------------------ 19 | * added done msg to base_module 20 | * added done msg to head_control_module 21 | * head control module msgs added 22 | * merged develop with feature_walking_upgrade 23 | * merged branch 'walking_upgrade' into develop 24 | * - fixed package dependencies 25 | * - optimized cpu usage by spin loop (by astumpf) 26 | * applied head_control_module_msgs 27 | * changed lidar moving method 28 | * edited range scan 29 | * - reduce CPU consumption 30 | * wholebody inipose added 31 | * update thormang3_head_control_module : scan lidar using range 32 | * Contributors: Jay Song, Zerom, Kayman, SCH 33 | 34 | 0.1.1 (2016-08-19) 35 | ------------------ 36 | * none 37 | 38 | 0.1.0 (2016-08-18) 39 | ------------------ 40 | * first public release for Kinetic 41 | * modified package information for release 42 | * modified thormang3_head_control_module/package.xml 43 | * added description in package.xml(head control module) 44 | * thormang3_head_control_module : insert license code 45 | * ROS C++ coding style is applied. 46 | * file name changed. 47 | * head_control_module : some coding style changed 48 | * ROS c++ coding stylie is applying, first thormang3_kinematics_dynamics complete 49 | * fixed high CPU consumption due to busy waits 50 | * Contributors: Alexander Stumpf, Jay Song, Zerom, Kayman, Pyo, SCH 51 | -------------------------------------------------------------------------------- /thormang3_kinematics_dynamics/include/thormang3_kinematics_dynamics/link_data.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * link_data.h 19 | * 20 | * Created on: June 7, 2016 21 | * Author: SCH 22 | */ 23 | 24 | #ifndef THORMANG3_KINEMATICS_DYNAMICS_LINK_DATA_H_ 25 | #define THORMANG3_KINEMATICS_DYNAMICS_LINK_DATA_H_ 26 | 27 | #define EIGEN_NO_DEBUG 28 | #define EIGEN_NO_STATIC_ASSERT 29 | 30 | #include 31 | #include "robotis_math/robotis_math.h" 32 | 33 | namespace thormang3 34 | { 35 | 36 | class LinkData 37 | { 38 | public: 39 | 40 | LinkData(); 41 | ~LinkData(); 42 | 43 | std::string name_; 44 | 45 | int parent_; 46 | int sibling_; 47 | int child_; 48 | 49 | double mass_; 50 | 51 | Eigen::MatrixXd relative_position_; 52 | Eigen::MatrixXd joint_axis_; 53 | Eigen::MatrixXd center_of_mass_; 54 | Eigen::MatrixXd inertia_; 55 | Eigen::MatrixXd joint_center_of_mass_; 56 | 57 | double joint_limit_max_; 58 | double joint_limit_min_; 59 | 60 | double joint_angle_; 61 | double joint_velocity_; 62 | double joint_acceleration_; 63 | 64 | Eigen::MatrixXd position_; 65 | Eigen::MatrixXd orientation_; 66 | Eigen::MatrixXd transformation_; 67 | 68 | }; 69 | 70 | } 71 | 72 | #endif /* THORMANG3_KINEMATICS_DYNAMICS_LINK_DATA_H_ */ 73 | -------------------------------------------------------------------------------- /thormang3_kinematics_dynamics/src/link_data.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * link_data.cpp 19 | * 20 | * Created on: June 7, 2016 21 | * Author: SCH 22 | */ 23 | 24 | #include "thormang3_kinematics_dynamics/link_data.h" 25 | 26 | namespace thormang3 27 | { 28 | 29 | LinkData::LinkData() 30 | { 31 | name_ = ""; 32 | 33 | parent_ = -1; 34 | sibling_ = -1; 35 | child_ = -1; 36 | 37 | mass_ = 0.0; 38 | 39 | relative_position_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0); 40 | joint_axis_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0); 41 | center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0); 42 | joint_center_of_mass_ = robotis_framework::getTransitionXYZ(0.0, 0.0, 0.0); 43 | inertia_ =robotis_framework::getInertiaXYZ(1.0, 0.0, 0.0, 1.0, 0.0, 1.0); 44 | 45 | joint_limit_max_ = 100.0; 46 | joint_limit_min_ = -100.0; 47 | 48 | joint_angle_ = 0.0; 49 | joint_velocity_ = 0.0; 50 | joint_acceleration_ = 0.0; 51 | 52 | position_ = robotis_framework::getTransitionXYZ( 0.0 , 0.0 , 0.0 ); 53 | orientation_ = robotis_framework::convertRPYToRotation( 0.0 , 0.0 , 0.0 ); 54 | transformation_ = robotis_framework::getTransformationXYZRPY( 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0); 55 | } 56 | 57 | LinkData::~LinkData(){} 58 | 59 | } 60 | -------------------------------------------------------------------------------- /sensor_module_tutorial/include/sensor_module_tutorial/sensor_module_tutorial.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #ifndef SENSOR_MODULE_TUTORIAL_SENSOR_MODULE_TUTORIAL_H_ 18 | #define SENSOR_MODULE_TUTORIAL_SENSOR_MODULE_TUTORIAL_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "robotis_framework_common/sensor_module.h" 26 | 27 | namespace ROBOTIS 28 | { 29 | 30 | class SensorModuleTutorial 31 | : public robotis_framework::SensorModule, 32 | public robotis_framework::Singleton 33 | { 34 | private: 35 | int control_cycle_msec_; 36 | boost::thread queue_thread_; 37 | 38 | /* sample subscriber & publisher */ 39 | ros::Subscriber sub1_; 40 | ros::Publisher pub1_; 41 | 42 | void queueThread(); 43 | 44 | public: 45 | SensorModuleTutorial(); 46 | virtual ~SensorModuleTutorial(); 47 | 48 | /* ROS Topic Callback Functions */ 49 | void topicCallback(const std_msgs::Int16::ConstPtr &msg); 50 | 51 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 52 | void process(std::map dxls, 53 | std::map sensors); 54 | }; 55 | 56 | } 57 | 58 | #endif /* SENSOR_MODULE_TUTORIAL_SENSOR_MODULE_TUTORIAL_H_ */ 59 | -------------------------------------------------------------------------------- /motion_module_tutorial/include/motion_module_tutorial/motion_module_tutorial.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #ifndef MOTION_MODULE_TUTORIAL_MOTION_MODULE_TUTORIAL_H_ 18 | #define MOTION_MODULE_TUTORIAL_MOTION_MODULE_TUTORIAL_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "robotis_framework_common/motion_module.h" 26 | 27 | namespace thormang3 28 | { 29 | 30 | class MotionModuleTutorial 31 | : public robotis_framework::MotionModule, 32 | public robotis_framework::Singleton 33 | { 34 | private: 35 | int control_cycle_msec_; 36 | boost::thread queue_thread_; 37 | 38 | /* sample subscriber & publisher */ 39 | ros::Subscriber sub1_; 40 | ros::Publisher pub1_; 41 | 42 | void queueThread(); 43 | 44 | public: 45 | MotionModuleTutorial(); 46 | virtual ~MotionModuleTutorial(); 47 | 48 | /* ROS Topic Callback Functions */ 49 | void topicCallback(const std_msgs::Int16::ConstPtr &msg); 50 | 51 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 52 | void process(std::map dxls, std::map sensors); 53 | 54 | void stop(); 55 | bool isRunning(); 56 | }; 57 | 58 | } 59 | 60 | #endif /* MOTION_MODULE_TUTORIAL_MOTION_MODULE_TUTORIAL_H_ */ 61 | -------------------------------------------------------------------------------- /thormang3_action_module/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_action_module 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * Contributors: SCH, Pyo 12 | 13 | 0.1.3 (2017-05-23) 14 | ------------------ 15 | * updated cmake file for ros install 16 | * Contributors: SCH 17 | 18 | 0.1.2 (2017-04-24) 19 | ------------------ 20 | * edited action file and dxl_init 21 | * added function for action_editor to action_module 22 | * bug fix : action file load 23 | * edited comment and some hard coded parameter 24 | * modified done msg in walking module and action module 25 | * add done msg : action module 26 | * code revision : action module 27 | * - fixed package dependencies 28 | * - optimized cpu usage by spin loop (by astumpf) 29 | * wholebody modified 30 | * - reduce CPU consumption 31 | * wholebody inipose added 32 | * update thormang3_head_control_module : scan lidar using range 33 | * Contributors: Jay Song, Zerom, Kayman 34 | 35 | 0.1.1 (2016-08-19) 36 | ------------------ 37 | * none 38 | 39 | 0.1.0 (2016-08-18) 40 | ------------------ 41 | * first public release for Kinetic 42 | * modified package information for release 43 | * action file edited 44 | added some new actions 45 | * walking Module Update 46 | divide balance algorithm 47 | make thormang3_balance_control 48 | * fixed a bug : check enable when action start 49 | * fixed abnormal message. 50 | * added Function - action play for only specified joints 51 | * modified motion_4096.bin 52 | added stand_motion 53 | added value for gripper 54 | * ROS C++ coding style is applied 55 | * added thormang3_action_module_msgs package 56 | * fixed a bug of initialize code in action_module.cpp 57 | * Change initialize code in action_module.cpp 58 | * fixed a bug 59 | * modified motion_4096.bin 60 | added and modify some actions in motion_4096.bin 61 | * added thormang3_action_module 62 | * Contributors: Jay Song, Zerom, SCH, Pyo 63 | -------------------------------------------------------------------------------- /thormang3_kinematics_dynamics/include/thormang3_kinematics_dynamics/kinematics_dynamics_define.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * kinematics_dynamics_define.h 19 | * 20 | * Created on: June 7, 2016 21 | * Author: SCH 22 | */ 23 | 24 | #ifndef THORMANG3_KINEMATICS_DYNAMICS_KINEMATICS_DYNAMICS_DEFINE_H_ 25 | #define THORMANG3_KINEMATICS_DYNAMICS_KINEMATICS_DYNAMICS_DEFINE_H_ 26 | 27 | namespace thormang3 28 | { 29 | 30 | #define MAX_JOINT_ID (31) // 29 + 2 31 | #define ALL_JOINT_ID (46) 32 | 33 | #define MAX_ARM_ID (7) 34 | #define MAX_LEG_ID (6) 35 | #define MAX_ITER (5) 36 | 37 | #define ID_HEAD_END (29) 38 | #define ID_COB (44) 39 | #define ID_TORSO (27) 40 | 41 | #define ID_R_ARM_START (1) 42 | #define ID_L_ARM_START (2) 43 | #define ID_R_ARM_END (35) 44 | #define ID_L_ARM_END (34) 45 | 46 | #define ID_R_LEG_START (15) 47 | #define ID_L_LEG_START (16) 48 | #define ID_R_LEG_END (45) 49 | #define ID_L_LEG_END (46) 50 | 51 | #define ID_R_LEG_FT (37) 52 | #define ID_L_LEG_FT (36) 53 | 54 | #define ID_BASE (0) 55 | #define ID_PELVIS (44) 56 | 57 | #define ID_PELVIS_POS_X (38) 58 | #define ID_PELVIS_POS_Y (39) 59 | #define ID_PELVIS_POS_Z (40) 60 | #define ID_PELVIS_ROT_X (41) 61 | #define ID_PELVIS_ROT_Y (42) 62 | #define ID_PELVIS_ROT_Z (43) 63 | 64 | 65 | #define GRAVITY_ACCELERATION (9.8) 66 | 67 | } 68 | 69 | #endif /* THORMANG3_KINEMATICS_DYNAMICS_KINEMATICS_DYNAMICS_DEFINE_H_ */ 70 | -------------------------------------------------------------------------------- /thormang3_manager/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_manager 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * changed depend pkg for catkin dependencies and package.xml format to v2 10 | * changed include path for dependencies and license to Apache v2 11 | * changed node setting 12 | * changed gazebo mode to false 13 | * upgrade walking algorism 14 | * Contributors: Kayman, SCH, Pyo 15 | 16 | 0.1.3 (2017-05-23) 17 | ------------------ 18 | * updated cmake file for ros install 19 | * Contributors: SCH 20 | 21 | 0.1.2 (2017-04-24) 22 | ------------------ 23 | * edited action file and dxl_init 24 | * reupload ft yaml 25 | * added function for action_editor to action_module 26 | * findpackage build_depend module msgs added 27 | * added control_cycle value. 28 | * upgrade walking balance control 29 | * manager parameter modified 30 | * manipulation & gripper module clean up 31 | * merged develop with feature_walking_upgrade 32 | * minor bug fixed 33 | * deleted old config 34 | * updated ft_data.yaml 35 | * initialize ftdata and offset data 36 | * wholebody inipose added 37 | * updated thormang3_head_control_module : scan lidar using range 38 | * wholebody balance gain update 39 | * gripper debug 40 | * manager modified 41 | * gripper module added 42 | * merge with wholebody 43 | * wholebody added 44 | * Contributors: Jay Song, Zerom, Kayman, SCH 45 | 46 | 0.1.1 (2016-08-19) 47 | ------------------ 48 | * package name changed 49 | imu_3dm_gx4 -> thormang3_imu_3dm_gx4 50 | * Contributors: Jay-Song 51 | 52 | 0.1.0 (2016-08-18) 53 | ------------------ 54 | * first public release for Kinetic 55 | * modified package information for release 56 | * a bug fixed 57 | test on real robot and fix bug 58 | * merged remote-tracking branch 'remotes/origin/develop' into walking_upgrade 59 | * modified motion_module_tutorial / sensor_module_tutorial 60 | * ROS C++ coding style is applied 61 | * file name changed 62 | * thormang3_manager : ROS C++ Coding Style is applied 63 | * ROS c++ coding stylie is applying, first thormang3_kinematics_dynamics complete 64 | * modify thormang3_manager.launch for real robot 65 | * added thormang3_action_module 66 | * fixed high CPU consumption due to busy waits 67 | * launch param name changed. 68 | * gazebo_robot_name changed. 69 | * Contributors: Alexander Stumpf, Jay Song, Zerom, SCH, Pyo 70 | -------------------------------------------------------------------------------- /thormang3_base_module/data/ini_pose.yaml: -------------------------------------------------------------------------------- 1 | # time parameter 2 | 3 | mov_time : 4.0 # movement time 4 | 5 | via_num : 1 # via-point number 6 | via_time : [ 2.0 ] # via-point time 7 | 8 | 9 | # via-point pose [deg] 10 | 11 | via_pose : 12 | 1 : [ -66.95 ] # r_arm_sh_p1 13 | 2 : [ 66.95 ] # l_arm_sh_p1 14 | 3 : [ -60.28 ] # r_arm_sh_r 15 | 4 : [ 60.28 ] # l_arm_sh_r 16 | 5 : [ 10.71 ] # r_arm_sh_p2 17 | 6 : [ -10.71 ] # l_arm_sh_p2 18 | 7 : [ -34.44 ] # r_arm_el_y 19 | 8 : [ 34.44 ] # l_arm_el_y 20 | 9 : [ -59.61 ] # r_arm_wr_r 21 | 10 : [ 59.61 ] # l_arm_wr_r 22 | 11 : [ 8.47 ] # r_arm_wr_y 23 | 12 : [ -8.47 ] # l_arm_wr_y 24 | 13 : [ -31.96 ] # r_arm_wr_p 25 | 14 : [ 31.96 ] # l_arm_wr_p 26 | 15 : [ 0.0 ] # r_leg_hip_y 27 | 16 : [ 0.0 ] # l_leg_hip_y 28 | 17 : [ 0.0 ] # r_leg_hip_r 29 | 18 : [ 0.0 ] # l_leg_hip_r 30 | 19 : [ 31.870072 ] # r_leg_hip_p 31 | 20 : [ -31.8700729 ] # l_leg_hip_p 32 | 21 : [ -61.517503 ] # r_leg_kn_p 33 | 22 : [ 61.517503 ] # l_leg_kn_p 34 | 23 : [ -29.647430 ] # r_leg_an_p 35 | 24 : [ 29.647430 ] # l_leg_an_p 36 | 25 : [ 0.0 ] # r_leg_an_r 37 | 26 : [ 0.0 ] # l_leg_an_r 38 | 27 : [ 0.0 ] # torso_y 39 | 28 : [ 0.0 ] # head_y 40 | 29 : [ 0.0 ] # head_p 41 | 30 : [ 3.0 ] # l_arm_grip 42 | 31 : [ 3.0 ] # r_arm_grip 43 | 44 | 45 | # target pose [deg] 46 | 47 | tar_pose : 48 | 1 : -25.0 # r_arm_sh_p1 49 | 2 : 25.0 # l_arm_sh_p1 50 | 3 : -75.0 # r_arm_sh_r 51 | 4 : 75.0 # l_arm_sh_r 52 | 5 : -5.0 # r_arm_sh_p2 53 | 6 : 5.0 # l_arm_sh_p2 54 | 7 : 30.0 # r_arm_el_y 55 | 8 : -30.0 # l_arm_el_y 56 | 9 : 0.0 # r_arm_wr_r 57 | 10 : 0.0 # l_arm_wr_r 58 | 11 : 0.0 # r_arm_wr_y 59 | 12 : 0.0 # l_arm_wr_y 60 | 13 : 0.0 # r_arm_wr_p 61 | 14 : 0.0 # l_arm_wr_p 62 | 15 : 0.0 # r_leg_hip_y 63 | 16 : 0.0 # l_leg_hip_y 64 | 17 : 0.0 # r_leg_hip_r 65 | 18 : 0.0 # l_leg_hip_r 66 | 19 : 32.40283362 # r_leg_hip_p 67 | 20 : -32.40283362 # l_leg_hip_p 68 | 21 : -61.47222691 # r_leg_kn_p 69 | 22 : 61.47222691 # l_leg_kn_p 70 | 23 : -29.06939329 # r_leg_an_p 71 | 24 : 29.06939329 # l_leg_an_p 72 | 25 : 0.0 # r_leg_an_r 73 | 26 : 0.0 # l_leg_an_r 74 | 27 : 0.0 # torso_y 75 | 28 : 0.0 # head_y 76 | 29 : 0.0 # head_p 77 | 30 : 60.0 # l_arm_grip 78 | 31 : 60.0 # r_arm_grip 79 | -------------------------------------------------------------------------------- /sensor_module_tutorial/src/sensor_module_tutorial/sensor_module_tutorial.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #include 18 | #include "sensor_module_tutorial/sensor_module_tutorial.h" 19 | 20 | using namespace ROBOTIS; 21 | 22 | SensorModuleTutorial::SensorModuleTutorial() 23 | : control_cycle_msec_(8) 24 | { 25 | module_name_ = "test_sensor_module"; // set unique module name 26 | 27 | result_["test_sensor"] = 0.0; 28 | } 29 | 30 | SensorModuleTutorial::~SensorModuleTutorial() 31 | { 32 | queue_thread_.join(); 33 | } 34 | 35 | void SensorModuleTutorial::initialize(const int control_cycle_msec, robotis_framework::Robot *robot) 36 | { 37 | control_cycle_msec_ = control_cycle_msec; 38 | queue_thread_ = boost::thread(boost::bind(&SensorModuleTutorial::queueThread, this)); 39 | } 40 | 41 | void SensorModuleTutorial::queueThread() 42 | { 43 | ros::NodeHandle ros_node; 44 | ros::CallbackQueue callback_queue; 45 | 46 | ros_node.setCallbackQueue(&callback_queue); 47 | 48 | /* subscriber */ 49 | sub1_ = ros_node.subscribe("/tutorial_topic", 10, &SensorModuleTutorial::topicCallback, this); 50 | 51 | /* publisher */ 52 | pub1_ = ros_node.advertise("/tutorial_publish", 1, true); 53 | 54 | ros::WallDuration duration(control_cycle_msec_ / 1000.0); 55 | while(ros_node.ok()) 56 | callback_queue.callAvailable(duration); 57 | } 58 | 59 | void SensorModuleTutorial::topicCallback(const std_msgs::Int16::ConstPtr &msg) 60 | { 61 | std_msgs::Int16 msg_int16; 62 | msg_int16.data = msg->data; 63 | pub1_.publish(msg_int16); 64 | } 65 | 66 | void SensorModuleTutorial::process(std::map dxls, 67 | std::map sensors) 68 | { 69 | uint16_t ext_port_data_1 = dxls["r_leg_an_p"]->dxl_state_->bulk_read_table_["external_port_data_1"]; 70 | uint16_t ext_port_data_2 = dxls["r_leg_an_p"]->dxl_state_->bulk_read_table_["external_port_data_2"]; 71 | 72 | // ... 73 | 74 | result_["test_sensor"] = 0.0; 75 | } 76 | 77 | -------------------------------------------------------------------------------- /thormang3_walking_module/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package thormang3_walking_module 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.0 (2018-03-27) 6 | ------------------ 7 | * modified build option and dependencies configuration 8 | * modified cmake & package setting for yaml-cpp 9 | * modified gain 10 | * changed depend pkg for catkin dependencies and package.xml format to v2 11 | * changed include path for dependencies and license to Apache v2 12 | * upgrade walking algorism 13 | * Contributors: SCH, Pyo 14 | 15 | 0.1.3 (2017-05-23) 16 | ------------------ 17 | * updated cmake file for ros install 18 | * Contributors: SCH 19 | 20 | 0.1.2 (2017-04-24) 21 | ------------------ 22 | * removed error of negative cut_off_freq 23 | * bug fix : balance param and joint feedback gain update 24 | * bug fix : joint feedback gain update 25 | * when balance turned off, walking can be started 26 | * modified done msg in walking module and action module 27 | * upgrade walking balance control 28 | * - fixed package dependencies 29 | * - optimized cpu usage by spin loop (by astumpf) 30 | * Merge branch 'whole-body' into walking_upgrade 31 | * bug fix : incorrect target force for balance 32 | * Merge branch 'whole-body' into walking_upgrade 33 | * wholebody modified 34 | * - reduce CPU consumption 35 | * wholebody inipose added 36 | * update thormang3_head_control_module : scan lidar using range 37 | * Contributors: Jay Song, Zerom, Kayman 38 | 39 | 0.1.1 (2016-08-19) 40 | ------------------ 41 | * modify CMakeLists.txt for sync 42 | * file name is changed 43 | * bug fix 44 | * Contributors: Jay Song 45 | 46 | 0.1.0 (2016-08-18) 47 | ------------------ 48 | * first public release for Kinetic 49 | * modified package information for release 50 | * modified thormang3_walking_module/package.xml 51 | * removed some codes that commented out. 52 | * fifth trajectory is applied 53 | * modified File name 54 | thormang3_online_walking.h 55 | thormang3_online_walking.cpp 56 | thormang3_walking_module.h 57 | thormang3_wakling_module.cpp 58 | * removed walking_module_math & modify thormang3_balance_control 59 | added set enable function for each balance algorithm 60 | removed walking module math 61 | * modified target force calculation 62 | modified thormang3_walking_module/src/robotis_online_walking.cpp 63 | add target force calculation for fx fy 64 | * added Function - action play for only specified joints 65 | modified action_module.h 66 | modified action_module.cpp 67 | modified message in walking_module.cpp 68 | * StepData can be changed in walking. 69 | modify thormang3_walking_module/src/wakling_module.cpp 70 | * ROS C++ coding style is applied. 71 | * fixed high CPU consumption due to busy waits 72 | * Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo, SCH 73 | -------------------------------------------------------------------------------- /thormang3_balance_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_balance_control) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | cmake_modules 16 | robotis_math 17 | ) 18 | 19 | find_package(Eigen3 REQUIRED) 20 | 21 | ################################################################################ 22 | # Setup for python modules and scripts 23 | ################################################################################ 24 | 25 | ################################################################################ 26 | # Declare ROS messages, services and actions 27 | ################################################################################ 28 | 29 | ################################################################################ 30 | # Declare ROS dynamic reconfigure parameters 31 | ################################################################################ 32 | 33 | ################################################################################ 34 | # Declare catkin specific configuration to be passed to dependent projects 35 | ################################################################################ 36 | catkin_package( 37 | INCLUDE_DIRS include 38 | LIBRARIES ${PROJECT_NAME} 39 | CATKIN_DEPENDS roscpp cmake_modules robotis_math 40 | DEPENDS EIGEN3 41 | ) 42 | 43 | ################################################################################ 44 | # Build 45 | ################################################################################ 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${EIGEN3_INCLUDE_DIRS} 50 | ) 51 | 52 | add_library(${PROJECT_NAME} src/thormang3_balance_control.cpp) 53 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}) 55 | 56 | ################################################################################ 57 | # Install 58 | ################################################################################ 59 | install(TARGETS ${PROJECT_NAME} 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY include/${PROJECT_NAME}/ 66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | ) 68 | 69 | ################################################################################ 70 | # Test 71 | ################################################################################ 72 | -------------------------------------------------------------------------------- /thormang3_kinematics_dynamics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_kinematics_dynamics) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | cmake_modules 16 | robotis_math 17 | ) 18 | 19 | find_package(Eigen3 REQUIRED) 20 | 21 | ################################################################################ 22 | # Setup for python modules and scripts 23 | ################################################################################ 24 | 25 | ################################################################################ 26 | # Declare ROS messages, services and actions 27 | ################################################################################ 28 | 29 | ################################################################################ 30 | # Declare ROS dynamic reconfigure parameters 31 | ################################################################################ 32 | 33 | ################################################################################ 34 | # Declare catkin specific configuration to be passed to dependent projects 35 | ################################################################################ 36 | catkin_package( 37 | INCLUDE_DIRS include 38 | LIBRARIES ${PROJECT_NAME} 39 | CATKIN_DEPENDS roscpp cmake_modules robotis_math 40 | DEPENDS EIGEN3 41 | ) 42 | 43 | ################################################################################ 44 | # Build 45 | ################################################################################ 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${EIGEN3_INCLUDE_DIRS} 50 | ) 51 | 52 | add_library(${PROJECT_NAME} src/link_data.cpp src/kinematics_dynamics.cpp) 53 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}) 55 | 56 | ################################################################################ 57 | # Install 58 | ################################################################################ 59 | install(TARGETS ${PROJECT_NAME} 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY include/${PROJECT_NAME}/ 66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | ) 68 | 69 | ################################################################################ 70 | # Test 71 | ################################################################################ 72 | -------------------------------------------------------------------------------- /motion_module_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(motion_module_tutorial) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | std_msgs 16 | robotis_framework_common 17 | ) 18 | 19 | find_package(Boost REQUIRED COMPONENTS thread) 20 | 21 | ################################################################################ 22 | # Setup for python modules and scripts 23 | ################################################################################ 24 | 25 | ################################################################################ 26 | # Declare ROS messages, services and actions 27 | ################################################################################ 28 | 29 | ################################################################################ 30 | # Declare ROS dynamic reconfigure parameters 31 | ################################################################################ 32 | 33 | ################################################################################ 34 | # Declare catkin specific configuration to be passed to dependent projects 35 | ################################################################################ 36 | catkin_package( 37 | INCLUDE_DIRS include 38 | LIBRARIES ${PROJECT_NAME} 39 | CATKIN_DEPENDS roscpp std_msgs robotis_framework_common 40 | DEPENDS Boost 41 | ) 42 | 43 | ################################################################################ 44 | # Build 45 | ################################################################################ 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${Boost_INCLUDE_DIRS} 50 | ) 51 | 52 | add_library(${PROJECT_NAME} src/${PROJECT_NAME}/motion_module_tutorial.cpp) 53 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 55 | 56 | ################################################################################ 57 | # Install 58 | ################################################################################ 59 | install(TARGETS ${PROJECT_NAME} 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY include/${PROJECT_NAME}/ 66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | ) 68 | 69 | ################################################################################ 70 | # Test 71 | ################################################################################ 72 | -------------------------------------------------------------------------------- /sensor_module_tutorial/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(sensor_module_tutorial) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | std_msgs 16 | robotis_framework_common 17 | ) 18 | 19 | find_package(Boost REQUIRED COMPONENTS thread) 20 | 21 | ################################################################################ 22 | # Setup for python modules and scripts 23 | ################################################################################ 24 | 25 | ################################################################################ 26 | # Declare ROS messages, services and actions 27 | ################################################################################ 28 | 29 | ################################################################################ 30 | # Declare ROS dynamic reconfigure parameters 31 | ################################################################################ 32 | 33 | ################################################################################ 34 | # Declare catkin specific configuration to be passed to dependent projects 35 | ################################################################################ 36 | catkin_package( 37 | INCLUDE_DIRS include 38 | LIBRARIES ${PROJECT_NAME} 39 | CATKIN_DEPENDS roscpp std_msgs robotis_framework_common 40 | DEPENDS Boost 41 | ) 42 | 43 | ################################################################################ 44 | # Build 45 | ################################################################################ 46 | include_directories( 47 | include 48 | ${catkin_INCLUDE_DIRS} 49 | ${Boost_INCLUDE_DIRS} 50 | ) 51 | 52 | add_library(${PROJECT_NAME} src/${PROJECT_NAME}/sensor_module_tutorial.cpp) 53 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 54 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 55 | 56 | ################################################################################ 57 | # Install 58 | ################################################################################ 59 | install(TARGETS ${PROJECT_NAME} 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY include/${PROJECT_NAME}/ 66 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | ) 68 | 69 | ################################################################################ 70 | # Test 71 | ################################################################################ 72 | -------------------------------------------------------------------------------- /thormang3_action_module/include/thormang3_action_module/action_file_define.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * action_module.h 19 | * 20 | * Created on: 2016. 2. 23. 21 | * Author: Jay Song 22 | */ 23 | 24 | #ifndef THORMANG3_ACTION_MODULE_ACTION_FILE_DEFINE_H_ 25 | #define THORMANG3_ACTION_MODULE_ACTION_FILE_DEFINE_H_ 26 | 27 | namespace thormang3 28 | { 29 | 30 | namespace action_file_define 31 | { 32 | 33 | const int MAXNUM_PAGE = 256; 34 | const int MAXNUM_STEP = 7; 35 | const int MAXNUM_NAME = 13; 36 | const int MAXNUM_JOINTS = 38; 37 | 38 | const int SPEED_BASE_SCHEDULE = 0; 39 | const int TIME_BASE_SCHEDULE = 0x0a; 40 | 41 | const int INVALID_BIT_MASK = 0x4000; 42 | const int TORQUE_OFF_BIT_MASK = 0x2000; 43 | 44 | typedef struct // Header Structure (total 64unsigned char) 45 | { 46 | unsigned char name[MAXNUM_NAME+1]; // Name 0~13 47 | unsigned char reserved1; // Reserved1 14 48 | unsigned char repeat; // Repeat count 15 49 | unsigned char schedule; // schedule 16 50 | unsigned char reserved2[3]; // reserved2 17~19 51 | unsigned char stepnum; // Number of step 20 52 | unsigned char reserved3; // reserved3 21 53 | unsigned char speed; // Speed 22 54 | unsigned char reserved4; // reserved4 23 55 | unsigned char accel; // Acceleration time 24 56 | unsigned char next; // Link to next 25 57 | unsigned char exit; // Link to exit 26 58 | unsigned char reserved5[4]; // reserved5 27~30 59 | unsigned char checksum; // checksum 31 60 | unsigned char pgain[MAXNUM_JOINTS]; // pgain 32~69 61 | unsigned char reserved6; // reserved6 70 62 | } PageHeader; 63 | 64 | typedef struct // Step Structure (total 64unsigned char) 65 | { 66 | unsigned short position[MAXNUM_JOINTS]; // Joint position 0~75 67 | unsigned char pause; // Pause time 76 68 | unsigned char time; // Time 77 69 | } Step; 70 | 71 | typedef struct // Page Structure (total 512unsigned char) 72 | { 73 | PageHeader header; // Page header 0~70 74 | Step step[MAXNUM_STEP]; // Page step 71~609 75 | } Page; 76 | 77 | } 78 | 79 | } 80 | 81 | #endif /* THORMANG3_ACTION_MODULE_ACTION_FILE_DEFINE_H_ */ 82 | -------------------------------------------------------------------------------- /thormang3_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_manager) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | robotis_controller 16 | thormang3_feet_ft_module 17 | thormang3_base_module 18 | thormang3_action_module 19 | thormang3_head_control_module 20 | thormang3_manipulation_module 21 | thormang3_walking_module 22 | thormang3_gripper_module 23 | ) 24 | 25 | ################################################################################ 26 | # Setup for python modules and scripts 27 | ################################################################################ 28 | 29 | ################################################################################ 30 | # Declare ROS messages, services and actions 31 | ################################################################################ 32 | 33 | ################################################################################ 34 | # Declare ROS dynamic reconfigure parameters 35 | ################################################################################ 36 | 37 | ################################################################################ 38 | # Declare catkin specific configuration to be passed to dependent projects 39 | ################################################################################ 40 | catkin_package( 41 | CATKIN_DEPENDS 42 | roscpp 43 | robotis_controller 44 | thormang3_feet_ft_module 45 | thormang3_base_module 46 | thormang3_action_module 47 | thormang3_head_control_module 48 | thormang3_manipulation_module 49 | thormang3_walking_module 50 | thormang3_gripper_module 51 | ) 52 | 53 | ################################################################################ 54 | # Build 55 | ################################################################################ 56 | include_directories( 57 | ${catkin_INCLUDE_DIRS} 58 | ) 59 | 60 | add_executable(thormang3_manager src/thormang3_manager.cpp) 61 | add_dependencies(thormang3_manager ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 62 | target_link_libraries(thormang3_manager ${catkin_LIBRARIES}) 63 | 64 | ################################################################################ 65 | # Install 66 | ################################################################################ 67 | install(TARGETS thormang3_manager 68 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 69 | ) 70 | 71 | install(DIRECTORY config launch 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 73 | ) 74 | 75 | ################################################################################ 76 | # Test 77 | ################################################################################ 78 | -------------------------------------------------------------------------------- /thormang3_action_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_action_module) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | roslib 16 | std_msgs 17 | robotis_controller_msgs 18 | thormang3_action_module_msgs 19 | robotis_framework_common 20 | ) 21 | 22 | find_package(Boost REQUIRED COMPONENTS thread) 23 | 24 | ################################################################################ 25 | # Setup for python modules and scripts 26 | ################################################################################ 27 | 28 | ################################################################################ 29 | # Declare ROS messages, services and actions 30 | ################################################################################ 31 | 32 | ################################################################################ 33 | # Declare ROS dynamic reconfigure parameters 34 | ################################################################################ 35 | 36 | ################################################################################ 37 | # Declare catkin specific configuration to be passed to dependent projects 38 | ################################################################################ 39 | catkin_package( 40 | INCLUDE_DIRS include 41 | LIBRARIES ${PROJECT_NAME} 42 | CATKIN_DEPENDS roscpp roslib std_msgs robotis_controller_msgs thormang3_action_module_msgs robotis_framework_common 43 | DEPENDS Boost 44 | ) 45 | 46 | ################################################################################ 47 | # Build 48 | ################################################################################ 49 | include_directories( 50 | include 51 | ${catkin_INCLUDE_DIRS} 52 | ${Boost_INCLUDE_DIRS} 53 | ) 54 | 55 | add_library(${PROJECT_NAME} src/action_module.cpp) 56 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 57 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 58 | 59 | ################################################################################ 60 | # Install 61 | ################################################################################ 62 | install(TARGETS ${PROJECT_NAME} 63 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | ) 67 | 68 | install(DIRECTORY include/${PROJECT_NAME}/ 69 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 70 | ) 71 | 72 | install(DIRECTORY data 73 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 74 | ) 75 | 76 | ################################################################################ 77 | # Test 78 | ################################################################################ 79 | -------------------------------------------------------------------------------- /thormang3_kinematics_dynamics/include/thormang3_kinematics_dynamics/kinematics_dynamics.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * kinematcis_dynamics.h 19 | * 20 | * Created on: June 7, 2016 21 | * Author: SCH 22 | */ 23 | 24 | #ifndef THORMANG3_KINEMATICS_DYNAMICS_KINEMATICS_DYNAMICS_H_ 25 | #define THORMANG3_KINEMATICS_DYNAMICS_KINEMATICS_DYNAMICS_H_ 26 | 27 | #define EIGEN_NO_DEBUG 28 | #define EIGEN_NO_STATIC_ASSERT 29 | 30 | #include 31 | #include 32 | 33 | #include "kinematics_dynamics_define.h" 34 | #include "link_data.h" 35 | 36 | namespace thormang3 37 | { 38 | 39 | enum TreeSelect { 40 | Manipulation, 41 | Walking, 42 | WholeBody 43 | }; 44 | 45 | class KinematicsDynamics 46 | { 47 | 48 | public: 49 | KinematicsDynamics(); 50 | ~KinematicsDynamics(); 51 | KinematicsDynamics(TreeSelect tree); 52 | 53 | std::vector findRoute(int to); 54 | std::vector findRoute(int from, int to); 55 | 56 | double calcTotalMass(int joint_id); 57 | Eigen::MatrixXd calcMassCenter(int joint_id); 58 | Eigen::MatrixXd calcCenterOfMass(Eigen::MatrixXd mc); 59 | 60 | void calcJointsCenterOfMass(int joint_id); 61 | 62 | void calcForwardKinematics(int joint_ID); 63 | 64 | Eigen::MatrixXd calcJacobian(std::vector idx); 65 | Eigen::MatrixXd calcJacobianCOM(std::vector idx); 66 | Eigen::MatrixXd calcVWerr(Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position, Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation); 67 | 68 | bool calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation , int max_iter, double ik_err); 69 | bool calcInverseKinematics(int from, int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err); 70 | 71 | // with weight 72 | bool calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err , Eigen::MatrixXd weight); 73 | bool calcInverseKinematics(int from, int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err, Eigen::MatrixXd weight); 74 | 75 | bool calcInverseKinematicsForLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw); 76 | bool calcInverseKinematicsForRightLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw); 77 | bool calcInverseKinematicsForLeftLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw); 78 | 79 | LinkData *thormang3_link_data_ [ ALL_JOINT_ID + 1 ]; 80 | 81 | double thigh_length_m_; 82 | double calf_length_m_; 83 | double ankle_length_m_; 84 | double leg_side_offset_m_; 85 | }; 86 | 87 | } 88 | 89 | #endif /* THORMANG3_KINEMATICS_DYNAMICS_KINEMATICS_DYNAMICS_H_ */ 90 | -------------------------------------------------------------------------------- /thormang3_gripper_module/include/thormang3_gripper_module/gripper_module.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #ifndef THORMANG3_GRIPPER_MODULE_H_ 18 | #define THORMANG3_GRIPPER_MODULE_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include "robotis_framework_common/motion_module.h" 35 | #include "robotis_math/robotis_math.h" 36 | 37 | #include "robotis_controller_msgs/JointCtrlModule.h" 38 | #include "robotis_controller_msgs/StatusMsg.h" 39 | #include "robotis_controller_msgs/SyncWriteItem.h" 40 | 41 | namespace thormang3 42 | { 43 | 44 | #define NUM_GRIPPER_JOINTS 2 45 | 46 | class GripperModule 47 | : public robotis_framework::MotionModule, 48 | public robotis_framework::Singleton 49 | { 50 | private: 51 | double control_cycle_sec_; 52 | boost::thread queue_thread_; 53 | boost::thread* tra_gene_tread_; 54 | 55 | /* sample subscriber & publisher */ 56 | ros::Publisher status_msg_pub_; 57 | ros::Publisher set_ctrl_module_pub_; 58 | ros::Publisher goal_torque_limit_pub_; 59 | ros::Publisher movement_done_pub_; 60 | 61 | std::map joint_name_to_id_; 62 | 63 | /* base parameters */ 64 | bool is_moving_; 65 | 66 | Eigen::VectorXd present_joint_position_; 67 | Eigen::VectorXd present_joint_velocity_; 68 | Eigen::VectorXd goal_joint_position_; 69 | 70 | sensor_msgs::JointState goal_joint_pose_msg_; 71 | 72 | std_msgs::String movement_done_msg_; 73 | 74 | /* movement */ 75 | double mov_time_; 76 | int all_time_steps_; 77 | int cnt_; 78 | 79 | Eigen::MatrixXd goal_joint_tra_; 80 | 81 | void queueThread(); 82 | 83 | void setJointPoseMsgCallback(const sensor_msgs::JointState::ConstPtr& msg); 84 | void traGeneProcJointSpace(); 85 | void setTorqueLimit(); 86 | void setEndTrajectory(); 87 | 88 | public: 89 | GripperModule(); 90 | virtual ~GripperModule(); 91 | 92 | /* ROS Topic Callback Functions */ 93 | void setModeMsgCallback(const std_msgs::String::ConstPtr& msg); 94 | 95 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 96 | void process(std::map dxls, std::map sensors); 97 | 98 | void stop(); 99 | bool isRunning(); 100 | void publishStatusMsg(unsigned int type, std::string msg); 101 | }; 102 | 103 | } 104 | 105 | #endif /* THORMANG3_GRIPPER_MODULE_H_ */ 106 | -------------------------------------------------------------------------------- /motion_module_tutorial/src/motion_module_tutorial/motion_module_tutorial.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #include 18 | #include "motion_module_tutorial/motion_module_tutorial.h" 19 | 20 | using namespace thormang3; 21 | 22 | MotionModuleTutorial::MotionModuleTutorial() 23 | : control_cycle_msec_(8) 24 | { 25 | enable_ = false; 26 | module_name_ = "test_motion_module"; // set unique module name 27 | control_mode_ = robotis_framework::PositionControl; 28 | 29 | result_["r_sho_pitch"] = new robotis_framework::DynamixelState(); 30 | result_["r_sho_roll"] = new robotis_framework::DynamixelState(); 31 | result_["r_el"] = new robotis_framework::DynamixelState(); 32 | } 33 | 34 | MotionModuleTutorial::~MotionModuleTutorial() 35 | { 36 | queue_thread_.join(); 37 | } 38 | 39 | void MotionModuleTutorial::initialize(const int control_cycle_msec, robotis_framework::Robot *robot) 40 | { 41 | control_cycle_msec_ = control_cycle_msec; 42 | queue_thread_ = boost::thread(boost::bind(&MotionModuleTutorial::queueThread, this)); 43 | } 44 | 45 | void MotionModuleTutorial::queueThread() 46 | { 47 | ros::NodeHandle ros_node; 48 | ros::CallbackQueue callback_queue; 49 | 50 | ros_node.setCallbackQueue(&callback_queue); 51 | 52 | /* subscriber */ 53 | sub1_ = ros_node.subscribe("/tutorial_topic", 10, &MotionModuleTutorial::topicCallback, this); 54 | 55 | /* publisher */ 56 | pub1_ = ros_node.advertise("/tutorial_publish", 1, true); 57 | 58 | ros::WallDuration duration(control_cycle_msec_ / 1000.0); 59 | while(ros_node.ok()) 60 | callback_queue.callAvailable(duration); 61 | } 62 | 63 | void MotionModuleTutorial::topicCallback(const std_msgs::Int16::ConstPtr &msg) 64 | { 65 | std_msgs::Int16 msg_int16; 66 | msg_int16.data = msg->data; 67 | pub1_.publish(msg_int16); 68 | } 69 | 70 | void MotionModuleTutorial::process(std::map dxls, 71 | std::map sensors) 72 | { 73 | if (enable_ == false) 74 | return; 75 | 76 | for (std::map::iterator state_iter = result_.begin(); state_iter != result_.end(); 77 | state_iter++) 78 | { 79 | int32_t p_pos = dxls[state_iter->first]->dxl_state_->present_position_; 80 | int32_t g_pos = dxls[state_iter->first]->dxl_state_->goal_position_; 81 | } 82 | 83 | // ... 84 | 85 | result_["r_sho_pitch"]->goal_position_ = 0; 86 | result_["r_sho_roll"]->goal_position_ = 0; 87 | result_["r_el"]->goal_position_ = 0; 88 | } 89 | 90 | void MotionModuleTutorial::stop() 91 | { 92 | return; 93 | } 94 | 95 | bool MotionModuleTutorial::isRunning() 96 | { 97 | return false; 98 | } 99 | 100 | -------------------------------------------------------------------------------- /thormang3_head_control_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_head_control_module) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | std_msgs 16 | sensor_msgs 17 | robotis_controller_msgs 18 | thormang3_head_control_module_msgs 19 | cmake_modules 20 | robotis_framework_common 21 | robotis_math 22 | ) 23 | 24 | find_package(Boost REQUIRED COMPONENTS thread) 25 | find_package(Eigen3 REQUIRED) 26 | 27 | ################################################################################ 28 | # Setup for python modules and scripts 29 | ################################################################################ 30 | 31 | ################################################################################ 32 | # Declare ROS messages, services and actions 33 | ################################################################################ 34 | 35 | ################################################################################ 36 | # Declare ROS dynamic reconfigure parameters 37 | ################################################################################ 38 | 39 | ################################################################################ 40 | # Declare catkin specific configuration to be passed to dependent projects 41 | ################################################################################ 42 | catkin_package( 43 | INCLUDE_DIRS include 44 | LIBRARIES ${PROJECT_NAME} 45 | CATKIN_DEPENDS 46 | roscpp 47 | std_msgs 48 | sensor_msgs 49 | robotis_controller_msgs 50 | thormang3_head_control_module_msgs 51 | cmake_modules 52 | robotis_framework_common 53 | robotis_math 54 | DEPENDS Boost EIGEN3 55 | ) 56 | 57 | ################################################################################ 58 | # Build 59 | ################################################################################ 60 | include_directories( 61 | include 62 | ${catkin_INCLUDE_DIRS} 63 | ${Boost_INCLUDE_DIRS} 64 | ${EIGEN3_INCLUDE_DIRS} 65 | ) 66 | 67 | add_library(${PROJECT_NAME} src/head_control_module.cpp) 68 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 69 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES}) 70 | 71 | ################################################################################ 72 | # Install 73 | ################################################################################ 74 | install(TARGETS ${PROJECT_NAME} 75 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 76 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 77 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 78 | ) 79 | 80 | install(DIRECTORY include/${PROJECT_NAME}/ 81 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 82 | ) 83 | 84 | ################################################################################ 85 | # Test 86 | ################################################################################ 87 | -------------------------------------------------------------------------------- /ati_ft_sensor/include/ati_ft_sensor/ati_force_torque_sensor_twe.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #ifndef ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_TWE_H_ 18 | #define ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_TWE_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "robotis_math/robotis_math.h" 28 | 29 | namespace thormang3 30 | { 31 | 32 | class ATIForceTorqueSensorTWE 33 | { 34 | public: 35 | ATIForceTorqueSensorTWE(); 36 | ~ATIForceTorqueSensorTWE(); 37 | 38 | bool initialize(const std::string& ft_data_path, const std::string& ft_data_key, 39 | const std::string& ft_frame_id, 40 | const std::string& ft_raw_publish_name, const std::string& ft_scaled_publish_name); 41 | 42 | void setScaleFactror(double ft_scale_factor); 43 | void setNullForceTorque(Eigen::MatrixXd _ft_null); 44 | void setScaleParam(double ft_scale_factor, Eigen::MatrixXd ft_null); 45 | 46 | 47 | void setCurrentVoltageOutput(double voltage0, double voltage1, double voltage2, 48 | double voltage3, double voltage4, double voltage5); 49 | void setCurrentVoltageOutput(Eigen::MatrixXd voltage); 50 | 51 | Eigen::MatrixXd getCurrentForceTorqueRaw(); 52 | Eigen::MatrixXd getCurrentForceTorqueScaled(); 53 | 54 | void getCurrentForceTorqueRaw(double *force_x_N, double *force_y_N, double *force_z_N, 55 | double *torque_x_Nm, double *torque_y_Nm, double *torque_z_Nm); 56 | void getCurrentForceTorqueScaled(double *force_x_N, double *force_y_N, double *force_z_N, 57 | double *torque_x_Nm, double *torque_y_Nm, double *torque_z_Nm); 58 | 59 | void setCurrentVoltageOutputPublish(double voltage0, double voltage1, double voltage2, 60 | double voltage3, double voltage4, double voltage5); 61 | void setCurrentVoltageOutputPublish(Eigen::MatrixXd voltage); 62 | 63 | private: 64 | bool parseFTData(const std::string& ft_data_path, const std::string& ft_data_key); 65 | 66 | Eigen::MatrixXd ft_coeff_mat_; 67 | Eigen::MatrixXd ft_unload_volatge_; 68 | Eigen::MatrixXd ft_current_volatge_; 69 | Eigen::MatrixXd ft_null_; 70 | Eigen::MatrixXd ft_raw_; 71 | Eigen::MatrixXd ft_scaled_; 72 | 73 | boost::mutex ft_scale_param_mutex_; 74 | 75 | 76 | double ft_scale_factor_; 77 | 78 | std::string ft_frame_id_; 79 | std::string ft_raw_publish_name_; 80 | std::string ft_scaled_publish_name_; 81 | 82 | bool is_ft_raw_published_; 83 | bool is_ft_scaled_published_; 84 | 85 | 86 | ros::Publisher ft_raw_pub_; 87 | ros::Publisher ft_scaled_pub_; 88 | 89 | geometry_msgs::WrenchStamped ft_raw_msg_; 90 | geometry_msgs::WrenchStamped ft_scaled_msg_; 91 | }; 92 | 93 | } 94 | 95 | 96 | #endif /* ATI_FT_SENSOR_ATI_FORCE_TORQUE_SENSOR_TWE_H_ */ 97 | -------------------------------------------------------------------------------- /thormang3_base_module/include/thormang3_base_module/base_module.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * ThorManipulation.h 19 | * 20 | * Created on: 2016. 1. 18. 21 | * Author: Zerom 22 | */ 23 | 24 | #ifndef THORMANG3_BASE_MODULE_BASE_MODULE_H_ 25 | #define THORMANG3_BASE_MODULE_BASE_MODULE_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include "robotis_controller_msgs/StatusMsg.h" 41 | 42 | #include "robotis_framework_common/motion_module.h" 43 | #include "robotis_math/robotis_math.h" 44 | #include "thormang3_kinematics_dynamics/kinematics_dynamics.h" 45 | 46 | #include "base_module_state.h" 47 | 48 | namespace thormang3 49 | { 50 | 51 | class BaseJointData 52 | { 53 | public: 54 | double position_; 55 | double velocity_; 56 | double effort_; 57 | 58 | int p_gain_; 59 | int i_gain_; 60 | int d_gain_; 61 | }; 62 | 63 | class BaseJointState 64 | { 65 | public: 66 | BaseJointData curr_joint_state_[MAX_JOINT_ID + 1]; 67 | BaseJointData goal_joint_state_[MAX_JOINT_ID + 1]; 68 | BaseJointData fake_joint_state_[MAX_JOINT_ID + 1]; 69 | }; 70 | 71 | class BaseModule: public robotis_framework::MotionModule, public robotis_framework::Singleton 72 | { 73 | public: 74 | BaseModule(); 75 | virtual ~BaseModule(); 76 | 77 | /* ROS Framework Functions */ 78 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 79 | void process(std::map dxls, std::map sensors); 80 | 81 | void stop(); 82 | bool isRunning(); 83 | 84 | /* ROS Topic Callback Functions */ 85 | void initPoseMsgCallback(const std_msgs::String::ConstPtr& msg); 86 | 87 | /* ROS Calculation Functions */ 88 | void initPoseTrajGenerateProc(); 89 | 90 | void poseGenerateProc(Eigen::MatrixXd joint_angle_pose); 91 | void poseGenerateProc(std::map& joint_angle_pose); 92 | 93 | /* Parameter */ 94 | BaseModuleState *base_module_state_; 95 | BaseJointState *joint_state_; 96 | 97 | private: 98 | void queueThread(); 99 | 100 | void setCtrlModule(std::string module); 101 | void parseIniPoseData(const std::string &path); 102 | void publishStatusMsg(unsigned int type, std::string msg); 103 | void publishDoneMsg(const std::string done_msg); 104 | 105 | int control_cycle_msec_; 106 | 107 | boost::thread queue_thread_; 108 | boost::thread tra_gene_tread_; 109 | 110 | // ros::Publisher send_tra_pub_; 111 | ros::Publisher status_msg_pub_; 112 | ros::Publisher set_ctrl_module_pub_; 113 | ros::Publisher movement_done_pub_; 114 | 115 | std::map joint_name_to_id_; 116 | 117 | bool has_goal_joints_; 118 | bool ini_pose_only_; 119 | }; 120 | 121 | } 122 | 123 | #endif /* THORMANG3_BASE_MODULE_BASE_MODULE_H_ */ 124 | -------------------------------------------------------------------------------- /thormang3_manager/src/thormang3_manager.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * thormang3_manager.cpp 19 | * 20 | * Created on: 2016. 1. 21. 21 | * Author: Zerom 22 | */ 23 | 24 | /* ROBOTIS Controller Header */ 25 | #include "robotis_controller/robotis_controller.h" 26 | 27 | /* Sensor Module Header */ 28 | #include "thormang3_feet_ft_module/feet_force_torque_sensor_module.h" 29 | 30 | /* Motion Module Header */ 31 | #include "thormang3_base_module/base_module.h" 32 | #include "thormang3_action_module/action_module.h" 33 | #include "thormang3_head_control_module/head_control_module.h" 34 | #include "thormang3_manipulation_module/manipulation_module.h" 35 | #include "thormang3_walking_module/walking_module.h" 36 | #include "thormang3_gripper_module/gripper_module.h" 37 | 38 | using namespace thormang3; 39 | 40 | int main(int argc, char **argv) 41 | { 42 | ros::init(argc, argv, "THORMANG3_Manager"); 43 | ros::NodeHandle nh; 44 | 45 | ROS_INFO("manager->init"); 46 | robotis_framework::RobotisController *controller = robotis_framework::RobotisController::getInstance(); 47 | 48 | /* Load ROS Parameter */ 49 | std::string offset_file = nh.param("offset_file_path", ""); 50 | std::string robot_file = nh.param("robot_file_path", ""); 51 | 52 | std::string init_file = nh.param("init_file_path", ""); 53 | 54 | /* gazebo simulation */ 55 | controller->gazebo_mode_ = nh.param("gazebo", false); 56 | if(controller->gazebo_mode_ == true) 57 | { 58 | ROS_WARN("SET TO GAZEBO MODE!"); 59 | std::string robot_name = nh.param("gazebo_robot_name", ""); 60 | if(robot_name != "") 61 | controller->gazebo_robot_name_ = robot_name; 62 | } 63 | 64 | if(robot_file == "") 65 | { 66 | ROS_ERROR("NO robot file path in the ROS parameters."); 67 | return -1; 68 | } 69 | 70 | if(controller->initialize(robot_file, init_file) == false) 71 | { 72 | ROS_ERROR("ROBOTIS Controller Initialize Fail!"); 73 | return -1; 74 | } 75 | 76 | if(offset_file != "") 77 | controller->loadOffset(offset_file); 78 | 79 | sleep(1); 80 | 81 | /* Add Sensor Module */ 82 | controller->addSensorModule((robotis_framework::SensorModule*)FeetForceTorqueSensor::getInstance()); 83 | 84 | /* Add Motion Module */ 85 | controller->addMotionModule((robotis_framework::MotionModule*)BaseModule::getInstance()); 86 | controller->addMotionModule((robotis_framework::MotionModule*)ActionModule::getInstance()); 87 | controller->addMotionModule((robotis_framework::MotionModule*)ManipulationModule::getInstance()); 88 | controller->addMotionModule((robotis_framework::MotionModule*)GripperModule::getInstance()); 89 | controller->addMotionModule((robotis_framework::MotionModule*)HeadControlModule::getInstance()); 90 | controller->addMotionModule((robotis_framework::MotionModule*)OnlineWalkingModule::getInstance()); 91 | 92 | controller->startTimer(); 93 | 94 | while(ros::ok()) 95 | { 96 | usleep(1000*1000); 97 | } 98 | 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /ati_ft_sensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(ati_ft_sensor) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | roslib 16 | geometry_msgs 17 | cmake_modules 18 | robotis_math 19 | ) 20 | 21 | find_package(Boost REQUIRED COMPONENTS thread) 22 | find_package(Eigen3 REQUIRED) 23 | 24 | ## Resolve system dependency on yaml-cpp, which apparently does not 25 | ## provide a CMake find_package() module. 26 | ## Insert your header file compatible specified path like '#include ' 27 | find_package(PkgConfig REQUIRED) 28 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 29 | find_path(YAML_CPP_INCLUDE_DIR 30 | NAMES yaml_cpp.h 31 | PATHS ${YAML_CPP_INCLUDE_DIRS} 32 | ) 33 | find_library(YAML_CPP_LIBRARY 34 | NAMES YAML_CPP 35 | PATHS ${YAML_CPP_LIBRARY_DIRS} 36 | ) 37 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 38 | 39 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 40 | add_definitions(-DHAVE_NEW_YAMLCPP) 41 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 42 | 43 | ################################################################################ 44 | # Setup for python modules and scripts 45 | ################################################################################ 46 | 47 | ################################################################################ 48 | # Declare ROS messages, services and actions 49 | ################################################################################ 50 | 51 | ################################################################################ 52 | # Declare ROS dynamic reconfigure parameters 53 | ################################################################################ 54 | 55 | ################################################################################ 56 | # Declare catkin specific configuration to be passed to dependent projects 57 | ################################################################################ 58 | catkin_package( 59 | INCLUDE_DIRS include 60 | LIBRARIES ${PROJECT_NAME} 61 | CATKIN_DEPENDS roscpp roslib geometry_msgs cmake_modules robotis_math 62 | DEPENDS Boost EIGEN3 63 | ) 64 | 65 | ################################################################################ 66 | # Build 67 | ################################################################################ 68 | include_directories( 69 | include 70 | ${catkin_INCLUDE_DIRS} 71 | ${Boost_INCLUDE_DIRS} 72 | ${EIGEN3_INCLUDE_DIRS} 73 | ${YAML_CPP_INCLUDE_DIRS} 74 | ) 75 | 76 | add_library(${PROJECT_NAME} src/ati_force_torque_sensor_twe.cpp) 77 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 78 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${YAML_CPP_LIBRARIES}) 79 | 80 | ################################################################################ 81 | # Install 82 | ################################################################################ 83 | install(TARGETS ${PROJECT_NAME} 84 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 85 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 87 | ) 88 | 89 | install(DIRECTORY include/${PROJECT_NAME}/ 90 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 91 | ) 92 | 93 | ################################################################################ 94 | # Test 95 | ################################################################################ 96 | -------------------------------------------------------------------------------- /thormang3_manager/config/THORMANG3.robot: -------------------------------------------------------------------------------- 1 | [ control info ] 2 | control_cycle = 8 # milliseconds 3 | 4 | [ port info ] 5 | # PORT NAME | BAUDRATE | DEFAULT JOINT 6 | /dev/ttyUSB0 | 2000000 | r_arm_sh_p1 7 | /dev/ttyUSB1 | 2000000 | l_arm_sh_p1 8 | /dev/ttyUSB2 | 2000000 | r_leg_hip_y 9 | /dev/ttyUSB3 | 2000000 | l_leg_hip_y 10 | 11 | [ device info ] 12 | # TYPE | PORT NAME | ID | MODEL | PROTOCOL | DEV NAME | BULK READ ITEMS 13 | dynamixel | /dev/ttyUSB0 | 1 | H54-100-S500-R | 2.0 | r_arm_sh_p1 | present_position, present_voltage 14 | dynamixel | /dev/ttyUSB1 | 2 | H54-100-S500-R | 2.0 | l_arm_sh_p1 | present_position, present_voltage 15 | dynamixel | /dev/ttyUSB0 | 3 | H54-100-S500-R | 2.0 | r_arm_sh_r | present_position, present_voltage 16 | dynamixel | /dev/ttyUSB1 | 4 | H54-100-S500-R | 2.0 | l_arm_sh_r | present_position, present_voltage 17 | dynamixel | /dev/ttyUSB0 | 5 | H54-100-S500-R | 2.0 | r_arm_sh_p2 | present_position, present_voltage 18 | dynamixel | /dev/ttyUSB1 | 6 | H54-100-S500-R | 2.0 | l_arm_sh_p2 | present_position, present_voltage 19 | dynamixel | /dev/ttyUSB0 | 7 | H54-100-S500-R | 2.0 | r_arm_el_y | present_position, present_voltage 20 | dynamixel | /dev/ttyUSB1 | 8 | H54-100-S500-R | 2.0 | l_arm_el_y | present_position, present_voltage 21 | dynamixel | /dev/ttyUSB0 | 9 | H42-20-S300-R | 2.0 | r_arm_wr_r | present_position, present_voltage 22 | dynamixel | /dev/ttyUSB1 | 10 | H42-20-S300-R | 2.0 | l_arm_wr_r | present_position, present_voltage 23 | dynamixel | /dev/ttyUSB0 | 11 | H42-20-S300-R | 2.0 | r_arm_wr_y | present_position, present_voltage 24 | dynamixel | /dev/ttyUSB1 | 12 | H42-20-S300-R | 2.0 | l_arm_wr_y | present_position, present_voltage 25 | dynamixel | /dev/ttyUSB0 | 13 | H42-20-S300-R | 2.0 | r_arm_wr_p | present_position, present_voltage 26 | dynamixel | /dev/ttyUSB1 | 14 | H42-20-S300-R | 2.0 | l_arm_wr_p | present_position, present_voltage 27 | dynamixel | /dev/ttyUSB2 | 15 | H54-100-S500-R | 2.0 | r_leg_hip_y | present_position, present_voltage 28 | dynamixel | /dev/ttyUSB3 | 16 | H54-100-S500-R | 2.0 | l_leg_hip_y | present_position, present_voltage 29 | dynamixel | /dev/ttyUSB2 | 17 | H54-200-S500-R | 2.0 | r_leg_hip_r | present_position, present_voltage 30 | dynamixel | /dev/ttyUSB3 | 18 | H54-200-S500-R | 2.0 | l_leg_hip_r | present_position, present_voltage 31 | dynamixel | /dev/ttyUSB2 | 19 | H54-200-B500-R | 2.0 | r_leg_hip_p | present_position, present_voltage 32 | dynamixel | /dev/ttyUSB3 | 20 | H54-200-B500-R | 2.0 | l_leg_hip_p | present_position, present_voltage 33 | dynamixel | /dev/ttyUSB2 | 21 | H54-200-S500-R | 2.0 | r_leg_kn_p | present_position, present_voltage 34 | dynamixel | /dev/ttyUSB3 | 22 | H54-200-S500-R | 2.0 | l_leg_kn_p | present_position, present_voltage 35 | dynamixel | /dev/ttyUSB2 | 23 | H54-200-B500-R | 2.0 | r_leg_an_p | present_position, present_voltage, external_port_data_1, external_port_data_2 36 | dynamixel | /dev/ttyUSB3 | 24 | H54-200-B500-R | 2.0 | l_leg_an_p | present_position, present_voltage, external_port_data_1, external_port_data_2 37 | dynamixel | /dev/ttyUSB2 | 25 | H54-200-S500-R | 2.0 | r_leg_an_r | present_position, present_voltage, external_port_data_1, external_port_data_2, external_port_data_3, external_port_data_4 38 | dynamixel | /dev/ttyUSB3 | 26 | H54-200-S500-R | 2.0 | l_leg_an_r | present_position, present_voltage, external_port_data_1, external_port_data_2, external_port_data_3, external_port_data_4 39 | dynamixel | /dev/ttyUSB0 | 27 | H54-100-S500-R | 2.0 | torso_y | present_position, present_voltage 40 | dynamixel | /dev/ttyUSB1 | 28 | H42-20-S300-R | 2.0 | head_y | present_position, present_voltage 41 | dynamixel | /dev/ttyUSB1 | 29 | H42-20-S300-R | 2.0 | head_p | present_position, present_voltage 42 | dynamixel | /dev/ttyUSB1 | 30 | GRIPPER | 2.0 | l_arm_grip | present_position, present_voltage 43 | dynamixel | /dev/ttyUSB0 | 31 | GRIPPER | 2.0 | r_arm_grip | present_position, present_voltage 44 | -------------------------------------------------------------------------------- /thormang3_gripper_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_gripper_module) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | roslib 16 | std_msgs 17 | sensor_msgs 18 | robotis_controller_msgs 19 | cmake_modules 20 | robotis_framework_common 21 | robotis_math 22 | ) 23 | 24 | find_package(Boost REQUIRED COMPONENTS thread) 25 | find_package(Eigen3 REQUIRED) 26 | 27 | ## Resolve system dependency on yaml-cpp, which apparently does not 28 | ## provide a CMake find_package() module. 29 | ## Insert your header file compatible specified path like '#include ' 30 | find_package(PkgConfig REQUIRED) 31 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 32 | find_path(YAML_CPP_INCLUDE_DIR 33 | NAMES yaml_cpp.h 34 | PATHS ${YAML_CPP_INCLUDE_DIRS} 35 | ) 36 | find_library(YAML_CPP_LIBRARY 37 | NAMES YAML_CPP 38 | PATHS ${YAML_CPP_LIBRARY_DIRS} 39 | ) 40 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 41 | 42 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 43 | add_definitions(-DHAVE_NEW_YAMLCPP) 44 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 45 | 46 | ################################################################################ 47 | # Setup for python modules and scripts 48 | ################################################################################ 49 | 50 | ################################################################################ 51 | # Declare ROS messages, services and actions 52 | ################################################################################ 53 | 54 | ################################################################################ 55 | # Declare ROS dynamic reconfigure parameters 56 | ################################################################################ 57 | 58 | ################################################################################ 59 | # Declare catkin specific configuration to be passed to dependent projects 60 | ################################################################################ 61 | catkin_package( 62 | INCLUDE_DIRS include 63 | LIBRARIES ${PROJECT_NAME} 64 | CATKIN_DEPENDS 65 | roscpp 66 | roslib 67 | std_msgs 68 | sensor_msgs 69 | robotis_controller_msgs 70 | cmake_modules 71 | robotis_framework_common 72 | robotis_math 73 | DEPENDS Boost EIGEN3 74 | ) 75 | 76 | ################################################################################ 77 | # Build 78 | ################################################################################ 79 | include_directories( 80 | include 81 | ${catkin_INCLUDE_DIRS} 82 | ${Boost_INCLUDE_DIRS} 83 | ${EIGEN3_INCLUDE_DIRS} 84 | ${YAML_CPP_INCLUDE_DIRS} 85 | ) 86 | 87 | add_library(${PROJECT_NAME} src/gripper_module.cpp) 88 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 89 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${YAML_CPP_LIBRARIES}) 90 | 91 | ################################################################################ 92 | # Install 93 | ################################################################################ 94 | install(TARGETS ${PROJECT_NAME} 95 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 96 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 97 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | ) 99 | 100 | install(DIRECTORY include/${PROJECT_NAME}/ 101 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 102 | ) 103 | 104 | ################################################################################ 105 | # Test 106 | ################################################################################ 107 | 108 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROBOTIS THORMANG3 2 | ![](https://github.com/ROBOTIS-GIT/emanual/blob/master/assets/images/platform/thormang3/thormang3.png) 3 | 4 | ## ROS Packages for ROBOTIS THORMANG3 MPC 5 | |Version|Kinetic + Ubuntu Xenial|Melodic + Ubuntu Bionic| 6 | |:---:|:---:|:---:| 7 | |[![GitHub version](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-THORMANG-MPC.svg)](https://badge.fury.io/gh/ROBOTIS-GIT%2FROBOTIS-THORMANG-MPC)|[![Build Status](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC.svg?branch=kinetic-devel)](https://travis-ci.org/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC)|-| 8 | 9 | ## ROBOTIS e-Manual for ROBOTIS THORMANG3 10 | - [ROBOTIS e-Manual for ROBOTIS THORMANG3](http://emanual.robotis.com/docs/en/platform/thormang3/introduction/) 11 | 12 | ## Wiki for thormang3_mpc Packages 13 | - http://wiki.ros.org/thormang3_mpc (metapackage) 14 | - http://wiki.ros.org/ati_ft_sensor 15 | - http://wiki.ros.org/motion_module_tutorial 16 | - http://wiki.ros.org/sensor_module_tutorial 17 | - http://wiki.ros.org/thormang3_action_module 18 | - http://wiki.ros.org/thormang3_balance_control 19 | - http://wiki.ros.org/thormang3_base_module 20 | - http://wiki.ros.org/thormang3_feet_ft_module 21 | - http://wiki.ros.org/thormang3_gripper_module 22 | - http://wiki.ros.org/thormang3_head_control_module 23 | - http://wiki.ros.org/thormang3_kinematics_dynamics 24 | - http://wiki.ros.org/thormang3_manager 25 | - http://wiki.ros.org/thormang3_manipulation_module 26 | - http://wiki.ros.org/thormang3_walking_module 27 | 28 | ## Open Source related to ROBOTIS THORMANG3 29 | - [thormang3_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-msgs) 30 | - [thormang3_common](https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-Common) 31 | - [thormang3_mpc](https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC) 32 | - [thormang3_mpc_sensors](https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-MPC-SENSORs) 33 | - [thormang3_ppc](https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-PPC) 34 | - [thormang3_opc](https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-OPC) 35 | - [thormang3_tools](https://github.com/ROBOTIS-GIT/ROBOTIS-THORMANG-Tools) 36 | - [manipulator_h](https://github.com/ROBOTIS-GIT/ROBOTIS-MANIPULATOR-H) 37 | - [rh_p12_rn](https://github.com/ROBOTIS-GIT/RH-P12-RN) 38 | - [robotis_controller_msgs](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework-msgs) 39 | - [robotis_framework](https://github.com/ROBOTIS-GIT/ROBOTIS-Framework) 40 | - [robotis_utility](https://github.com/ROBOTIS-GIT/ROBOTIS-Utility) 41 | - [robotis_math](https://github.com/ROBOTIS-GIT/ROBOTIS-Math) 42 | - [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) 43 | - [robotis_op3](https://github.com/ROBOTIS-GIT/ROBOTIS-OP3) 44 | 45 | ## Documents and Videos related to ROBOTIS THORMANG3 46 | - [ROBOTIS e-Manual for ROBOTIS THORMANG3](http://emanual.robotis.com/docs/en/platform/thormang3/introduction/) 47 | - [ROBOTIS e-Manual for ROBOTIS Manipulator-H](http://emanual.robotis.com/docs/en/platform/manipulator_h/introduction/) 48 | - [ROBOTIS e-Manual for RH-P12-RN](http://emanual.robotis.com/docs/en/platform/rh_p12_rn/) 49 | - [ROBOTIS e-Manual for ROBOTIS Framework](http://emanual.robotis.com/docs/en/software/robotis_framework_packages/) 50 | - [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) 51 | - [ROBOTIS e-Manual for ROBOTIS OP3](http://emanual.robotis.com/docs/en/platform/op3/introduction/) 52 | 53 | ## What is THORMANG3? 54 | THOR (Tactical Hazardous Operations Robot) is an affordable, full size humanoid robot platform with advanced 55 | computational power, sophisticated sensors, high payload capacity, and dynamic motion abilities to enable many exciting researches and educational activities. 56 | 57 | ### Hardware 58 | |Feature|Description| 59 | |:---:|:---:| 60 | |DOF|29| 61 | |Actuator|200W x 10 / 100W x 11 / 20W x 8| 62 | |Computer|Intel® NUC with Intel® Core™ i5 Processor
(DDR4 RAM 8GB / M.2 SSD 128GB) x2| 63 | |Wireless router|Dlink DIR-806A x 1| 64 | |Sensor|Logitech C920 HD Camera x 1
Intel Realsense(Option) x 1
Hokuyo UTM-30LX-EW(Option) x 1
F/T: ATi Mini58-SI-2800-120 x 2
IMU: MicroSrain 3DM-GX4-25 x 1| 65 | |Battery|22V, 22000mA x 1
18.5V, 11000mA x 1| 66 | |Height|137.5cm| 67 | |Weight|42Kg| 68 | 69 | ### Software 70 | The followings are source code development environments. 71 | - OS : Linux (Ubuntu LTS 64-bit) with ROS (Robot Operating System) 72 | - Compiler : GNU project C and C++ Compiler 73 | - Programming Language : C++ 74 | -------------------------------------------------------------------------------- /thormang3_feet_ft_module/include/thormang3_feet_ft_module/feet_force_torque_sensor_module.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * feet_force_torque_sensor_module.h 19 | * 20 | * Created on: 2016. 3. 22. 21 | * Author: Jay Song 22 | */ 23 | 24 | #ifndef THORMANG3_FEET_FT_MODULE_FEET_FORCE_TORQUE_SENSOR_MODULE_H_ 25 | #define THORMANG3_FEET_FT_MODULE_FEET_FORCE_TORQUE_SENSOR_MODULE_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "robotis_controller_msgs/StatusMsg.h" 36 | #include "thormang3_feet_ft_module_msgs/BothWrench.h" 37 | 38 | #include "robotis_framework_common/sensor_module.h" 39 | #include "robotis_math/robotis_math.h" 40 | #include "thormang3_kinematics_dynamics/kinematics_dynamics.h" 41 | #include "ati_ft_sensor/ati_force_torque_sensor.h" 42 | 43 | namespace thormang3 44 | { 45 | 46 | class FeetForceTorqueSensor : public robotis_framework::SensorModule, public robotis_framework::Singleton 47 | { 48 | public: 49 | FeetForceTorqueSensor(); 50 | ~FeetForceTorqueSensor(); 51 | 52 | 53 | double r_foot_fx_raw_N_, r_foot_fy_raw_N_, r_foot_fz_raw_N_; 54 | double r_foot_tx_raw_Nm_, r_foot_ty_raw_Nm_, r_foot_tz_raw_Nm_; 55 | double l_foot_fx_raw_N_, l_foot_fy_raw_N_, l_foot_fz_raw_N_; 56 | double l_foot_tx_raw_Nm_, l_foot_ty_raw_Nm_, l_foot_tz_raw_Nm_; 57 | 58 | double r_foot_fx_scaled_N_, r_foot_fy_scaled_N_, r_foot_fz_scaled_N_; 59 | double r_foot_tx_scaled_Nm_, r_foot_ty_scaled_Nm_, r_foot_tz_scaled_Nm_; 60 | double l_foot_fx_scaled_N_, l_foot_fy_scaled_N_, l_foot_fz_scaled_N_; 61 | double l_foot_tx_scaled_Nm_, l_foot_ty_scaled_Nm_, l_foot_tz_scaled_Nm_; 62 | 63 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 64 | void process(std::map dxls, std::map sensors); 65 | 66 | private: 67 | void queueThread(); 68 | 69 | void initializeFeetForceTorqueSensor(); 70 | void saveFTCalibrationData(const std::string &path); 71 | 72 | void ftSensorCalibrationCommandCallback(const std_msgs::String::ConstPtr& msg); 73 | void publishStatusMsg(unsigned int type, std::string msg); 74 | void publishBothFTData(int type, Eigen::MatrixXd &ft_right, Eigen::MatrixXd &ft_left); 75 | 76 | 77 | int control_cycle_msec_; 78 | boost::thread queue_thread_; 79 | boost::mutex publish_mutex_; 80 | 81 | 82 | KinematicsDynamics* thormang3_kd_; 83 | 84 | 85 | bool exist_r_leg_an_r_, exist_r_leg_an_p_; 86 | bool exist_l_leg_an_r_, exist_l_leg_an_p_; 87 | 88 | ATIForceTorqueSensorTWE r_foot_ft_sensor_; 89 | ATIForceTorqueSensorTWE l_foot_ft_sensor_; 90 | 91 | Eigen::MatrixXd r_foot_ft_air_, l_foot_ft_air_; 92 | Eigen::MatrixXd r_foot_ft_gnd_, l_foot_ft_gnd_; 93 | 94 | double r_foot_ft_current_voltage_[6]; 95 | double l_foot_ft_current_voltage_[6]; 96 | 97 | 98 | double total_mass_; 99 | double r_foot_ft_scale_factor_, l_foot_ft_scale_factor_; 100 | 101 | bool has_ft_air_; 102 | bool has_ft_gnd_; 103 | int ft_command_; 104 | int ft_period_; 105 | int ft_get_count_; 106 | 107 | const int FT_NONE; 108 | const int FT_AIR; 109 | const int FT_GND; 110 | const int FT_CALC; 111 | 112 | 113 | ros::Publisher thormang3_foot_ft_status_pub_; 114 | ros::Publisher thormang3_foot_ft_both_ft_pub_; 115 | }; 116 | 117 | 118 | } 119 | 120 | 121 | #endif /* THORMANG3_FEET_FT_MODULE_FEET_FORCE_TORQUE_SENSOR_MODULE_H_ */ 122 | -------------------------------------------------------------------------------- /thormang3_feet_ft_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_feet_ft_module) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | std_msgs 16 | robotis_controller_msgs 17 | thormang3_feet_ft_module_msgs 18 | cmake_modules 19 | robotis_framework_common 20 | robotis_math 21 | thormang3_kinematics_dynamics 22 | ati_ft_sensor 23 | ) 24 | 25 | find_package(Boost REQUIRED COMPONENTS thread) 26 | find_package(Eigen3 REQUIRED) 27 | 28 | ## Resolve system dependency on yaml-cpp, which apparently does not 29 | ## provide a CMake find_package() module. 30 | ## Insert your header file compatible specified path like '#include ' 31 | find_package(PkgConfig REQUIRED) 32 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 33 | find_path(YAML_CPP_INCLUDE_DIR 34 | NAMES yaml_cpp.h 35 | PATHS ${YAML_CPP_INCLUDE_DIRS} 36 | ) 37 | find_library(YAML_CPP_LIBRARY 38 | NAMES YAML_CPP 39 | PATHS ${YAML_CPP_LIBRARY_DIRS} 40 | ) 41 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 42 | 43 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 44 | add_definitions(-DHAVE_NEW_YAMLCPP) 45 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 46 | 47 | ################################################################################ 48 | # Setup for python modules and scripts 49 | ################################################################################ 50 | 51 | ################################################################################ 52 | # Declare ROS messages, services and actions 53 | ################################################################################ 54 | 55 | ################################################################################ 56 | # Declare ROS dynamic reconfigure parameters 57 | ################################################################################ 58 | 59 | ################################################################################ 60 | # Declare catkin specific configuration to be passed to dependent projects 61 | ################################################################################ 62 | catkin_package( 63 | INCLUDE_DIRS include 64 | LIBRARIES ${PROJECT_NAME} 65 | CATKIN_DEPENDS 66 | roscpp 67 | std_msgs 68 | robotis_controller_msgs 69 | thormang3_feet_ft_module_msgs 70 | cmake_modules 71 | robotis_framework_common 72 | robotis_math 73 | thormang3_kinematics_dynamics 74 | ati_ft_sensor 75 | DEPENDS Boost EIGEN3 76 | ) 77 | 78 | ################################################################################ 79 | # Build 80 | ################################################################################ 81 | include_directories( 82 | include 83 | ${catkin_INCLUDE_DIRS} 84 | ${Boost_INCLUDE_DIRS} 85 | ${EIGEN3_INCLUDE_DIRS} 86 | ${YAML_CPP_INCLUDE_DIRS} 87 | ) 88 | 89 | add_library(${PROJECT_NAME} src/feet_force_torque_sensor_module.cpp) 90 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 91 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${YAML_CPP_LIBRARIES}) 92 | 93 | ################################################################################ 94 | # Install 95 | ################################################################################ 96 | install(TARGETS ${PROJECT_NAME} 97 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 98 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 99 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 100 | ) 101 | 102 | install(DIRECTORY include/${PROJECT_NAME}/ 103 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 104 | ) 105 | 106 | ################################################################################ 107 | # Test 108 | ################################################################################ 109 | -------------------------------------------------------------------------------- /thormang3_base_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_base_module) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | roslib 16 | std_msgs 17 | geometry_msgs 18 | robotis_controller_msgs 19 | cmake_modules 20 | robotis_framework_common 21 | robotis_math 22 | thormang3_kinematics_dynamics 23 | ) 24 | 25 | find_package(Boost REQUIRED COMPONENTS thread) 26 | find_package(Eigen3 REQUIRED) 27 | 28 | ## Resolve system dependency on yaml-cpp, which apparently does not 29 | ## provide a CMake find_package() module. 30 | ## Insert your header file compatible specified path like '#include ' 31 | find_package(PkgConfig REQUIRED) 32 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 33 | find_path(YAML_CPP_INCLUDE_DIR 34 | NAMES yaml_cpp.h 35 | PATHS ${YAML_CPP_INCLUDE_DIRS} 36 | ) 37 | find_library(YAML_CPP_LIBRARY 38 | NAMES YAML_CPP 39 | PATHS ${YAML_CPP_LIBRARY_DIRS} 40 | ) 41 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 42 | 43 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 44 | add_definitions(-DHAVE_NEW_YAMLCPP) 45 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 46 | 47 | ################################################################################ 48 | # Setup for python modules and scripts 49 | ################################################################################ 50 | 51 | ################################################################################ 52 | # Declare ROS messages, services and actions 53 | ################################################################################ 54 | 55 | ################################################################################ 56 | # Declare ROS dynamic reconfigure parameters 57 | ################################################################################ 58 | 59 | ################################################################################ 60 | # Declare catkin specific configuration to be passed to dependent projects 61 | ################################################################################ 62 | catkin_package( 63 | INCLUDE_DIRS include 64 | LIBRARIES ${PROJECT_NAME} 65 | CATKIN_DEPENDS 66 | roscpp 67 | roslib 68 | std_msgs 69 | geometry_msgs 70 | robotis_controller_msgs 71 | cmake_modules 72 | robotis_framework_common 73 | robotis_math 74 | thormang3_kinematics_dynamics 75 | DEPENDS Boost EIGEN3 76 | ) 77 | 78 | ################################################################################ 79 | # Build 80 | ################################################################################ 81 | include_directories( 82 | include 83 | ${catkin_INCLUDE_DIRS} 84 | ${Boost_INCLUDE_DIRS} 85 | ${EIGEN3_INCLUDE_DIRS} 86 | ${YAML_CPP_INCLUDE_DIRS} 87 | ) 88 | 89 | add_library(${PROJECT_NAME} src/base_module.cpp src/base_module_state.cpp) 90 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 91 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${YAML_CPP_LIBRARIES}) 92 | 93 | ################################################################################ 94 | # Install 95 | ################################################################################ 96 | install(TARGETS ${PROJECT_NAME} 97 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 98 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 99 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 100 | ) 101 | 102 | install(DIRECTORY include/${PROJECT_NAME}/ 103 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 104 | ) 105 | 106 | install(DIRECTORY data 107 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 108 | ) 109 | 110 | ################################################################################ 111 | # Test 112 | ################################################################################ 113 | -------------------------------------------------------------------------------- /thormang3_manipulation_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_manipulation_module) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | roslib 16 | std_msgs 17 | geometry_msgs 18 | robotis_controller_msgs 19 | thormang3_manipulation_module_msgs 20 | cmake_modules 21 | robotis_framework_common 22 | robotis_math 23 | thormang3_kinematics_dynamics 24 | ) 25 | 26 | find_package(Boost REQUIRED COMPONENTS thread) 27 | find_package(Eigen3 REQUIRED) 28 | 29 | ## Resolve system dependency on yaml-cpp, which apparently does not 30 | ## provide a CMake find_package() module. 31 | ## Insert your header file compatible specified path like '#include ' 32 | find_package(PkgConfig REQUIRED) 33 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 34 | find_path(YAML_CPP_INCLUDE_DIR 35 | NAMES yaml_cpp.h 36 | PATHS ${YAML_CPP_INCLUDE_DIRS} 37 | ) 38 | find_library(YAML_CPP_LIBRARY 39 | NAMES YAML_CPP 40 | PATHS ${YAML_CPP_LIBRARY_DIRS} 41 | ) 42 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 43 | 44 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 45 | add_definitions(-DHAVE_NEW_YAMLCPP) 46 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 47 | 48 | ################################################################################ 49 | # Setup for python modules and scripts 50 | ################################################################################ 51 | 52 | ################################################################################ 53 | # Declare ROS messages, services and actions 54 | ################################################################################ 55 | 56 | ################################################################################ 57 | # Declare ROS dynamic reconfigure parameters 58 | ################################################################################ 59 | 60 | ################################################################################ 61 | # Declare catkin specific configuration to be passed to dependent projects 62 | ################################################################################# 63 | catkin_package( 64 | INCLUDE_DIRS include 65 | LIBRARIES ${PROJECT_NAME} 66 | CATKIN_DEPENDS 67 | roscpp 68 | roslib 69 | std_msgs 70 | geometry_msgs 71 | robotis_controller_msgs 72 | thormang3_manipulation_module_msgs 73 | cmake_modules 74 | robotis_framework_common 75 | robotis_math 76 | thormang3_kinematics_dynamics 77 | DEPENDS Boost EIGEN3 78 | ) 79 | 80 | ################################################################################ 81 | # Build 82 | ################################################################################ 83 | include_directories( 84 | include 85 | ${catkin_INCLUDE_DIRS} 86 | ${Boost_INCLUDE_DIRS} 87 | ${EIGEN3_INCLUDE_DIRS} 88 | ${YAML_CPP_INCLUDE_DIRS} 89 | ) 90 | 91 | add_library(${PROJECT_NAME} src/manipulation_module.cpp) 92 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 93 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${YAML_CPP_LIBRARIES}) 94 | 95 | ################################################################################ 96 | # Install 97 | ################################################################################ 98 | install(TARGETS ${PROJECT_NAME} 99 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 100 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 101 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 102 | ) 103 | 104 | install(DIRECTORY include/${PROJECT_NAME}/ 105 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 106 | ) 107 | 108 | install(DIRECTORY config 109 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 110 | ) 111 | 112 | ################################################################################ 113 | # Test 114 | ################################################################################ 115 | -------------------------------------------------------------------------------- /thormang3_head_control_module/include/thormang3_head_control_module/head_control_module.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * HeadControlModule.h 19 | * 20 | * Created on: 2016. 1. 27. 21 | * Author: Kayman 22 | */ 23 | 24 | #ifndef THORMANG3_HEAD_CONTROL_MODULE_HEAD_CONTROL_MODULE_H_ 25 | #define THORMANG3_HEAD_CONTROL_MODULE_HEAD_CONTROL_MODULE_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "robotis_controller_msgs/StatusMsg.h" 37 | 38 | #include "robotis_framework_common/motion_module.h" 39 | #include "robotis_math/robotis_math.h" 40 | #include "thormang3_head_control_module_msgs/HeadJointPose.h" 41 | 42 | namespace thormang3 43 | { 44 | 45 | class HeadControlModule : public robotis_framework::MotionModule, public robotis_framework::Singleton 46 | { 47 | public: 48 | HeadControlModule(); 49 | virtual ~HeadControlModule(); 50 | 51 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 52 | void process(std::map dxls, std::map sensors); 53 | 54 | void stop(); 55 | bool isRunning(); 56 | 57 | private: 58 | const double SCAN_START_ANGLE = -10 * M_PI / 180; 59 | const double SCAN_END_ANGLE = 85 * M_PI / 180; 60 | 61 | /* ROS Topic Callback Functions */ 62 | void get3DLidarCallback(const std_msgs::String::ConstPtr &msg); 63 | void get3DLidarRangeCallback(const std_msgs::Float64::ConstPtr &msg); 64 | void setHeadJointCallback(const sensor_msgs::JointState::ConstPtr &msg); 65 | void setHeadJointTimeCallback(const thormang3_head_control_module_msgs::HeadJointPose::ConstPtr &msg); 66 | 67 | void queueThread(); 68 | void jointTraGeneThread(); 69 | void lidarJointTraGeneThread(); 70 | 71 | void beforeMoveLidar(double start_angle); 72 | void startMoveLidar(double target_angle); 73 | void afterMoveLidar(); 74 | void publishLidarMoveMsg(std::string msg_data); 75 | void publishDoneMsg(const std::string done_msg); 76 | 77 | void startMoving(); 78 | void finishMoving(); 79 | void stopMoving(); 80 | 81 | void publishStatusMsg(unsigned int type, std::string msg); 82 | 83 | Eigen::MatrixXd calcMinimumJerkTraPVA(double pos_start, double vel_start, double accel_start, double pos_end, 84 | double vel_end, double accel_end, double smp_time, double mov_time); 85 | 86 | Eigen::MatrixXd calcLinearInterpolationTra(double pos_start, double pos_end, double smp_time, double mov_time); 87 | 88 | int control_cycle_msec_; 89 | boost::thread queue_thread_; 90 | boost::thread *tra_gene_thread_; 91 | boost::mutex tra_lock_; 92 | ros::Publisher moving_head_pub_; 93 | ros::Publisher status_msg_pub_; 94 | ros::Publisher movement_done_pub_; 95 | const bool DEBUG; 96 | bool stop_process_; 97 | bool is_moving_; 98 | bool is_direct_control_; 99 | int tra_count_, tra_size_; 100 | double moving_time_; 101 | int current_state_; 102 | double original_position_lidar_; 103 | double scan_range_; 104 | 105 | Eigen::MatrixXd target_position_; 106 | Eigen::MatrixXd current_position_; 107 | Eigen::MatrixXd goal_position_; 108 | Eigen::MatrixXd goal_velocity_; 109 | Eigen::MatrixXd goal_acceleration_; 110 | Eigen::MatrixXd calc_joint_tra_; 111 | Eigen::MatrixXd calc_joint_vel_tra_; 112 | Eigen::MatrixXd calc_joint_accel_tra_; 113 | 114 | std::map using_joint_name_; 115 | 116 | enum HeadLidarMode 117 | { 118 | None, 119 | BeforeStart, 120 | StartMove, 121 | EndMove, 122 | AfterMove, 123 | ModeCount 124 | }; 125 | }; 126 | } 127 | 128 | #endif /* THORMANG3_HEAD_CONTROL_MODULE_HEAD_CONTROL_MODULE_H_ */ 129 | -------------------------------------------------------------------------------- /thormang3_walking_module/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # Set minimum required version of cmake, project name and compile options 3 | ################################################################################ 4 | cmake_minimum_required(VERSION 2.8.3) 5 | project(thormang3_walking_module) 6 | 7 | ## Compile as C++11, supported in ROS Kinetic and newer 8 | add_compile_options(-std=c++11) 9 | 10 | ################################################################################ 11 | # Find catkin packages and libraries for catkin and system dependencies 12 | ################################################################################ 13 | find_package(catkin REQUIRED COMPONENTS 14 | roscpp 15 | roslib 16 | std_msgs 17 | sensor_msgs 18 | geometry_msgs 19 | robotis_controller_msgs 20 | thormang3_walking_module_msgs 21 | cmake_modules 22 | eigen_conversions 23 | robotis_framework_common 24 | robotis_math 25 | thormang3_kinematics_dynamics 26 | thormang3_balance_control 27 | ) 28 | 29 | find_package(Boost REQUIRED COMPONENTS thread) 30 | find_package(Eigen3 REQUIRED) 31 | 32 | ## Resolve system dependency on yaml-cpp, which apparently does not 33 | ## provide a CMake find_package() module. 34 | ## Insert your header file compatible specified path like '#include ' 35 | find_package(PkgConfig REQUIRED) 36 | pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) 37 | find_path(YAML_CPP_INCLUDE_DIR 38 | NAMES yaml_cpp.h 39 | PATHS ${YAML_CPP_INCLUDE_DIRS} 40 | ) 41 | find_library(YAML_CPP_LIBRARY 42 | NAMES YAML_CPP 43 | PATHS ${YAML_CPP_LIBRARY_DIRS} 44 | ) 45 | link_directories(${YAML_CPP_LIBRARY_DIRS}) 46 | 47 | if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 48 | add_definitions(-DHAVE_NEW_YAMLCPP) 49 | endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") 50 | 51 | ################################################################################ 52 | # Setup for python modules and scripts 53 | ################################################################################ 54 | 55 | ################################################################################ 56 | # Declare ROS messages, services and actions 57 | ################################################################################ 58 | 59 | ################################################################################ 60 | # Declare ROS dynamic reconfigure parameters 61 | ################################################################################ 62 | 63 | ################################################################################ 64 | # Declare catkin specific configuration to be passed to dependent projects 65 | ################################################################################ 66 | catkin_package( 67 | INCLUDE_DIRS include 68 | LIBRARIES ${PROJECT_NAME} 69 | CATKIN_DEPENDS 70 | roscpp 71 | roslib 72 | std_msgs 73 | sensor_msgs 74 | geometry_msgs 75 | robotis_controller_msgs 76 | thormang3_walking_module_msgs 77 | cmake_modules 78 | eigen_conversions 79 | robotis_framework_common 80 | robotis_math 81 | thormang3_kinematics_dynamics 82 | thormang3_balance_control 83 | DEPENDS Boost EIGEN3 84 | ) 85 | 86 | ################################################################################ 87 | # Build 88 | ################################################################################ 89 | include_directories( 90 | include 91 | ${catkin_INCLUDE_DIRS} 92 | ${Boost_INCLUDE_DIRS} 93 | ${EIGEN3_INCLUDE_DIRS} 94 | ${YAML_CPP_INCLUDE_DIRS} 95 | ) 96 | 97 | add_library(${PROJECT_NAME} src/thormang3_online_walking.cpp src/walking_module.cpp) 98 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 99 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${YAML_CPP_LIBRARIES}) 100 | 101 | ################################################################################ 102 | # Install 103 | ################################################################################ 104 | install(TARGETS ${PROJECT_NAME} 105 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 106 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 107 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 108 | ) 109 | 110 | install(DIRECTORY include/${PROJECT_NAME}/ 111 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 112 | ) 113 | 114 | install(DIRECTORY config 115 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 116 | ) 117 | 118 | ################################################################################ 119 | # Test 120 | ################################################################################ 121 | -------------------------------------------------------------------------------- /thormang3_action_module/include/thormang3_action_module/action_module.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * action_module.h 19 | * 20 | * Created on: 2016. 2. 23. 21 | * Author: Jay Song 22 | */ 23 | 24 | #ifndef THORMANG3_ACTION_MODULE_ACTION_MODULE_H_ 25 | #define THORMANG3_ACTION_MODULE_ACTION_MODULE_H_ 26 | 27 | #define _USE_MATH_DEFINES 28 | 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "robotis_controller_msgs/StatusMsg.h" 39 | #include "thormang3_action_module_msgs/IsRunning.h" 40 | #include "thormang3_action_module_msgs/StartAction.h" 41 | 42 | #include "robotis_framework_common/motion_module.h" 43 | 44 | #include "action_file_define.h" 45 | 46 | namespace thormang3 47 | { 48 | 49 | class ActionModule : public robotis_framework::MotionModule, public robotis_framework::Singleton 50 | { 51 | public: 52 | ActionModule(); 53 | virtual ~ActionModule(); 54 | 55 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 56 | void process(std::map dxls, std::map sensors); 57 | 58 | void stop(); 59 | bool isRunning(); 60 | 61 | bool loadFile(std::string file_name); 62 | bool createFile(std::string file_name); 63 | 64 | bool start(int page_number); 65 | bool start(std::string page_name); 66 | bool start(int page_number, action_file_define::Page* page); 67 | 68 | void onModuleEnable(); 69 | void onModuleDisable(); 70 | 71 | void brake(); 72 | bool isRunning(int* playing_page_num, int* playing_step_num); 73 | bool loadPage(int page_number, action_file_define::Page* page); 74 | bool savePage(int page_number, action_file_define::Page* page); 75 | void resetPage(action_file_define::Page* page); 76 | 77 | void enableAllJoints(); 78 | void actionPlayProcess(std::map dxls); 79 | 80 | private: 81 | void queueThread(); 82 | 83 | bool verifyChecksum( action_file_define::Page* page ); 84 | void setChecksum( action_file_define::Page* page ); 85 | 86 | void publishStatusMsg(unsigned int type, std::string msg); 87 | void publishDoneMsg(std::string msg); 88 | 89 | bool isRunningServiceCallback(thormang3_action_module_msgs::IsRunning::Request &req, 90 | thormang3_action_module_msgs::IsRunning::Response &res); 91 | 92 | void pageNumberCallback(const std_msgs::Int32::ConstPtr& msg); 93 | void startActionCallback(const thormang3_action_module_msgs::StartAction::ConstPtr& msg); 94 | 95 | int convertRadTow4095(double rad); 96 | double convertw4095ToRad(int w4095); 97 | std::string convertIntToString(int n); 98 | 99 | std::map action_joints_enable_; 100 | std::map action_result_; 101 | int control_cycle_msec_; 102 | boost::thread queue_thread_; 103 | 104 | ros::Publisher status_msg_pub_; 105 | ros::Publisher done_msg_pub_; 106 | 107 | std::map joint_name_to_id_; 108 | std::map joint_id_to_name_; 109 | FILE* action_file_; 110 | action_file_define::Page play_page_; 111 | action_file_define::Page next_play_page_; 112 | action_file_define::Step current_step_; 113 | 114 | int play_page_idx_; 115 | bool first_driving_start_; 116 | int page_step_count_; 117 | 118 | bool playing_; 119 | bool stop_playing_; 120 | bool playing_finished_; 121 | 122 | bool action_module_enabled_; 123 | bool previous_running_; 124 | bool present_running_; 125 | 126 | const int PRE_SECTION; 127 | const int MAIN_SECTION; 128 | const int POST_SECTION; 129 | const int PAUSE_SECTION; 130 | const int ZERO_FINISH; 131 | const int NONE_ZERO_FINISH; 132 | }; 133 | 134 | } 135 | 136 | #endif /* THORMANG3_ACTION_MODULE_ACTION_MODULE_H_ */ 137 | -------------------------------------------------------------------------------- /thormang3_manipulation_module/include/thormang3_manipulation_module/manipulation_module.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * manipulation_module.h 19 | * 20 | * Created on: June 7, 2016 21 | * Author: SCH 22 | */ 23 | 24 | #ifndef THORMANG3_MANIPULATION_MODULE_MANIPULATION_MODULE_H_ 25 | #define THORMANG3_MANIPULATION_MODULE_MANIPULATION_MODULE_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include "robotis_controller_msgs/JointCtrlModule.h" 40 | #include "robotis_controller_msgs/StatusMsg.h" 41 | 42 | #include "thormang3_manipulation_module_msgs/JointPose.h" 43 | #include "thormang3_manipulation_module_msgs/KinematicsPose.h" 44 | #include "thormang3_manipulation_module_msgs/GetJointPose.h" 45 | #include "thormang3_manipulation_module_msgs/GetKinematicsPose.h" 46 | 47 | #include "robotis_framework_common/motion_module.h" 48 | #include "robotis_math/robotis_math.h" 49 | 50 | #include "thormang3_kinematics_dynamics/kinematics_dynamics.h" 51 | 52 | 53 | namespace thormang3 54 | { 55 | 56 | class ManipulationModule: public robotis_framework::MotionModule, 57 | public robotis_framework::Singleton 58 | { 59 | public: 60 | ManipulationModule(); 61 | virtual ~ManipulationModule(); 62 | 63 | /* ROS Topic Callback Functions */ 64 | void initPoseMsgCallback(const std_msgs::String::ConstPtr& msg); 65 | void jointPoseMsgCallback(const thormang3_manipulation_module_msgs::JointPose::ConstPtr& msg); 66 | void kinematicsPoseMsgCallback(const thormang3_manipulation_module_msgs::KinematicsPose::ConstPtr& msg); 67 | 68 | bool getJointPoseCallback(thormang3_manipulation_module_msgs::GetJointPose::Request &req, 69 | thormang3_manipulation_module_msgs::GetJointPose::Response &res); 70 | bool getKinematicsPoseCallback(thormang3_manipulation_module_msgs::GetKinematicsPose::Request &req, 71 | thormang3_manipulation_module_msgs::GetKinematicsPose::Response &res); 72 | 73 | /* ROS Calculation Functions */ 74 | void initPoseTrajGenerateProc(); 75 | void jointTrajGenerateProc(); 76 | void taskTrajGenerateProc(); 77 | 78 | /* ROS Framework Functions */ 79 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 80 | void process(std::map dxls, std::map sensors); 81 | void stop(); 82 | bool isRunning(); 83 | 84 | void publishStatusMsg(unsigned int type, std::string msg); 85 | 86 | /* Parameter */ 87 | KinematicsDynamics *robotis_; 88 | 89 | private: 90 | void queueThread(); 91 | 92 | void parseData(const std::string &path); 93 | void parseIniPoseData(const std::string &path); 94 | 95 | bool arm_angle_display_; 96 | 97 | double control_cycle_sec_; 98 | boost::thread queue_thread_; 99 | boost::thread *traj_generate_tread_; 100 | 101 | std_msgs::String movement_done_msg_; 102 | 103 | ros::Publisher status_msg_pub_; 104 | ros::Publisher movement_done_pub_; 105 | 106 | /* joint state */ 107 | Eigen::VectorXd present_joint_position_; 108 | Eigen::VectorXd goal_joint_position_; 109 | Eigen::VectorXd init_joint_position_; 110 | 111 | /* trajectory */ 112 | bool is_moving_; 113 | double mov_time_; 114 | int cnt_; 115 | int all_time_steps_; 116 | 117 | Eigen::MatrixXd goal_joint_tra_; 118 | Eigen::MatrixXd goal_task_tra_; 119 | 120 | /* msgs */ 121 | thormang3_manipulation_module_msgs::JointPose goal_joint_pose_msg_; 122 | thormang3_manipulation_module_msgs::KinematicsPose goal_kinematics_pose_msg_; 123 | 124 | /* inverse kinematics */ 125 | bool ik_solving_; 126 | int ik_id_start_; 127 | int ik_id_end_; 128 | 129 | Eigen::MatrixXd ik_target_position_; 130 | Eigen::MatrixXd ik_start_rotation_; 131 | Eigen::MatrixXd ik_target_rotation_; 132 | Eigen::MatrixXd ik_weight_; 133 | 134 | void setInverseKinematics(int cnt, Eigen::MatrixXd start_rotation); 135 | 136 | std::map joint_name_to_id_; 137 | }; 138 | 139 | } 140 | 141 | #endif /* THORMANG3_MANIPULATION_MODULE_MANIPULATION_MODULE_H_ */ 142 | -------------------------------------------------------------------------------- /thormang3_walking_module/include/thormang3_walking_module/walking_module.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * online_walking_module.h 19 | * 20 | * Created on: 2016. 9. 30. 21 | * Author: Jay Song 22 | */ 23 | 24 | #ifndef THORMANG3_ONLINE_WALKING_MODULE_ONLINE_WALKING_MODULE_H_ 25 | #define THORMANG3_ONLINE_WALKING_MODULE_ONLINE_WALKING_MODULE_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "thormang3_walking_module/thormang3_online_walking.h" 36 | 37 | #include "robotis_controller_msgs/StatusMsg.h" 38 | #include "thormang3_walking_module_msgs/RobotPose.h" 39 | #include "thormang3_walking_module_msgs/GetReferenceStepData.h" 40 | #include "thormang3_walking_module_msgs/AddStepDataArray.h" 41 | #include "thormang3_walking_module_msgs/StartWalking.h" 42 | #include "thormang3_walking_module_msgs/IsRunning.h" 43 | #include "thormang3_walking_module_msgs/RemoveExistingStepData.h" 44 | #include "thormang3_walking_module_msgs/SetBalanceParam.h" 45 | #include "thormang3_walking_module_msgs/SetJointFeedBackGain.h" 46 | 47 | #include "robotis_framework_common/motion_module.h" 48 | 49 | #define WALKING_TUNE 50 | #ifdef WALKING_TUNE 51 | #include "thormang3_walking_module_msgs/WalkingJointStatesStamped.h" 52 | #endif 53 | 54 | namespace thormang3 55 | { 56 | 57 | class OnlineWalkingModule : public robotis_framework::MotionModule, public robotis_framework::Singleton 58 | { 59 | public: 60 | OnlineWalkingModule(); 61 | virtual ~OnlineWalkingModule(); 62 | 63 | void initialize(const int control_cycle_msec, robotis_framework::Robot *robot); 64 | void process(std::map dxls, std::map sensors); 65 | 66 | void onModuleEnable(); 67 | void onModuleDisable(); 68 | 69 | void stop(); 70 | bool isRunning(); 71 | 72 | double gyro_roll_, gyro_pitch_; 73 | double orientation_roll_, orientation_pitch_; 74 | double r_foot_fx_N_, r_foot_fy_N_, r_foot_fz_N_; 75 | double r_foot_Tx_Nm_, r_foot_Ty_Nm_, r_foot_Tz_Nm_; 76 | double l_foot_fx_N_, l_foot_fy_N_, l_foot_fz_N_; 77 | double l_foot_Tx_Nm_, l_foot_Ty_Nm_, l_foot_Tz_Nm_; 78 | 79 | private: 80 | void publishRobotPose(void); 81 | void publishStatusMsg(unsigned int type, std::string msg); 82 | void publishDoneMsg(std::string msg); 83 | #ifdef WALKING_TUNE 84 | void publishWalkingTuningData(); 85 | #endif 86 | 87 | /* ROS Topic Callback Functions */ 88 | void imuDataOutputCallback(const sensor_msgs::Imu::ConstPtr &msg); 89 | 90 | /* ROS Service Callback Functions */ 91 | bool setBalanceParamServiceCallback(thormang3_walking_module_msgs::SetBalanceParam::Request &req, 92 | thormang3_walking_module_msgs::SetBalanceParam::Response &res); 93 | 94 | bool setJointFeedBackGainServiceCallback(thormang3_walking_module_msgs::SetJointFeedBackGain::Request &req, 95 | thormang3_walking_module_msgs::SetJointFeedBackGain::Response &res); 96 | 97 | bool getReferenceStepDataServiceCallback(thormang3_walking_module_msgs::GetReferenceStepData::Request &req, 98 | thormang3_walking_module_msgs::GetReferenceStepData::Response &res); 99 | bool addStepDataServiceCallback(thormang3_walking_module_msgs::AddStepDataArray::Request &req, 100 | thormang3_walking_module_msgs::AddStepDataArray::Response &res); 101 | bool startWalkingServiceCallback(thormang3_walking_module_msgs::StartWalking::Request &req, 102 | thormang3_walking_module_msgs::StartWalking::Response &res); 103 | bool IsRunningServiceCallback(thormang3_walking_module_msgs::IsRunning::Request &req, 104 | thormang3_walking_module_msgs::IsRunning::Response &res); 105 | bool removeExistingStepDataServiceCallback(thormang3_walking_module_msgs::RemoveExistingStepData::Request &req, 106 | thormang3_walking_module_msgs::RemoveExistingStepData::Response &res); 107 | 108 | int convertStepDataMsgToStepData(thormang3_walking_module_msgs::StepData& src, robotis_framework::StepData& des); 109 | int convertStepDataToStepDataMsg(robotis_framework::StepData& src, thormang3_walking_module_msgs::StepData& des); 110 | 111 | void setBalanceParam(thormang3_walking_module_msgs::BalanceParam& balance_param_msg); 112 | 113 | void updateBalanceParam(); 114 | 115 | bool checkBalanceOnOff(); 116 | 117 | void queueThread(); 118 | 119 | void setJointFeedBackGain(thormang3_walking_module_msgs::JointFeedBackGain& msg); 120 | void updateJointFeedBackGain(); 121 | 122 | std::map joint_name_to_index_; 123 | 124 | bool gazebo_; 125 | int control_cycle_msec_; 126 | boost::thread queue_thread_; 127 | boost::mutex process_mutex_; 128 | 129 | Eigen::MatrixXd rot_x_pi_3d_, rot_z_pi_3d_; 130 | Eigen::MatrixXd desired_matrix_g_to_cob_; 131 | Eigen::MatrixXd desired_matrix_g_to_rfoot_; 132 | Eigen::MatrixXd desired_matrix_g_to_lfoot_; 133 | 134 | bool previous_running_, present_running; 135 | 136 | /* ROS Topic Publish Functions */ 137 | int r_foot_ft_publish_checker_; 138 | int l_foot_ft_publish_checker_; 139 | ros::Publisher robot_pose_pub_; 140 | ros::Publisher status_msg_pub_; 141 | ros::Publisher pelvis_base_msg_pub_; 142 | ros::Publisher done_msg_pub_; 143 | #ifdef WALKING_TUNE 144 | ros::Publisher walking_joint_states_pub_; 145 | ros::Publisher imu_orientation_states_pub_; 146 | ros::Publisher ft_states_pub_; 147 | thormang3_walking_module_msgs::WalkingJointStatesStamped walking_joint_states_msg_; 148 | #endif 149 | 150 | thormang3_walking_module_msgs::RobotPose robot_pose_msg_; 151 | bool balance_update_with_loop_; 152 | double balance_update_duration_; 153 | double balance_update_sys_time_; 154 | Eigen::MatrixXd balance_update_polynomial_coeff_; 155 | 156 | 157 | bool joint_feedback_update_with_loop_; 158 | double joint_feedback_update_duration_; 159 | double joint_feedback_update_sys_time_; 160 | Eigen::MatrixXd joint_feedback_update_polynomial_coeff_; 161 | 162 | thormang3_walking_module_msgs::JointFeedBackGain previous_joint_feedback_gain_; 163 | thormang3_walking_module_msgs::JointFeedBackGain current_joint_feedback_gain_; 164 | thormang3_walking_module_msgs::JointFeedBackGain desired_joint_feedback_gain_; 165 | 166 | thormang3_walking_module_msgs::BalanceParam previous_balance_param_; 167 | thormang3_walking_module_msgs::BalanceParam current_balance_param_; 168 | thormang3_walking_module_msgs::BalanceParam desired_balance_param_; 169 | 170 | }; 171 | 172 | } 173 | 174 | #endif /* THORMANG3_ONLINE_WALKING_MODULE_ONLINE_WALKING_MODULE_H_ */ 175 | -------------------------------------------------------------------------------- /ati_ft_sensor/src/ati_force_torque_sensor_twe.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #include "ati_ft_sensor/ati_force_torque_sensor_twe.h" 18 | 19 | using namespace thormang3; 20 | 21 | ATIForceTorqueSensorTWE::ATIForceTorqueSensorTWE() 22 | { 23 | ft_coeff_mat_ = Eigen::MatrixXd::Zero(6,6); 24 | ft_unload_volatge_.resize(6, 1); 25 | ft_unload_volatge_.fill(3.3*0.5); 26 | 27 | ft_current_volatge_.resize(6, 1); 28 | ft_current_volatge_.fill(3.3*0.5); 29 | 30 | ft_null_.resize(6, 1); 31 | ft_null_.fill(0); 32 | ft_raw_.resize(6, 1); 33 | ft_raw_.fill(0); 34 | ft_scaled_.resize(6, 1); 35 | ft_scaled_.fill(0); 36 | 37 | ft_scale_factor_ = 1.0; 38 | 39 | ft_frame_id_ = ""; 40 | ft_raw_publish_name_ =""; 41 | ft_scaled_publish_name_ =""; 42 | 43 | is_ft_raw_published_ = false; 44 | is_ft_scaled_published_ = false; 45 | } 46 | 47 | 48 | ATIForceTorqueSensorTWE::~ATIForceTorqueSensorTWE() 49 | { 50 | 51 | } 52 | 53 | 54 | bool ATIForceTorqueSensorTWE::initialize(const std::string& ft_data_path, const std::string& ft_data_key, 55 | const std::string& ft_frame_id, 56 | const std::string& ft_raw_publish_name, const std::string& ft_scaled_publish_name) 57 | { 58 | ft_frame_id_ = ft_frame_id; 59 | ft_raw_publish_name_ = ft_raw_publish_name; 60 | ft_scaled_publish_name_ = ft_scaled_publish_name; 61 | 62 | ros::NodeHandle _nh; 63 | 64 | ft_raw_msg_.header.frame_id = ft_frame_id_; 65 | ft_scaled_msg_.header.frame_id = ft_frame_id_; 66 | 67 | if(ft_raw_publish_name_ != "") { 68 | ft_raw_pub_ = _nh.advertise(ft_raw_publish_name_, 1); 69 | is_ft_raw_published_ = true; 70 | } 71 | 72 | if(ft_scaled_publish_name_ != "") { 73 | ft_scaled_pub_ = _nh.advertise(ft_scaled_publish_name_, 1); 74 | is_ft_scaled_published_ = true; 75 | } 76 | 77 | return parseFTData(ft_data_path, ft_data_key);; 78 | } 79 | 80 | void ATIForceTorqueSensorTWE::setScaleFactror(double ft_scale_factor) 81 | { 82 | ft_scale_param_mutex_.lock(); 83 | ft_scale_factor_ = ft_scale_factor; 84 | ft_scale_param_mutex_.unlock(); 85 | } 86 | 87 | void ATIForceTorqueSensorTWE::setNullForceTorque(Eigen::MatrixXd ft_null) 88 | { 89 | if( (ft_null.rows() != 6) || (ft_null.cols() != 1) ){ 90 | ROS_ERROR("Invalid ft null size"); 91 | return; 92 | } 93 | 94 | ft_scale_param_mutex_.lock(); 95 | ft_null_ = ft_null; 96 | ft_scale_param_mutex_.unlock(); 97 | } 98 | 99 | void ATIForceTorqueSensorTWE::setScaleParam(double ft_scale_factor, Eigen::MatrixXd ft_null) 100 | { 101 | setScaleFactror(ft_scale_factor); 102 | setNullForceTorque(ft_null); 103 | } 104 | 105 | bool ATIForceTorqueSensorTWE::parseFTData(const std::string& ft_data_path, const std::string& ft_data_key) 106 | { 107 | std::string _ft_mat_key = ft_data_key + "_calibration_matrix"; 108 | std::string _ft_unload_key = ft_data_key + "_unload"; 109 | 110 | YAML::Node doc; 111 | 112 | doc = YAML::LoadFile(ft_data_path.c_str()); 113 | 114 | std::vector _ft; 115 | 116 | _ft = doc[_ft_mat_key].as< std::vector >(); 117 | ft_coeff_mat_ = Eigen::Map(_ft.data(), 6, 6); 118 | ft_coeff_mat_.transposeInPlace(); 119 | std::cout << "[" <<_ft_mat_key << "_mat]" < >(); 124 | ft_unload_volatge_ = Eigen::Map(_ft.data(), 6, 1); 125 | std::cout << "[" <<_ft_unload_key << "]" << std::endl; 126 | std::cout << ft_unload_volatge_.transpose() << std::endl; 127 | 128 | return true; 129 | } 130 | 131 | 132 | void ATIForceTorqueSensorTWE::setCurrentVoltageOutput(double voltage0, double voltage1, double voltage2, 133 | double voltage3, double voltage4, double voltage5) 134 | { 135 | ft_current_volatge_.coeffRef(0, 0) = voltage0; 136 | ft_current_volatge_.coeffRef(1, 0) = voltage1; 137 | ft_current_volatge_.coeffRef(2, 0) = voltage2; 138 | ft_current_volatge_.coeffRef(3, 0) = voltage3; 139 | ft_current_volatge_.coeffRef(4, 0) = voltage4; 140 | ft_current_volatge_.coeffRef(5, 0) = voltage5; 141 | 142 | 143 | ft_raw_ = ft_coeff_mat_ * (ft_current_volatge_ - ft_unload_volatge_); 144 | 145 | ft_scale_param_mutex_.lock(); 146 | ft_scaled_ = ft_scale_factor_ * (ft_raw_ - ft_null_ ); 147 | ft_scale_param_mutex_.unlock(); 148 | 149 | ft_raw_msg_.header.stamp = ros::Time::now(); 150 | ft_scaled_msg_.header.stamp = ft_raw_msg_.header.stamp; 151 | ft_raw_msg_.wrench.force.x = ft_raw_.coeff(0,0); 152 | ft_raw_msg_.wrench.force.y = ft_raw_.coeff(1,0); 153 | ft_raw_msg_.wrench.force.z = ft_raw_.coeff(2,0); 154 | ft_raw_msg_.wrench.torque.x = ft_raw_.coeff(3,0); 155 | ft_raw_msg_.wrench.torque.y = ft_raw_.coeff(4,0); 156 | ft_raw_msg_.wrench.torque.z = ft_raw_.coeff(5,0); 157 | 158 | ft_scaled_msg_.wrench.force.x = ft_scaled_.coeff(0,0); 159 | ft_scaled_msg_.wrench.force.y = ft_scaled_.coeff(1,0); 160 | ft_scaled_msg_.wrench.force.z = ft_scaled_.coeff(2,0); 161 | ft_scaled_msg_.wrench.torque.x = ft_scaled_.coeff(3,0); 162 | ft_scaled_msg_.wrench.torque.y = ft_scaled_.coeff(4,0); 163 | ft_scaled_msg_.wrench.torque.z = ft_scaled_.coeff(5,0); 164 | } 165 | 166 | void ATIForceTorqueSensorTWE::setCurrentVoltageOutput(Eigen::MatrixXd voltage) 167 | { 168 | if((voltage.rows() != 6) || (voltage.cols() != 1)) { 169 | ROS_ERROR("Invalid voltage size"); 170 | return; 171 | } 172 | 173 | setCurrentVoltageOutput(voltage.coeff(0,0), voltage.coeff(1,0), voltage.coeff(2,0), 174 | voltage.coeff(3,0), voltage.coeff(4,0), voltage.coeff(5,0)); 175 | } 176 | 177 | Eigen::MatrixXd ATIForceTorqueSensorTWE::getCurrentForceTorqueRaw() 178 | { 179 | return ft_raw_; 180 | } 181 | 182 | Eigen::MatrixXd ATIForceTorqueSensorTWE::getCurrentForceTorqueScaled() 183 | { 184 | return ft_scaled_; 185 | } 186 | 187 | void ATIForceTorqueSensorTWE::getCurrentForceTorqueRaw(double* force_x_N, double* force_y_N, double* force_z_N, 188 | double* torque_x_Nm, double* torque_y_Nm, double* torque_z_Nm) 189 | { 190 | *force_x_N = ft_raw_.coeff(0,0); 191 | *force_y_N = ft_raw_.coeff(1,0); 192 | *force_z_N = ft_raw_.coeff(2,0); 193 | *torque_x_Nm = ft_raw_.coeff(3,0); 194 | *torque_y_Nm = ft_raw_.coeff(4,0); 195 | *torque_z_Nm = ft_raw_.coeff(5,0); 196 | } 197 | 198 | void ATIForceTorqueSensorTWE::getCurrentForceTorqueScaled(double* force_x_N, double* force_y_N, double* force_z_N, 199 | double* torque_x_Nm, double* torque_y_Nm, double* torque_z_Nm) 200 | { 201 | *force_x_N = ft_scaled_.coeff(0,0); 202 | *force_y_N = ft_scaled_.coeff(1,0); 203 | *force_z_N = ft_scaled_.coeff(2,0); 204 | *torque_x_Nm = ft_scaled_.coeff(3,0); 205 | *torque_y_Nm = ft_scaled_.coeff(4,0); 206 | *torque_z_Nm = ft_scaled_.coeff(5,0); 207 | } 208 | 209 | void ATIForceTorqueSensorTWE::setCurrentVoltageOutputPublish(double voltage0, double voltage1, double voltage2, 210 | double voltage3, double voltage4, double voltage5) 211 | { 212 | setCurrentVoltageOutput(voltage0, voltage1, voltage2, voltage3, voltage4, voltage5); 213 | 214 | if(is_ft_raw_published_) 215 | ft_raw_pub_.publish(ft_raw_msg_); 216 | 217 | if(is_ft_scaled_published_) 218 | ft_scaled_pub_.publish(ft_scaled_msg_); 219 | } 220 | 221 | void ATIForceTorqueSensorTWE::setCurrentVoltageOutputPublish(Eigen::MatrixXd voltage) 222 | { 223 | if((voltage.rows() != 6) || (voltage.cols() != 1)) { 224 | ROS_ERROR("Invalid voltage size"); 225 | return; 226 | } 227 | 228 | setCurrentVoltageOutput(voltage); 229 | 230 | if(is_ft_raw_published_) 231 | ft_raw_pub_.publish(ft_raw_msg_); 232 | 233 | if(is_ft_scaled_published_) 234 | ft_scaled_pub_.publish(ft_scaled_msg_); 235 | } 236 | -------------------------------------------------------------------------------- /thormang3_walking_module/include/thormang3_walking_module/thormang3_online_walking.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | /* 18 | * thormang3_online_walking.h 19 | * 20 | * Created on: 2016. 6. 10. 21 | * Author: Jay Song 22 | */ 23 | 24 | 25 | #ifndef THORMANG3_WALKING_MODULE_THORMANG3_ONLINEL_WALKING_H_ 26 | #define THORMANG3_WALKING_MODULE_THORMANG3_ONLINEL_WALKING_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include "robotis_framework_common/singleton.h" 36 | #include "robotis_math/robotis_math.h" 37 | 38 | #include "thormang3_kinematics_dynamics/kinematics_dynamics.h" 39 | #include "thormang3_balance_control/thormang3_balance_control.h" 40 | 41 | #define _USE_PD_BALANCE_ 42 | 43 | namespace thormang3 44 | { 45 | 46 | class THORMANG3OnlineWalking : public robotis_framework::Singleton 47 | { 48 | public: 49 | THORMANG3OnlineWalking(); 50 | virtual ~THORMANG3OnlineWalking(); 51 | 52 | void initialize(); 53 | void reInitialize(); 54 | void start(); 55 | void stop(); 56 | void process(); 57 | bool isRunning(); 58 | 59 | bool addStepData(robotis_framework::StepData step_data); 60 | void eraseLastStepData(); 61 | int getNumofRemainingUnreservedStepData(); 62 | void getReferenceStepDatafotAddition(robotis_framework::StepData *ref_step_data_for_addition); 63 | 64 | void setRefZMPDecisionParameter(double X_ZMP_CenterShift, double Y_ZMP_CenterShift, double Y_ZMP_Convergence); 65 | 66 | bool setInitialPose(double r_foot_x, double r_foot_y, double r_foot_z, double r_foot_roll, double r_foot_pitch, double r_foot_yaw, 67 | double l_foot_x, double l_foot_y, double l_foot_z, double l_foot_roll, double l_foot_pitch, double l_foot_yaw, 68 | double center_of_body_x, double center_of_body_y, double center_of_body_z, 69 | double center_of_body_roll, double center_of_body_pitch, double center_of_body_yaw); 70 | 71 | void setInitalWaistYawAngle(double waist_yaw_angle_rad); 72 | 73 | void setInitialRightShoulderAngle(double shoulder_angle_rad); 74 | void setInitialLeftShoulderAngle(double shoulder_angle_rad); 75 | void setInitialRightElbowAngle(double elbow_angle_rad); 76 | void setInitialLeftElbowAngle(double elbow_angle_rad); 77 | 78 | void setCurrentIMUSensorOutput(double gyro_x, double gyro_y, double quat_x, double quat_y, double quat_z, double quat_w); 79 | 80 | Eigen::MatrixXd mat_cob_to_g_, mat_g_to_cob_; 81 | Eigen::MatrixXd mat_robot_to_cob_, mat_cob_to_robot_; 82 | Eigen::MatrixXd mat_robot_to_g_, mat_g_to_robot_; 83 | Eigen::MatrixXd mat_cob_to_rhip_, mat_rhip_to_cob_; 84 | Eigen::MatrixXd mat_cob_to_lhip_, mat_lhip_to_cob_; 85 | 86 | Eigen::MatrixXd mat_g_to_rfoot_, mat_g_to_lfoot_; 87 | 88 | double r_shoulder_out_angle_rad_; 89 | double l_shoulder_out_angle_rad_; 90 | double r_elbow_out_angle_rad_; 91 | double l_elbow_out_angle_rad_; 92 | double r_leg_out_angle_rad_[6]; 93 | double l_leg_out_angle_rad_[6]; 94 | double out_angle_rad_[16]; 95 | 96 | double hip_roll_feedforward_angle_rad_; 97 | 98 | double curr_angle_rad_[12]; 99 | thormang3::BalancePDController leg_angle_feed_back_[12]; 100 | 101 | // balance control 102 | int balance_error_; 103 | thormang3::BalanceControlUsingPDController balance_ctrl_; 104 | 105 | 106 | // sensor value 107 | double current_right_fx_N_, current_right_fy_N_, current_right_fz_N_; 108 | double current_right_tx_Nm_, current_right_ty_Nm_, current_right_tz_Nm_; 109 | double current_left_fx_N_, current_left_fy_N_, current_left_fz_N_; 110 | double current_left_tx_Nm_, current_left_ty_Nm_, current_left_tz_Nm_; 111 | 112 | Eigen::Quaterniond quat_current_imu_; 113 | Eigen::MatrixXd mat_current_imu_; 114 | double current_imu_roll_rad_, current_imu_pitch_rad_; 115 | double current_gyro_roll_rad_per_sec_, current_gyro_pitch_rad_per_sec_; 116 | 117 | private: 118 | void calcStepIdxData(); 119 | void calcRefZMP(); 120 | void calcDesiredPose(); 121 | 122 | double wsin(double time, double period, double period_shift, double mag, double mag_shift); 123 | double wsigmoid(double time, double period, double time_shift, double mag, double mag_shift, double sigmoid_ratio, double distortion_ratio); 124 | 125 | double r_leg_to_body_roll_gain_, l_leg_to_body_roll_gain_; 126 | double r_leg_to_body_pitch_gain_, l_leg_to_body_pitch_gain_; 127 | Eigen::MatrixXd des_balance_offset_; 128 | robotis_framework::MinimumJerkViaPoint *feed_forward_tra_; 129 | int mov_size_, mov_step_; 130 | double mov_time_; 131 | bool init_balance_offset_; 132 | 133 | void parseBalanceOffsetData(const std::string &path); 134 | void initBalanceOffset(); 135 | void setBalanceOffset(); 136 | 137 | KinematicsDynamics* thormang3_kd_; 138 | 139 | double left_fz_trajectory_start_time_; 140 | double left_fz_trajectory_end_time_; 141 | double left_fz_trajectory_target_; 142 | double left_fz_trajectory_shift_; 143 | 144 | double total_mass_of_robot_; 145 | double right_dsp_fz_N_, right_ssp_fz_N_; 146 | double left_dsp_fz_N_, left_ssp_fz_N_; 147 | 148 | Eigen::MatrixXd mat_robot_to_cob_modified_, mat_cob_to_robot_modified_; 149 | Eigen::MatrixXd mat_robot_to_rf_modified_; 150 | Eigen::MatrixXd mat_robot_to_lf_modified_; 151 | Eigen::MatrixXd mat_robot_to_rfoot_; 152 | Eigen::MatrixXd mat_robot_to_lfoot_; 153 | 154 | std::vector added_step_data_; 155 | 156 | double goal_waist_yaw_angle_rad_; 157 | robotis_framework::StepData current_step_data_; 158 | robotis_framework::StepData reference_step_data_for_addition_; 159 | robotis_framework::Pose3D initial_right_foot_pose_, initial_left_foot_pose_, initial_body_pose_; 160 | robotis_framework::Pose3D present_right_foot_pose_, present_left_foot_pose_, present_body_pose_; 161 | robotis_framework::Pose3D previous_step_right_foot_pose_, previous_step_left_foot_pose_, previous_step_body_pose_; 162 | robotis_framework::Pose3D rhip_to_rfoot_pose_, lhip_to_lfoot_pose_; 163 | robotis_framework::FifthOrderPolynomialTrajectory foot_x_tra_, foot_y_tra_, foot_z_tra_; 164 | robotis_framework::FifthOrderPolynomialTrajectory foot_roll_tra_, foot_pitch_tra_, foot_yaw_tra_; 165 | robotis_framework::FifthOrderPolynomialTrajectory foot_z_swap_tra_; 166 | robotis_framework::FifthOrderPolynomialTrajectory body_z_tra_, body_roll_tra_, body_pitch_tra_, body_yaw_tra_; 167 | robotis_framework::FifthOrderPolynomialTrajectory waist_yaw_tra_; 168 | robotis_framework::FifthOrderPolynomialTrajectory body_z_swap_tra_; 169 | robotis_framework::FifthOrderPolynomialTrajectory hip_roll_swap_tra_; 170 | 171 | double present_waist_yaw_angle_rad_; 172 | double previous_step_waist_yaw_angle_rad_; 173 | double r_init_shoulder_angle_rad_, r_init_elbow_angle_rad_; 174 | double l_init_shoulder_angle_rad_, l_init_elbow_angle_rad_; 175 | double r_shoulder_dir_, r_elbow_dir_; 176 | double l_shoulder_dir_, l_elbow_dir_; 177 | double shouler_swing_gain_, elbow_swing_gain_; 178 | 179 | Eigen::MatrixXd mat_rfoot_to_rft_, mat_lfoot_to_lft_; 180 | Eigen::MatrixXd rot_x_pi_3d_, rot_z_pi_3d_; 181 | 182 | Eigen::VectorXi step_idx_data_; 183 | boost::mutex step_data_mutex_lock_; 184 | boost::mutex imu_data_mutex_lock_; 185 | 186 | //Time for Preview Control and Dynamics Regulator 187 | double preview_time_; 188 | int preview_size_; 189 | 190 | //These matrix and parameters are for preview control 191 | Eigen::MatrixXd A_, b_, c_; 192 | Eigen::MatrixXd k_x_; 193 | Eigen::MatrixXd f_; 194 | double k_s_; 195 | double sum_of_zmp_x_ ; 196 | double sum_of_zmp_y_ ; 197 | double sum_of_cx_ ; 198 | double sum_of_cy_ ; 199 | Eigen::MatrixXd u_x, u_y; 200 | Eigen::MatrixXd x_lipm_, y_lipm_; 201 | 202 | int current_start_idx_for_ref_zmp_; 203 | 204 | double ref_zmp_x_at_this_time_, ref_zmp_y_at_this_time_; 205 | Eigen::MatrixXd reference_zmp_x_, reference_zmp_y_; 206 | 207 | bool real_running, ctrl_running; 208 | 209 | double walking_time_; //Absolute Time 210 | double reference_time_; //Absolute Time 211 | int balancing_index_; 212 | int current_step_data_status_; 213 | }; 214 | 215 | } 216 | 217 | #endif /* THORMANG3_WALKING_MODULE_THORMANG3_ONLINEL_WALKING_H_ */ 218 | -------------------------------------------------------------------------------- /thormang3_gripper_module/src/gripper_module.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * Copyright 2018 ROBOTIS CO., LTD. 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | *******************************************************************************/ 16 | 17 | #include 18 | #include "thormang3_gripper_module/gripper_module.h" 19 | 20 | using namespace thormang3; 21 | 22 | GripperModule::GripperModule() 23 | : control_cycle_sec_(0.008), 24 | is_moving_(false) 25 | { 26 | enable_ = false; 27 | module_name_ = "gripper_module"; 28 | control_mode_ = robotis_framework::PositionControl; 29 | 30 | /* gripper */ 31 | result_["r_arm_grip"] = new robotis_framework::DynamixelState(); 32 | result_["l_arm_grip"] = new robotis_framework::DynamixelState(); 33 | 34 | /* gripper */ 35 | joint_name_to_id_["r_arm_grip"] = 0; 36 | joint_name_to_id_["l_arm_grip"] = 1; 37 | 38 | /* ----- parameter initialization ----- */ 39 | present_joint_position_ = Eigen::VectorXd::Zero(NUM_GRIPPER_JOINTS); 40 | present_joint_velocity_ = Eigen::VectorXd::Zero(NUM_GRIPPER_JOINTS); 41 | goal_joint_position_ = Eigen::VectorXd::Zero(NUM_GRIPPER_JOINTS); 42 | } 43 | 44 | GripperModule::~GripperModule() 45 | { 46 | queue_thread_.join(); 47 | } 48 | 49 | void GripperModule::initialize(const int control_cycle_msec, robotis_framework::Robot *robot) 50 | { 51 | control_cycle_sec_ = control_cycle_msec * 0.001; 52 | queue_thread_ = boost::thread(boost::bind(&GripperModule::queueThread, this)); 53 | } 54 | 55 | void GripperModule::queueThread() 56 | { 57 | ros::NodeHandle ros_node; 58 | ros::CallbackQueue callback_queue; 59 | 60 | ros_node.setCallbackQueue(&callback_queue); 61 | 62 | /* publish topics */ 63 | status_msg_pub_ = ros_node.advertise("/robotis/status", 1); 64 | set_ctrl_module_pub_ = ros_node.advertise("/robotis/enable_ctrl_module", 1); 65 | goal_torque_limit_pub_ = ros_node.advertise("/robotis/sync_write_item", 1); 66 | movement_done_pub_ = ros_node.advertise("/robotis/movement_done", 1); 67 | 68 | /* subscribe topics */ 69 | ros::Subscriber set_mode_msg_sub = ros_node.subscribe("/robotis/gripper/set_mode_msg", 5, 70 | &GripperModule::setModeMsgCallback, this); 71 | ros::Subscriber joint_pose_msg_sub = ros_node.subscribe("/robotis/gripper/joint_pose_msg", 5, 72 | &GripperModule::setJointPoseMsgCallback, this); 73 | 74 | /* service */ 75 | 76 | ros::WallDuration duration(control_cycle_sec_); 77 | while(ros_node.ok()) 78 | callback_queue.callAvailable(duration); 79 | } 80 | 81 | void GripperModule::setModeMsgCallback(const std_msgs::String::ConstPtr& msg) 82 | { 83 | // ROS_INFO("--- Set Torque Control Mode ---"); 84 | std_msgs::String str_msg; 85 | str_msg.data = "gripper_module"; 86 | set_ctrl_module_pub_.publish(str_msg); 87 | return; 88 | } 89 | 90 | void GripperModule::setJointPoseMsgCallback(const sensor_msgs::JointState::ConstPtr& msg) 91 | { 92 | if(enable_ == false) 93 | return; 94 | 95 | goal_joint_pose_msg_ = *msg; 96 | 97 | if (is_moving_ == false) 98 | { 99 | setTorqueLimit(); 100 | 101 | movement_done_msg_.data = "gripper"; 102 | 103 | tra_gene_tread_ = new boost::thread(boost::bind(&GripperModule::traGeneProcJointSpace, this)); 104 | delete tra_gene_tread_; 105 | } 106 | else 107 | ROS_INFO("previous task is alive"); 108 | 109 | return; 110 | } 111 | 112 | void GripperModule::traGeneProcJointSpace() 113 | { 114 | mov_time_ = 1.5; 115 | int all_time_steps = int(floor((mov_time_/control_cycle_sec_) + 1 )); 116 | mov_time_ = double (all_time_steps - 1) * control_cycle_sec_; 117 | 118 | all_time_steps_ = int(mov_time_ / control_cycle_sec_) + 1; 119 | goal_joint_tra_.resize(all_time_steps_, NUM_GRIPPER_JOINTS + 1); 120 | 121 | /* calculate joint trajectory */ 122 | for (int dim=0; dim= all_time_steps_) 179 | { 180 | ROS_INFO("[end] send trajectory"); 181 | publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "End Trajectory"); 182 | 183 | movement_done_pub_.publish(movement_done_msg_); 184 | movement_done_msg_.data = ""; 185 | 186 | is_moving_ = false; 187 | cnt_ = 0; 188 | } 189 | } 190 | } 191 | 192 | void GripperModule::process(std::map dxls, 193 | std::map sensors) 194 | { 195 | if (enable_ == false) 196 | return; 197 | 198 | /*----- Get Joint Data & Sensor Data-----*/ 199 | for (std::map::iterator state_iter = result_.begin(); 200 | state_iter != result_.end(); state_iter++) 201 | { 202 | std::string joint_name = state_iter->first; 203 | 204 | robotis_framework::Dynamixel *dxl = NULL; 205 | std::map::iterator dxl_it = dxls.find(joint_name); 206 | if (dxl_it != dxls.end()) 207 | dxl = dxl_it->second; 208 | else 209 | continue; 210 | 211 | // Get Joint Data 212 | present_joint_position_(joint_name_to_id_[joint_name]) = dxl->dxl_state_->present_position_; 213 | present_joint_velocity_(joint_name_to_id_[joint_name]) = dxl->dxl_state_->present_velocity_; 214 | 215 | goal_joint_position_(joint_name_to_id_[joint_name]) = dxl->dxl_state_->goal_position_; 216 | } 217 | 218 | /* ----- Movement Event -----*/ 219 | if (is_moving_ == true) 220 | { 221 | if (cnt_ == 0) 222 | publishStatusMsg(robotis_controller_msgs::StatusMsg::STATUS_INFO, "Start Trajectory"); 223 | 224 | // joint space control 225 | for (int dim=0; dim::iterator state_iter = result_.begin(); 233 | state_iter != result_.end(); state_iter++) 234 | { 235 | std::string joint_name = state_iter->first; 236 | result_[joint_name]->goal_position_ = goal_joint_position_(joint_name_to_id_[joint_name]); 237 | } 238 | 239 | /*---------- Movement End Event ----------*/ 240 | setEndTrajectory(); 241 | } 242 | 243 | void GripperModule::stop() 244 | { 245 | is_moving_ = false; 246 | 247 | return; 248 | } 249 | 250 | bool GripperModule::isRunning() 251 | { 252 | return is_moving_; 253 | } 254 | 255 | void GripperModule::publishStatusMsg(unsigned int type, std::string msg) 256 | { 257 | robotis_controller_msgs::StatusMsg status_msg; 258 | status_msg.header.stamp = ros::Time::now(); 259 | status_msg.type = type; 260 | status_msg.module_name = "Gripper"; 261 | status_msg.status_msg = msg; 262 | 263 | status_msg_pub_.publish(status_msg); 264 | } 265 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "{}" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright {yyyy} {name of copyright owner} 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | --------------------------------------------------------------------------------