[g1eus] Add Unitree g1 eus model and robot-interface for supporting general teleoperation framework

jsk_g1_robot/g1eus/.gitignore

@@ -0,0 +1,2 @@
+g1.l
+models/g1.urdf

jsk_g1_robot/g1eus/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(g1eus)
+
+find_package(catkin REQUIRED COMPONENTS
+  euscollada
+  pr2eus
+  unitree_controller
+)
+
+catkin_package(
+  CATKIN_DEPENDS euscollada pr2eus unitree_controller
+)
+
+set(G1_DESCRIPTION_DIR ${unitree_controller_SOURCE_PREFIX}/../robots/g1_description)
+set(G1_SOURCE_URDF ${G1_DESCRIPTION_DIR}/g1_29dof.urdf)
+set(G1_GENERATED_URDF ${PROJECT_SOURCE_DIR}/models/g1.urdf)
+set(G1_EUSLISP ${PROJECT_SOURCE_DIR}/g1.l)
+
+if(NOT EXISTS ${G1_SOURCE_URDF})
+  message(FATAL_ERROR "Could not find G1 URDF at ${G1_SOURCE_URDF}")
+endif()
+
+file(READ ${G1_SOURCE_URDF} G1_URDF_CONTENT)
+string(REPLACE
+  "filename=\"meshes/"
+  "filename=\"package://unitree_controller/../robots/g1_description/meshes/"
+  G1_URDF_CONTENT
+  "${G1_URDF_CONTENT}")
+string(REPLACE
+  "<robot name=\"g1_29dof\">"
+  "<robot name=\"g1\">"
+  G1_URDF_CONTENT
+  "${G1_URDF_CONTENT}")
+file(WRITE ${G1_GENERATED_URDF} "${G1_URDF_CONTENT}")
+
+add_custom_command(
+  OUTPUT ${G1_EUSLISP}
+  COMMAND rosrun euscollada collada2eus -I ${G1_GENERATED_URDF} -N g1 -C ${PROJECT_SOURCE_DIR}/config/g1.yaml -O ${G1_EUSLISP}
+  DEPENDS ${G1_GENERATED_URDF} ${PROJECT_SOURCE_DIR}/config/g1.yaml
+  WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
+  COMMENT "Generating G1 EusLisp model"
+)
+
+add_custom_target(generate_g1_eus_model ALL DEPENDS ${G1_EUSLISP})
+
+install(DIRECTORY config euslisp models
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)
+
+install(FILES ${G1_GENERATED_URDF} ${G1_EUSLISP} g1-interface.l
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  OPTIONAL)

jsk_g1_robot/g1eus/README.md

@@ -0,0 +1,142 @@
+# g1eus
+![g1eus](./figs/g1eus.png)
+
+## Setup
+
+### Clone this repository in your catkin workspace (ros1)
+```
+mkdir -p <desirable path to your catkin workspace>/src
+cd <path to your catkin workspace>/src
+git clone git@github.com:Michi-Tsubaki/jsk_robot.git -b support-eus-unitree-g1
+```
+
+
+### Build package
+Please install ROS 1 and some development tools like rosdep, vcstools at first.
+
+Then build the packages by running the following commands.
+
+```shell
+source /opt/ros/<ROS DISTRO>/setup.bash
+cd <path to your catkin workspace>/src
+wget https://raw.githubusercontent.com/Michi-Tsubaki/jsk_robot/refs/heads/support-eus-unitree-g1/jsk_g1_robot/g1eus/ros-o.repos.yaml -O- | vcs import
+sudo apt update && rosdep update && rosdep install -iqry --from-paths .
+cd ..
+catkin build g1eus
+```
+
+
+## How to use real robot
+
+### Build g1_ros (ROS2 package)
+
+```bash
+source /opt/ros/<ROS2 DISTRO>/setup.bash
+mkdir -p <path to your desirable colcon workspace>/src
+cd <path to colcon workspace>/src
+wget https://raw.githubusercontent.com/mqcmd196/g1_ros/refs/heads/master/jazzy.repos.yaml -O- | vcs import
+sudo apt update && rosdep update && rosdep install -iqry --from-paths .
+cd ..
+colcon build --symlink-install --packages-up-to g1_bringup
+```
+
+For more information, please visit [g1_ros](https://github.com/mqcmd196/g1_ros)
+
+### Install ros1_bridge
+ros1_bridge is a ROS2 package. Please check [here](https://github.com/ros-o/ros1_bridge)
+
+The deb file for this package is shared at https://drive.google.com/file/d/1jXZlvovTGa_PU6stJOznvv5ZYkarB62n/view?usp=sharing
+
+Please download this deb file and install the package:
+
+```bash
+cd ~/Downloads
+sudo apt install ./ros-jazzy-ros1-bridge_0.10.3-0noble_amd64.deb
+```
+
+
+### Preparation
+
+#### Connect Ethernet cable from g1 to your computer
+
+Please connect your computer to the robot following [official instruction](https://support.unitree.com/home/en/G1_developer/quick_development#heading-7). Please check the network interface name.
+
+Also please allocate the correct IP adress instructed [official instruction](https://support.unitree.com/home/en/G1_developer/quick_development#heading-7) manually.
+
+
+### Execution
+
+- terminal 1
+
+```bash
+source <path to your catkin ws>/devel/setup.bash
+roscore
+```
+
+- terminal 2 (Bringup hands)
+
+```bash
+ssh unitree@192.168.123.164  # default password is 123
+cd dfx_inspire_service/build/
+sudo ./inspire_g1 -k -u
+```
+
+- terminal 3 (Bringup robot as a ROS2 robot)
+
+```bash
+source <path_to_your_colcon_ws>/install/setup.bash
+ros2 launch g1_bringup g1_bringup.launch.py network_interface:=<network (ex. enp0s31f6)> hand_type:=inspire_dfq
+```
+
+- terminal 4 (Activate ros2_control upper_body_controller)
+```bash
+source <path_to_your_colcon_ws>/install/setup.bash
+ros2 control set_controller_state upper_body_controller active
+```
+
+- terminal 5 (Bridge ros2 topic and action to ros1 topic and actionlib)
+```bash
+source <path_to_your_colcon_ws>/install/setup.bash
+source <path_to_your_catkin_ws>/devel/setup.bash
+ros2 run ros1_bridge dynamic_bridge --bridge-all-topics
+```
+
+- terminal 6 ~ (Optional)
+Please run your own program using g1-interface.l like
+
+```shell
+source <path_to_your_catkin_ws>/devel/setup.bash
+```
+
+```lisp
+(load "package://g1eus/g1-interface.l")
+```
+
+### If you want to do teleoperation using spacenav
+
+- terminal 6
+
+``` bash
+source <path_to_your_catkin_ws>/devel/setup.bash
+roslaunch jsk_generic_teleop g1_spacenav_teleop.launch
+```
+
+
+## Tips
+
+- Hand Interface
+
+```lisp
+(send *ri* :hand-angle-vector :larm #f(0 0 0 0 0 0) 1000) ;; to move left hand.
+
+;; Open
+(send *ri* :stop-grasp :larm) ;; to move left hand.
+;; Before grasping
+(send *ri* :prepare-grasp :larm) ;; to move left hand.
+;; Grasp
+(send *ri* :start-grasp :larm) ;; to move left hand.
+;; Power-of position
+(send *ri* :default-grasp :larm) ;; to move left hand.
+```
+
+Other methods than the hand interface is inherit from pr2eus.

jsk_g1_robot/g1eus/config/g1.yaml

@@ -0,0 +1,57 @@
+torso:
+  - waist_yaw_joint   : torso-waist-y
+  - waist_roll_joint  : torso-waist-r
+  - waist_pitch_joint : torso-waist-p
+
+lleg:
+  - left_hip_pitch_joint   : lleg-hip-p
+  - left_hip_roll_joint    : lleg-hip-r
+  - left_hip_yaw_joint     : lleg-hip-y
+  - left_knee_joint        : lleg-knee-p
+  - left_ankle_pitch_joint : lleg-ankle-p
+  - left_ankle_roll_joint  : lleg-ankle-r
+
+rleg:
+  - right_hip_pitch_joint   : rleg-hip-p
+  - right_hip_roll_joint    : rleg-hip-r
+  - right_hip_yaw_joint     : rleg-hip-y
+  - right_knee_joint        : rleg-knee-p
+  - right_ankle_pitch_joint : rleg-ankle-p
+  - right_ankle_roll_joint  : rleg-ankle-r
+
+larm:
+  - left_shoulder_pitch_joint : larm-shoulder-p
+  - left_shoulder_roll_joint  : larm-shoulder-r
+  - left_shoulder_yaw_joint   : larm-shoulder-y
+  - left_elbow_joint          : larm-elbow-p
+  - left_wrist_roll_joint     : larm-wrist-r
+  - left_wrist_pitch_joint    : larm-wrist-p
+  - left_wrist_yaw_joint      : larm-wrist-y
+
+rarm:
+  - right_shoulder_pitch_joint : rarm-shoulder-p
+  - right_shoulder_roll_joint  : rarm-shoulder-r
+  - right_shoulder_yaw_joint   : rarm-shoulder-y
+  - right_elbow_joint          : rarm-elbow-p
+  - right_wrist_roll_joint     : rarm-wrist-r
+  - right_wrist_pitch_joint    : rarm-wrist-p
+  - right_wrist_yaw_joint      : rarm-wrist-y
+
+angle-vector:
+  reset-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+
+larm-end-coords:
+  parent    : left_wrist_yaw_link
+  translate : [0.1, 0.0, 0.0]
+
+rarm-end-coords:
+  parent    : right_wrist_yaw_link
+  translate : [0.1, 0.0, 0.0]
+
+lleg-end-coords:
+  parent    : left_ankle_roll_link
+  translate : [0.0, 0.0, 0.0]
+
+rleg-end-coords:
+  parent    : right_ankle_roll_link
+  translate : [0.0, 0.0, 0.0]