diff --git a/jsk_g1_robot/g1eus/.gitignore b/jsk_g1_robot/g1eus/.gitignore new file mode 100644 index 0000000000..b8af841467 --- /dev/null +++ b/jsk_g1_robot/g1eus/.gitignore @@ -0,0 +1,2 @@ +g1.l +models/g1.urdf \ No newline at end of file diff --git a/jsk_g1_robot/g1eus/CMakeLists.txt b/jsk_g1_robot/g1eus/CMakeLists.txt new file mode 100644 index 0000000000..8e0920c79d --- /dev/null +++ b/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 + "" + "" + 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) diff --git a/jsk_g1_robot/g1eus/README.md b/jsk_g1_robot/g1eus/README.md new file mode 100644 index 0000000000..6dd3d499b2 --- /dev/null +++ b/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 /src +cd /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//setup.bash +cd /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//setup.bash +mkdir -p /src +cd /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 /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 /install/setup.bash +ros2 launch g1_bringup g1_bringup.launch.py network_interface:= hand_type:=inspire_dfq +``` + +- terminal 4 (Activate ros2_control upper_body_controller) +```bash +source /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 /install/setup.bash +source /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 /devel/setup.bash +``` + +```lisp +(load "package://g1eus/g1-interface.l") +``` + +### If you want to do teleoperation using spacenav + +- terminal 6 + +``` bash +source /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. diff --git a/jsk_g1_robot/g1eus/config/g1.yaml b/jsk_g1_robot/g1eus/config/g1.yaml new file mode 100644 index 0000000000..350f299cde --- /dev/null +++ b/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] diff --git a/jsk_g1_robot/g1eus/figs/g1eus.png b/jsk_g1_robot/g1eus/figs/g1eus.png new file mode 100644 index 0000000000..f9899fc088 Binary files /dev/null and b/jsk_g1_robot/g1eus/figs/g1eus.png differ diff --git a/jsk_g1_robot/g1eus/g1-interface.l b/jsk_g1_robot/g1eus/g1-interface.l new file mode 100644 index 0000000000..78ff7b094e --- /dev/null +++ b/jsk_g1_robot/g1eus/g1-interface.l @@ -0,0 +1,182 @@ +(load "package://pr2eus/robot-interface.l") +(require :g1 "package://g1eus/g1.l") + +(defclass g1-interface + :super robot-interface + :slots (hand-actions- + hand-joint-names-)) + +(defmethod g1-interface + (:arm-to-hand-controller-name + (arm) + (case arm + (:rarm "right_hand_controller") + (:larm "left_hand_controller") + (t nil))) + + (:default-hand-joint-names + (arm) + (case arm + (:rarm + (list + "R_thumb_proximal_yaw_joint" + "R_thumb_proximal_pitch_joint" + "R_index_proximal_joint" + "R_middle_proximal_joint" + "R_ring_proximal_joint" + "R_pinky_proximal_joint")) + (:larm + (list + "L_thumb_proximal_yaw_joint" + "L_thumb_proximal_pitch_joint" + "L_index_proximal_joint" + "L_middle_proximal_joint" + "L_ring_proximal_joint" + "L_pinky_proximal_joint")) + (t nil))) + + (:upper-body-controller + () + (list + (list + (cons :controller-action "upper_body_controller/follow_joint_trajectory") + (cons :controller-state "upper_body_controller/state") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (list + "waist_yaw_joint" + "waist_roll_joint" + "waist_pitch_joint" + "left_shoulder_pitch_joint" + "left_shoulder_roll_joint" + "left_shoulder_yaw_joint" + "left_elbow_joint" + "left_wrist_roll_joint" + "left_wrist_pitch_joint" + "left_wrist_yaw_joint" + "right_shoulder_pitch_joint" + "right_shoulder_roll_joint" + "right_shoulder_yaw_joint" + "right_elbow_joint" + "right_wrist_roll_joint" + "right_wrist_pitch_joint" + "right_wrist_yaw_joint"))))) + + (:default-controller + () + (append + (send self :upper-body-controller) + ) + ) + + (:init (&rest args) + (send-super* :init :robot g1-robot args) + (setq hand-actions- (make-hash-table)) + (setq hand-joint-names- (make-hash-table)) + (dolist (arm (list :rarm :larm)) + (let* ((controller-name (send self :arm-to-hand-controller-name arm)) + (action-name (format nil "~a/follow_joint_trajectory" controller-name)) + (joint-names (ros::get-param (format nil "~a/joints" controller-name) + (send self :default-hand-joint-names arm))) + action) + (sethash arm hand-joint-names- joint-names) + (setq action + (instance ros::simple-action-client :init + action-name + control_msgs::FollowJointTrajectoryAction + :groupname groupname)) + (if (and joint-action-enable (send action :wait-for-server 3)) + (sethash arm hand-actions- action) + (ros::ros-warn "~A is not respond" action))))) + +;; (:larm (&rest args) +;; (send* self :limb :larm args)) + +;; (:rarm (&rest args) +;; (send* self :limb :rarm args)) + +;; (:limb +;; (arm &rest args) +;; (case (car args) +;; (:hand-angle-vector +;; (send* self :hand-angle-vector arm (cdr args))) +;; (:cancel-hand-angle-vector +;; (send self :cancel-hand-angle-vector arm)) +;; (:hand-interpolatingp +;; (send self :hand-interpolatingp arm)) +;; (t +;; (send* robot arm args)))) + + (:stop-grasp + (arm) + (send self :hand-angle-vector arm #f(0 0 0 0 0 0) 1000)) + + (:prepare-grasp + (arm) + (send self :hand-angle-vector arm #f(100 0 0 0 0 0) 1000)) + + (:start-grasp + (arm) + (send self :hand-angle-vector arm #f(40 20 80 80 80 80) 1000)) + + (:default-grasp + (arm) + (send self :hand-angle-vector arm #f(10 30 100 100 100 100) 1000)) + + (:hand-angle-vector + (arm av tm &key (wait t)) + (let ((action (gethash arm hand-actions-)) + (names (gethash arm hand-joint-names-))) + (cond + ((null action) + (ros::ros-warn "hand action for ~A is not available" arm) + nil) + ((null names) + (ros::ros-warn "joint names for ~A hand are not available" arm) + nil) + ((/= (length av) (length names)) + (ros::ros-warn "hand angle-vector length ~A does not match joint names length ~A for ~A" + (length av) (length names) arm) + nil) + (t + (let ((av-list (coerce av cons)) + res + (goal (instance control_msgs::FollowJointTrajectoryActionGoal :init)) + (traj (instance trajectory_msgs::JointTrajectory :init))) + (send traj :header :seq 1) + (send traj :header :stamp (ros::time-now)) + (send traj :joint_names names) + (send traj :points + (list (instance trajectory_msgs::JointTrajectoryPoint + :init + :positions (mapcar #'deg2rad av-list) + :time_from_start (ros::time (/ tm 1000.0))))) + (send goal :goal :trajectory traj) + (send action :send-goal goal) + (setq res (send action :get-result)) + (when wait + (send action :wait-for-result) + (setq res (send action :get-result))) + (unless (eq (send res :error_code) 0) + (warning-message 3 ":hand-angle-vector error code ~A returns from JTA: ~A~%" + (send res :error_code) (send res :error_string))) + res))))) + + (:cancel-hand-angle-vector + (arm) + (let ((action (gethash arm hand-actions-))) + (if action + (send action :cancel-goal) + (ros::ros-warn "hand action for ~A is not available" arm)))) + + (:hand-interpolatingp + (arm) + (send self :spin-once) + (let ((action (gethash arm hand-actions-))) + (and action + (eq (send action :get-state) ros::*simple-goal-state-active*))))) + +(defun g1-init () + (unless (boundp '*ri*) + (setq *ri* (instance g1-interface :init))) + (unless (boundp '*g1*) + (setq *g1* (instance g1-robot :init)))) diff --git a/jsk_g1_robot/g1eus/package.xml b/jsk_g1_robot/g1eus/package.xml new file mode 100644 index 0000000000..beedfd9099 --- /dev/null +++ b/jsk_g1_robot/g1eus/package.xml @@ -0,0 +1,13 @@ + + + g1eus + 0.0.0 + This package provides EusLisp interface for Unitree G1 Robot + Kei Okada + Michitoshi Tsubaki + BSD + euscollada + pr2eus + unitree_controller + catkin + diff --git a/jsk_g1_robot/g1eus/ros-o.repos.yaml b/jsk_g1_robot/g1eus/ros-o.repos.yaml new file mode 100644 index 0000000000..404a0d4992 --- /dev/null +++ b/jsk_g1_robot/g1eus/ros-o.repos.yaml @@ -0,0 +1,9 @@ +repositories: + unitree/unitree_ros: + type: git + url: https://github.com/unitreerobotics/unitree_ros.git + version: master + unitree/unitree_ros_to_real: + type: git + url: https://github.com/unitreerobotics/unitree_ros_to_real.git + version: master