From c3a9bf7a3e3d1c8f1e410701b57c6d92cefcbd8f Mon Sep 17 00:00:00 2001 From: Liao Dengting <540614105@qq.com> Date: Thu, 7 Mar 2024 15:07:08 +0800 Subject: [PATCH] delete go1_gym_deploy folder --- go1_gym_deploy/__init__.py | 0 go1_gym_deploy/autostart/start_controller.sh | 5 - go1_gym_deploy/autostart/start_unitree_sdk.sh | 6 - go1_gym_deploy/docker/Dockerfile | 190 -------- go1_gym_deploy/docker/Makefile | 39 -- go1_gym_deploy/docker/unzip_image.sh | 2 - go1_gym_deploy/docker/zip_image.sh | 2 - go1_gym_deploy/envs/__init__.py | 0 go1_gym_deploy/envs/history_wrapper.py | 51 --- go1_gym_deploy/envs/lcm_agent.py | 301 ------------- .../installer/install_deployment_code.sh | 22 - go1_gym_deploy/lcm_types/__init__.py | 0 .../lcm_types/camera_message_lcmt.py | 60 --- .../lcm_types/camera_message_rect_wide.py | 57 --- .../lcm_types/leg_control_data_lcmt.lcm | 11 - .../lcm_types/leg_control_data_lcmt.py | 85 ---- .../lcm_types/pd_tau_targets_lcmt.lcm | 12 - .../lcm_types/pd_tau_targets_lcmt.py | 88 ---- go1_gym_deploy/lcm_types/rc_command_lcmt.lcm | 13 - go1_gym_deploy/lcm_types/rc_command_lcmt.py | 90 ---- .../lcm_types/state_estimator_lcmt.lcm | 16 - .../lcm_types/state_estimator_lcmt.py | 102 ----- go1_gym_deploy/scripts/__init__.py | 0 go1_gym_deploy/scripts/deploy_policy.py | 77 ---- go1_gym_deploy/scripts/send_to_unitree.sh | 8 - go1_gym_deploy/setup.py | 12 - go1_gym_deploy/tests/__init__.py | 0 go1_gym_deploy/tests/check_camera_msgs.py | 154 ------- .../unitree_legged_sdk_bin/__init__.py | 0 .../unitree_legged_sdk_bin/lcm_position | Bin 95968 -> 0 bytes .../unitree_legged_sdk_bin/lcm_position.cpp | 236 ---------- go1_gym_deploy/utils/__init__.py | 0 .../utils/cheetah_state_estimator.py | 406 ------------------ go1_gym_deploy/utils/command_profile.py | 231 ---------- go1_gym_deploy/utils/deployment_runner.py | 222 ---------- go1_gym_deploy/utils/logger.py | 79 ---- .../utils/network_config_unitree.py | 69 --- 37 files changed, 2646 deletions(-) delete mode 100644 go1_gym_deploy/__init__.py delete mode 100644 go1_gym_deploy/autostart/start_controller.sh delete mode 100644 go1_gym_deploy/autostart/start_unitree_sdk.sh delete mode 100644 go1_gym_deploy/docker/Dockerfile delete mode 100644 go1_gym_deploy/docker/Makefile delete mode 100755 go1_gym_deploy/docker/unzip_image.sh delete mode 100755 go1_gym_deploy/docker/zip_image.sh delete mode 100644 go1_gym_deploy/envs/__init__.py delete mode 100755 go1_gym_deploy/envs/history_wrapper.py delete mode 100755 go1_gym_deploy/envs/lcm_agent.py delete mode 100644 go1_gym_deploy/installer/install_deployment_code.sh delete mode 100644 go1_gym_deploy/lcm_types/__init__.py delete mode 100644 go1_gym_deploy/lcm_types/camera_message_lcmt.py delete mode 100644 go1_gym_deploy/lcm_types/camera_message_rect_wide.py delete mode 100755 go1_gym_deploy/lcm_types/leg_control_data_lcmt.lcm delete mode 100755 go1_gym_deploy/lcm_types/leg_control_data_lcmt.py delete mode 100755 go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.lcm delete mode 100755 go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.py delete mode 100755 go1_gym_deploy/lcm_types/rc_command_lcmt.lcm delete mode 100755 go1_gym_deploy/lcm_types/rc_command_lcmt.py delete mode 100755 go1_gym_deploy/lcm_types/state_estimator_lcmt.lcm delete mode 100755 go1_gym_deploy/lcm_types/state_estimator_lcmt.py delete mode 100644 go1_gym_deploy/scripts/__init__.py delete mode 100644 go1_gym_deploy/scripts/deploy_policy.py delete mode 100755 go1_gym_deploy/scripts/send_to_unitree.sh delete mode 100755 go1_gym_deploy/setup.py delete mode 100644 go1_gym_deploy/tests/__init__.py delete mode 100644 go1_gym_deploy/tests/check_camera_msgs.py delete mode 100644 go1_gym_deploy/unitree_legged_sdk_bin/__init__.py delete mode 100755 go1_gym_deploy/unitree_legged_sdk_bin/lcm_position delete mode 100755 go1_gym_deploy/unitree_legged_sdk_bin/lcm_position.cpp delete mode 100644 go1_gym_deploy/utils/__init__.py delete mode 100755 go1_gym_deploy/utils/cheetah_state_estimator.py delete mode 100644 go1_gym_deploy/utils/command_profile.py delete mode 100755 go1_gym_deploy/utils/deployment_runner.py delete mode 100755 go1_gym_deploy/utils/logger.py delete mode 100644 go1_gym_deploy/utils/network_config_unitree.py diff --git a/go1_gym_deploy/__init__.py b/go1_gym_deploy/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/go1_gym_deploy/autostart/start_controller.sh b/go1_gym_deploy/autostart/start_controller.sh deleted file mode 100644 index e0b71b1..0000000 --- a/go1_gym_deploy/autostart/start_controller.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash -sudo docker stop foxy_controller || true -sudo docker rm foxy_controller || true -cd ~/go1_gym/go1_gym_deploy/docker/ -sudo make autostart \ No newline at end of file diff --git a/go1_gym_deploy/autostart/start_unitree_sdk.sh b/go1_gym_deploy/autostart/start_unitree_sdk.sh deleted file mode 100644 index d323590..0000000 --- a/go1_gym_deploy/autostart/start_unitree_sdk.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/bin/bash -sudo docker stop foxy_controller || true -sudo docker rm foxy_controller || true -sudo kill $(ps aux |grep lcm_position | awk '{print $2}') -cd ~/go1_gym/go1_gym_deploy/unitree_legged_sdk_bin/ -yes "" | sudo ./lcm_position & \ No newline at end of file diff --git a/go1_gym_deploy/docker/Dockerfile b/go1_gym_deploy/docker/Dockerfile deleted file mode 100644 index bae7629..0000000 --- a/go1_gym_deploy/docker/Dockerfile +++ /dev/null @@ -1,190 +0,0 @@ -# syntax=docker/dockerfile:experimental - -FROM nvcr.io/nvidia/l4t-pytorch:r32.6.1-pth1.9-py3 - -#ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all} -#ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics - -# add new sudo user -ENV USERNAME improbable -ENV HOME /home/$USERNAME -RUN useradd -m $USERNAME && \ - echo "$USERNAME:$USERNAME" | chpasswd && \ - usermod --shell /bin/bash $USERNAME && \ - usermod -aG sudo $USERNAME && \ - mkdir /etc/sudoers.d && \ - echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers.d/$USERNAME && \ - chmod 0440 /etc/sudoers.d/$USERNAME && \ - # Replace 1000 with your user/group id - usermod --uid 1000 $USERNAME && \ - groupmod --gid 1000 $USERNAME - - -# install package -ENV DEBIAN_FRONTEND=noninteractive -RUN apt-get update && apt-get install -y --no-install-recommends \ - build-essential \ - curl \ - sudo \ - less \ - emacs \ - apt-utils \ - tzdata \ - git \ - tmux \ - bash-completion \ - command-not-found \ - libglib2.0-0 \ - gstreamer1.0-plugins-* \ - libgstreamer1.0-* \ - libgstreamer-plugins-*1.0-* \ - && \ - apt-get clean && \ - rm -rf /var/lib/apt/lists/* - -RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections - -#COPY config/nvidia_icd.json /usr/share/vulkan/icd.d/ - - -USER root - - -#RUN apt-get update && apt-get install -y python3-pip && pip3 install torch==1.9.0+cu111 torchvision==0.10.0+cu111 torchaudio==0.9.0 -f https://download.pytorch.org/whl/torch_stable.html - - -# ================================================================== -# Useful Libraries for Development -# ------------------------------------------------------------------ -#RUN apt update && apt install -y apt-transport-https ca-certificates curl software-properties-common -#RUN curl -fsSL https://download.sublimetext.com/sublimehq-pub.gpg | apt-key add - && add-apt-repository "deb https://download.sublimetext.com/ apt/stable/" && apt update && apt install sublime-text - - -# ================================================================== -# Python dependencies defined in requirements.txt -# ------------------------------------------------------------------ -#RUN pip3 install --upgrade pip -# copy local requirements file for pip install python deps -#COPY ./requirements.txt /home/$USERNAME -#WORKDIR /home/$USERNAME -#RUN pip3 install -r requirements.txt - -# LCM -RUN apt-get -y update && apt-get install -y make gcc-8 g++-8 -RUN cd /home/$USERNAME && git clone https://github.com/lcm-proj/lcm.git && cd lcm && mkdir build && cd build && cmake .. && make -j && make install -RUN cd /home/$USERNAME/lcm/lcm-python && pip3 install -e . - - -RUN apt-get install -y vim -#RUN pip3 install pandas - -# ROS -# ENV ROS_DISTRO melodic - -RUN apt-get install -y gnupg - -# COPY install_scripts/install_ros.sh /tmp/install_ros.sh -# RUN chmod +x /tmp/install_ros.sh -# RUN /tmp/install_ros.sh - -# # bootstrap rosdep -# RUN rosdep init \ -# && rosdep update - -# # create catkin workspace -# ENV CATKIN_WS=/root/catkin_ws -# RUN bash /opt/ros/melodic/setup.bash -# RUN mkdir -p $CATKIN_WS/src -# WORKDIR ${CATKIN_WS} -# RUN catkin init -# RUN catkin config --extend /opt/ros/$ROS_DISTRO \ -# --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False -# WORKDIR $CATKIN_WS/src - - -RUN apt-get update && apt-get install -y freeglut3-dev libudev-dev -#COPY ./install_scripts/install_vision_opencv.sh /tmp/install_vision_opencv.sh -#RUN chmod +x /tmp/install_vision_opencv.sh -#RUN /tmp/install_vision_opencv.sh - - -RUN apt-get install -y libgl1-mesa-dev libudev1 libudev-dev - - -#RUN apt-get install unzip -# -#RUN cd ~ && \ -# wget -O opencv.zip https://github.com/opencv/opencv/archive/4.5.1.zip && \ -# wget -O opencv_contrib.zip https://github.com/opencv/opencv_contrib/archive/4.5.1.zip && \ -# unzip opencv.zip && \ -# unzip opencv_contrib.zip && \ -# mv opencv-4.5.1 opencv && \ -# mv opencv_contrib-4.5.1 opencv_contrib && \ -# rm opencv.zip && \ -# rm opencv_contrib.zip -# -#RUN cd ~/opencv && \ -# mkdir build && \ -# cd build && \ -# cmake -D CMAKE_BUILD_TYPE=RELEASE \ -# -D CMAKE_INSTALL_PREFIX=/usr \ -# -D OPENCV_EXTRA_MODULES_PATH=~/opencv_contrib/modules \ -# -D EIGEN_INCLUDE_PATH=/usr/include/eigen3 \ -# -D WITH_OPENCL=OFF \ -# -D WITH_CUDA=OFF \ -# -D CUDA_ARCH_BIN=5.3 \ -# -D CUDA_ARCH_PTX="" \ -# -D WITH_CUDNN=OFF \ -# -D WITH_CUBLAS=OFF \ -# -D ENABLE_FAST_MATH=ON \ -# -D CUDA_FAST_MATH=OFF \ -# -D OPENCV_DNN_CUDA=OFF \ -# -D ENABLE_NEON=ON \ -# -D WITH_QT=OFF \ -# -D WITH_OPENMP=ON \ -# -D WITH_OPENGL=ON \ -# -D BUILD_TIFF=ON \ -# -D WITH_FFMPEG=ON \ -# -D WITH_GSTREAMER=ON \ -# -D WITH_TBB=ON \ -# -D BUILD_TBB=ON \ -# -D BUILD_TESTS=OFF \ -# -D WITH_EIGEN=ON \ -# -D WITH_V4L=ON \ -# -D WITH_LIBV4L=ON \ -# -D OPENCV_ENABLE_NONFREE=ON \ -# -D INSTALL_C_EXAMPLES=OFF \ -# -D INSTALL_PYTHON_EXAMPLES=OFF \ -# -D BUILD_NEW_PYTHON_SUPPORT=ON \ -# -D BUILD_opencv_python3=TRUE \ -# -D OPENCV_GENERATE_PKGCONFIG=ON \ -# -D BUILD_EXAMPLES=OFF .. && \ -# make -j4 && cd ~ && \ -# # sudo rm -r /usr/include/opencv4/opencv2 && \ -# cd ~/opencv/build && \ -# sudo make install && \ -# sudo ldconfig && \ -# make clean && \ -# sudo apt-get update - -RUN apt-get install -y libgtk2.0-dev pkg-config -RUN pip3 install opencv-python opencv-contrib-python - -#################################################################################### -###### START HERE -- Install whatever dependencies you need specific to this project! -#################################################################################### - - -#COPY ./rsc/IsaacGym_Preview_2_Package.tar.gz /home/$USERNAME/ -#RUN cd /home/$USERNAME && tar -xvzf IsaacGym_Preview_2_Package.tar.gz -#COPY ./rsc/learning_to_walk_in_minutes.zip /home/$USERNAME/ -#RUN apt-get install unzip && cd /home/$USERNAME/ && unzip learning_to_walk_in_minutes.zip && cd ./code/rl-pytorch && pip3 install -e . -#RUN cd /home/$USERNAME/isaacgym/python && pip3 install -e . -#RUN cd /home/$USERNAME/code/isaacgym_anymal && pip3 install -e . -#COPY ./src/isaacgym_anymal/ /home/$USERNAME/code/isaacgym_anymal/ - -# setup entrypoint -COPY entrypoint.sh / - -ENTRYPOINT ["/entrypoint.sh"] -CMD ["bash"] diff --git a/go1_gym_deploy/docker/Makefile b/go1_gym_deploy/docker/Makefile deleted file mode 100644 index f5efcbe..0000000 --- a/go1_gym_deploy/docker/Makefile +++ /dev/null @@ -1,39 +0,0 @@ -default: build -build: - docker build -t jetson-model-deployment . -clean-build: - docker build -t jetson-model-deployment . --no-cache=true -run: - docker stop foxy_controller || true - docker rm foxy_controller || true - docker run -it \ - --env="DISPLAY" \ - --env="QT_X11_NO_MITSHM=1" \ - --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ - --env="XAUTHORITY=${XAUTH}" \ - --volume="${XAUTH}:${XAUTH}" \ - --volume="/home/unitree/go1_gym:/home/isaac/go1_gym" \ - --privileged \ - --runtime=nvidia \ - --net=host \ - --workdir="/home/isaac/go1_gym" \ - --name="foxy_controller" \ - jetson-model-deployment bash -autostart: - docker stop foxy_controller || true - docker rm foxy_controller || true - docker run -d\ - --env="DISPLAY" \ - --env="QT_X11_NO_MITSHM=1" \ - --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ - --env="XAUTHORITY=${XAUTH}" \ - --volume="${XAUTH}:${XAUTH}" \ - --volume="/home/unitree/go1_gym:/home/isaac/go1_gym" \ - --privileged \ - --runtime=nvidia \ - --net=host \ - --workdir="/home/isaac/go1_gym" \ - --name="foxy_controller" \ - jetson-model-deployment tail -f /dev/null - docker start foxy_controller - docker exec foxy_controller bash -c 'cd /home/isaac/go1_gym/ && python3 setup.py install && cd go1_gym_deploy/scripts && ls && python3 deploy_policy.py' diff --git a/go1_gym_deploy/docker/unzip_image.sh b/go1_gym_deploy/docker/unzip_image.sh deleted file mode 100755 index a1f1e84..0000000 --- a/go1_gym_deploy/docker/unzip_image.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker load -i deployment_image.tar diff --git a/go1_gym_deploy/docker/zip_image.sh b/go1_gym_deploy/docker/zip_image.sh deleted file mode 100755 index 1263cdf..0000000 --- a/go1_gym_deploy/docker/zip_image.sh +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/bash -docker save -o deployment_image.tar jetson-model-deployment:latest diff --git a/go1_gym_deploy/envs/__init__.py b/go1_gym_deploy/envs/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/go1_gym_deploy/envs/history_wrapper.py b/go1_gym_deploy/envs/history_wrapper.py deleted file mode 100755 index c187340..0000000 --- a/go1_gym_deploy/envs/history_wrapper.py +++ /dev/null @@ -1,51 +0,0 @@ -# import isaacgym - -# assert isaacgym, "import isaacgym before pytorch" -import torch - - -class HistoryWrapper: - def __init__(self, env): - self.env = env - - if isinstance(self.env.cfg, dict): - self.obs_history_length = self.env.cfg["env"]["num_observation_history"] - else: - self.obs_history_length = self.env.cfg.env.num_observation_history - self.num_obs_history = self.obs_history_length * self.env.num_obs - self.obs_history = torch.zeros(self.env.num_envs, self.num_obs_history, dtype=torch.float, - device=self.env.device, requires_grad=False) - self.num_privileged_obs = self.env.num_privileged_obs - - def step(self, action): - obs, rew, done, info = self.env.step(action) - privileged_obs = info["privileged_obs"] - - self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1) - return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}, rew, done, info - - def get_observations(self): - obs = self.env.get_observations() - privileged_obs = self.env.get_privileged_observations() - self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1) - return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history} - - def get_obs(self): - obs = self.env.get_obs() - privileged_obs = self.env.get_privileged_observations() - self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1) - return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history} - - def reset_idx(self, env_ids): # it might be a problem that this isn't getting called!! - ret = self.env.reset_idx(env_ids) - self.obs_history[env_ids, :] = 0 - return ret - - def reset(self): - ret = self.env.reset() - privileged_obs = self.env.get_privileged_observations() - self.obs_history[:, :] = 0 - return {"obs": ret, "privileged_obs": privileged_obs, "obs_history": self.obs_history} - - def __getattr__(self, name): - return getattr(self.env, name) diff --git a/go1_gym_deploy/envs/lcm_agent.py b/go1_gym_deploy/envs/lcm_agent.py deleted file mode 100755 index 656b6f9..0000000 --- a/go1_gym_deploy/envs/lcm_agent.py +++ /dev/null @@ -1,301 +0,0 @@ -import time - -import lcm -import numpy as np -import torch -import cv2 - -from go1_gym_deploy.lcm_types.pd_tau_targets_lcmt import pd_tau_targets_lcmt - -lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") - - -def class_to_dict(obj) -> dict: - if not hasattr(obj, "__dict__"): - return obj - result = {} - for key in dir(obj): - if key.startswith("_") or key == "terrain": - continue - element = [] - val = getattr(obj, key) - if isinstance(val, list): - for item in val: - element.append(class_to_dict(item)) - else: - element = class_to_dict(val) - result[key] = element - return result - - -class LCMAgent(): - def __init__(self, cfg, se, command_profile): - if not isinstance(cfg, dict): - cfg = class_to_dict(cfg) - self.cfg = cfg - self.se = se - self.command_profile = command_profile - - self.dt = self.cfg["control"]["decimation"] * self.cfg["sim"]["dt"] - self.timestep = 0 - - self.num_obs = self.cfg["env"]["num_observations"] - self.num_envs = 1 - self.num_privileged_obs = self.cfg["env"]["num_privileged_obs"] - self.num_actions = self.cfg["env"]["num_actions"] - self.num_commands = self.cfg["commands"]["num_commands"] - self.device = 'cpu' - - if "obs_scales" in self.cfg.keys(): - self.obs_scales = self.cfg["obs_scales"] - else: - self.obs_scales = self.cfg["normalization"]["obs_scales"] - - self.commands_scale = np.array( - [self.obs_scales["lin_vel"], self.obs_scales["lin_vel"], - self.obs_scales["ang_vel"], self.obs_scales["body_height_cmd"], 1, 1, 1, 1, 1, - self.obs_scales["footswing_height_cmd"], self.obs_scales["body_pitch_cmd"], - # 0, self.obs_scales["body_pitch_cmd"], - self.obs_scales["body_roll_cmd"], self.obs_scales["stance_width_cmd"], - self.obs_scales["stance_length_cmd"], self.obs_scales["aux_reward_cmd"], 1, 1, 1, 1, 1, 1 - ])[:self.num_commands] - - - joint_names = [ - "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", - "FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", - "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint", - "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", ] - self.default_dof_pos = np.array([self.cfg["init_state"]["default_joint_angles"][name] for name in joint_names]) - try: - self.default_dof_pos_scale = np.array([self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"], - self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"], - self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"], - self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"]]) - except KeyError: - self.default_dof_pos_scale = np.ones(12) - self.default_dof_pos = self.default_dof_pos * self.default_dof_pos_scale - - self.p_gains = np.zeros(12) - self.d_gains = np.zeros(12) - for i in range(12): - joint_name = joint_names[i] - found = False - for dof_name in self.cfg["control"]["stiffness"].keys(): - if dof_name in joint_name: - self.p_gains[i] = self.cfg["control"]["stiffness"][dof_name] - self.d_gains[i] = self.cfg["control"]["damping"][dof_name] - found = True - if not found: - self.p_gains[i] = 0. - self.d_gains[i] = 0. - if self.cfg["control"]["control_type"] in ["P", "V"]: - print(f"PD gain of joint {joint_name} were not defined, setting them to zero") - - print(f"p_gains: {self.p_gains}") - - self.commands = np.zeros((1, self.num_commands)) - self.actions = torch.zeros(12) - self.last_actions = torch.zeros(12) - self.gravity_vector = np.zeros(3) - self.dof_pos = np.zeros(12) - self.dof_vel = np.zeros(12) - self.body_linear_vel = np.zeros(3) - self.body_angular_vel = np.zeros(3) - self.joint_pos_target = np.zeros(12) - self.joint_vel_target = np.zeros(12) - self.torques = np.zeros(12) - self.contact_state = np.ones(4) - - self.joint_idxs = self.se.joint_idxs - - self.gait_indices = torch.zeros(self.num_envs, dtype=torch.float) - self.clock_inputs = torch.zeros(self.num_envs, 4, dtype=torch.float) - - if "obs_scales" in self.cfg.keys(): - self.obs_scales = self.cfg["obs_scales"] - else: - self.obs_scales = self.cfg["normalization"]["obs_scales"] - - self.is_currently_probing = False - - def set_probing(self, is_currently_probing): - self.is_currently_probing = is_currently_probing - - def get_obs(self): - - self.gravity_vector = self.se.get_gravity_vector() - cmds, reset_timer = self.command_profile.get_command(self.timestep * self.dt, probe=self.is_currently_probing) - self.commands[:, :] = cmds[:self.num_commands] - if reset_timer: - self.reset_gait_indices() - #else: - # self.commands[:, 0:3] = self.command_profile.get_command(self.timestep * self.dt)[0:3] - self.dof_pos = self.se.get_dof_pos() - self.dof_vel = self.se.get_dof_vel() - self.body_linear_vel = self.se.get_body_linear_vel() - self.body_angular_vel = self.se.get_body_angular_vel() - - ob = np.concatenate((self.gravity_vector.reshape(1, -1), - self.commands * self.commands_scale, - (self.dof_pos - self.default_dof_pos).reshape(1, -1) * self.obs_scales["dof_pos"], - self.dof_vel.reshape(1, -1) * self.obs_scales["dof_vel"], - torch.clip(self.actions, -self.cfg["normalization"]["clip_actions"], - self.cfg["normalization"]["clip_actions"]).cpu().detach().numpy().reshape(1, -1) - ), axis=1) - - if self.cfg["env"]["observe_two_prev_actions"]: - ob = np.concatenate((ob, - self.last_actions.cpu().detach().numpy().reshape(1, -1)), axis=1) - - if self.cfg["env"]["observe_clock_inputs"]: - ob = np.concatenate((ob, - self.clock_inputs), axis=1) - # print(self.clock_inputs) - - if self.cfg["env"]["observe_vel"]: - ob = np.concatenate( - (self.body_linear_vel.reshape(1, -1) * self.obs_scales["lin_vel"], - self.body_angular_vel.reshape(1, -1) * self.obs_scales["ang_vel"], - ob), axis=1) - - if self.cfg["env"]["observe_only_lin_vel"]: - ob = np.concatenate( - (self.body_linear_vel.reshape(1, -1) * self.obs_scales["lin_vel"], - ob), axis=1) - - if self.cfg["env"]["observe_yaw"]: - heading = self.se.get_yaw() - ob = np.concatenate((ob, heading.reshape(1, -1)), axis=-1) - - self.contact_state = self.se.get_contact_state() - if "observe_contact_states" in self.cfg["env"].keys() and self.cfg["env"]["observe_contact_states"]: - ob = np.concatenate((ob, self.contact_state.reshape(1, -1)), axis=-1) - - if "terrain" in self.cfg.keys() and self.cfg["terrain"]["measure_heights"]: - robot_height = 0.25 - self.measured_heights = np.zeros( - (len(self.cfg["terrain"]["measured_points_x"]), len(self.cfg["terrain"]["measured_points_y"]))).reshape( - 1, -1) - heights = np.clip(robot_height - 0.5 - self.measured_heights, -1, 1.) * self.obs_scales["height_measurements"] - ob = np.concatenate((ob, heights), axis=1) - - - return torch.tensor(ob, device=self.device).float() - - def get_privileged_observations(self): - return None - - def publish_action(self, action, hard_reset=False): - - command_for_robot = pd_tau_targets_lcmt() - self.joint_pos_target = \ - (action[0, :12].detach().cpu().numpy() * self.cfg["control"]["action_scale"]).flatten() - self.joint_pos_target[[0, 3, 6, 9]] *= self.cfg["control"]["hip_scale_reduction"] - # self.joint_pos_target[[0, 3, 6, 9]] *= -1 - self.joint_pos_target = self.joint_pos_target - self.joint_pos_target += self.default_dof_pos - joint_pos_target = self.joint_pos_target[self.joint_idxs] - self.joint_vel_target = np.zeros(12) - # print(f'cjp {self.joint_pos_target}') - - command_for_robot.q_des = joint_pos_target - command_for_robot.qd_des = self.joint_vel_target - command_for_robot.kp = self.p_gains - command_for_robot.kd = self.d_gains - command_for_robot.tau_ff = np.zeros(12) - command_for_robot.se_contactState = np.zeros(4) - command_for_robot.timestamp_us = int(time.time() * 10 ** 6) - command_for_robot.id = 0 - - if hard_reset: - command_for_robot.id = -1 - - - self.torques = (self.joint_pos_target - self.dof_pos) * self.p_gains + (self.joint_vel_target - self.dof_vel) * self.d_gains - - lc.publish("pd_plustau_targets", command_for_robot.encode()) - - def reset(self): - self.actions = torch.zeros(12) - self.time = time.time() - self.timestep = 0 - return self.get_obs() - - def reset_gait_indices(self): - self.gait_indices = torch.zeros(self.num_envs, dtype=torch.float) - - def step(self, actions, hard_reset=False): - clip_actions = self.cfg["normalization"]["clip_actions"] - self.last_actions = self.actions[:] - self.actions = torch.clip(actions[0:1, :], -clip_actions, clip_actions) - self.publish_action(self.actions, hard_reset=hard_reset) - time.sleep(max(self.dt - (time.time() - self.time), 0)) - if self.timestep % 100 == 0: print(f'frq: {1 / (time.time() - self.time)} Hz'); - self.time = time.time() - obs = self.get_obs() - - # clock accounting - frequencies = self.commands[:, 4] - phases = self.commands[:, 5] - offsets = self.commands[:, 6] - if self.num_commands == 8: - bounds = 0 - durations = self.commands[:, 7] - else: - bounds = self.commands[:, 7] - durations = self.commands[:, 8] - self.gait_indices = torch.remainder(self.gait_indices + self.dt * frequencies, 1.0) - - if "pacing_offset" in self.cfg["commands"] and self.cfg["commands"]["pacing_offset"]: - self.foot_indices = [self.gait_indices + phases + offsets + bounds, - self.gait_indices + bounds, - self.gait_indices + offsets, - self.gait_indices + phases] - else: - self.foot_indices = [self.gait_indices + phases + offsets + bounds, - self.gait_indices + offsets, - self.gait_indices + bounds, - self.gait_indices + phases] - self.clock_inputs[:, 0] = torch.sin(2 * np.pi * self.foot_indices[0]) - self.clock_inputs[:, 1] = torch.sin(2 * np.pi * self.foot_indices[1]) - self.clock_inputs[:, 2] = torch.sin(2 * np.pi * self.foot_indices[2]) - self.clock_inputs[:, 3] = torch.sin(2 * np.pi * self.foot_indices[3]) - - - images = {'front': self.se.get_camera_front(), - 'bottom': self.se.get_camera_bottom(), - 'rear': self.se.get_camera_rear(), - 'left': self.se.get_camera_left(), - 'right': self.se.get_camera_right() - } - downscale_factor = 2 - temporal_downscale = 3 - - for k, v in images.items(): - if images[k] is not None: - images[k] = cv2.resize(images[k], dsize=(images[k].shape[0]//downscale_factor, images[k].shape[1]//downscale_factor), interpolation=cv2.INTER_CUBIC) - if self.timestep % temporal_downscale != 0: - images[k] = None - #print(self.commands) - - infos = {"joint_pos": self.dof_pos[np.newaxis, :], - "joint_vel": self.dof_vel[np.newaxis, :], - "joint_pos_target": self.joint_pos_target[np.newaxis, :], - "joint_vel_target": self.joint_vel_target[np.newaxis, :], - "body_linear_vel": self.body_linear_vel[np.newaxis, :], - "body_angular_vel": self.body_angular_vel[np.newaxis, :], - "contact_state": self.contact_state[np.newaxis, :], - "clock_inputs": self.clock_inputs[np.newaxis, :], - "body_linear_vel_cmd": self.commands[:, 0:2], - "body_angular_vel_cmd": self.commands[:, 2:], - "privileged_obs": None, - "camera_image_front": images['front'], - "camera_image_bottom": images['bottom'], - "camera_image_rear": images['rear'], - "camera_image_left": images['left'], - "camera_image_right": images['right'], - } - - self.timestep += 1 - return obs, None, None, infos diff --git a/go1_gym_deploy/installer/install_deployment_code.sh b/go1_gym_deploy/installer/install_deployment_code.sh deleted file mode 100644 index 0d297ac..0000000 --- a/go1_gym_deploy/installer/install_deployment_code.sh +++ /dev/null @@ -1,22 +0,0 @@ -#!/bin/bash - -echo "======================================" -echo "== Go1 Sim-to-Real Installation Kit ==" -echo "======================================" -echo "" -echo "Author: Gabriel Margolis, Improbable AI Lab, MIT" -echo "This software is intended to support controls research. It includes safety features but may still damage your Go1. The user assumes all risk." -echo "" - -read -r -p "Extract docker container? [y/N] " response -if [[ "$response" =~ ^([yY][eE][sS]|[yY])$ ]] -then - # load docker image - echo "[Step 1] Extracting docker image..." - docker load -i ../scripts/deployment_image.tar - printf "\nDone!\n" -else - echo "Quitting" -fi - - diff --git a/go1_gym_deploy/lcm_types/__init__.py b/go1_gym_deploy/lcm_types/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/go1_gym_deploy/lcm_types/camera_message_lcmt.py b/go1_gym_deploy/lcm_types/camera_message_lcmt.py deleted file mode 100644 index a556fca..0000000 --- a/go1_gym_deploy/lcm_types/camera_message_lcmt.py +++ /dev/null @@ -1,60 +0,0 @@ -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -try: - import cStringIO.StringIO as BytesIO -except ImportError: - from io import BytesIO -import struct - -class camera_message_lcmt(object): - __slots__ = ["data"] - - __typenames__ = ["byte"] - - __dimensions__ = [[278400]] - - def __init__(self): - self.data = "" - - def encode(self): - buf = BytesIO() - buf.write(camera_message_lcmt._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf): - buf.write(bytearray(self.data[:278400])) - - def decode(data): - if hasattr(data, 'read'): - buf = data - else: - buf = BytesIO(data) - if buf.read(8) != camera_message_lcmt._get_packed_fingerprint(): - raise ValueError("Decode error") - return camera_message_lcmt._decode_one(buf) - decode = staticmethod(decode) - - def _decode_one(buf): - self = camera_message_lcmt() - self.data = buf.read(278400) - return self - _decode_one = staticmethod(_decode_one) - - _hash = None - def _get_hash_recursive(parents): - if camera_message_lcmt in parents: return 0 - tmphash = (0x1610a8a9f4d174b7) & 0xffffffffffffffff - tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff - return tmphash - _get_hash_recursive = staticmethod(_get_hash_recursive) - _packed_fingerprint = None - - def _get_packed_fingerprint(): - if camera_message_lcmt._packed_fingerprint is None: - camera_message_lcmt._packed_fingerprint = struct.pack(">Q", camera_message_lcmt._get_hash_recursive([])) - return camera_message_lcmt._packed_fingerprint - _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) \ No newline at end of file diff --git a/go1_gym_deploy/lcm_types/camera_message_rect_wide.py b/go1_gym_deploy/lcm_types/camera_message_rect_wide.py deleted file mode 100644 index 042de05..0000000 --- a/go1_gym_deploy/lcm_types/camera_message_rect_wide.py +++ /dev/null @@ -1,57 +0,0 @@ -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -try: - import cStringIO.StringIO as BytesIO -except ImportError: - from io import BytesIO -import struct - -class camera_message_rect_wide(object): - __slots__ = ["data"] - - def __init__(self): - self.data = "" - - def encode(self): - buf = BytesIO() - buf.write(camera_message_rect_wide._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf): - buf.write(bytearray(self.data[:34800])) - - def decode(data): - if hasattr(data, 'read'): - buf = data - else: - buf = BytesIO(data) - if buf.read(8) != camera_message_rect_wide._get_packed_fingerprint(): - raise ValueError("Decode error") - return camera_message_rect_wide._decode_one(buf) - decode = staticmethod(decode) - - def _decode_one(buf): - self = camera_message_rect_wide() - self.data = buf.read(34800) - return self - _decode_one = staticmethod(_decode_one) - - _hash = None - def _get_hash_recursive(parents): - if camera_message_rect_wide in parents: return 0 - tmphash = (0xc3e9f058530b2a8b) & 0xffffffffffffffff - tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff - return tmphash - _get_hash_recursive = staticmethod(_get_hash_recursive) - _packed_fingerprint = None - - def _get_packed_fingerprint(): - if camera_message_rect_wide._packed_fingerprint is None: - camera_message_rect_wide._packed_fingerprint = struct.pack(">Q", camera_message_rect_wide._get_hash_recursive([])) - return camera_message_rect_wide._packed_fingerprint - _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) - diff --git a/go1_gym_deploy/lcm_types/leg_control_data_lcmt.lcm b/go1_gym_deploy/lcm_types/leg_control_data_lcmt.lcm deleted file mode 100755 index cdae9c6..0000000 --- a/go1_gym_deploy/lcm_types/leg_control_data_lcmt.lcm +++ /dev/null @@ -1,11 +0,0 @@ -struct leg_control_data_lcmt -{ - float q[12]; - float qd[12]; - float p[12]; - float v[12]; - float tau_est[12]; - int64_t timestamp_us; - int64_t id; - int64_t robot_id; -} \ No newline at end of file diff --git a/go1_gym_deploy/lcm_types/leg_control_data_lcmt.py b/go1_gym_deploy/lcm_types/leg_control_data_lcmt.py deleted file mode 100755 index ef660c0..0000000 --- a/go1_gym_deploy/lcm_types/leg_control_data_lcmt.py +++ /dev/null @@ -1,85 +0,0 @@ -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -try: - import cStringIO.StringIO as BytesIO -except ImportError: - from io import BytesIO -import struct - - -class leg_control_data_lcmt(object): - __slots__ = ["q", "qd", "p", "v", "tau_est", "timestamp_us", "id", "robot_id"] - - __typenames__ = ["float", "float", "float", "float", "float", "int64_t", "int64_t", "int64_t"] - - __dimensions__ = [[12], [12], [12], [12], [12], None, None, None] - - def __init__(self): - self.q = [0.0 for dim0 in range(12)] - self.qd = [0.0 for dim0 in range(12)] - self.p = [0.0 for dim0 in range(12)] - self.v = [0.0 for dim0 in range(12)] - self.tau_est = [0.0 for dim0 in range(12)] - self.timestamp_us = 0 - self.id = 0 - self.robot_id = 0 - - def encode(self): - buf = BytesIO() - buf.write(leg_control_data_lcmt._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf): - buf.write(struct.pack('>12f', *self.q[:12])) - buf.write(struct.pack('>12f', *self.qd[:12])) - buf.write(struct.pack('>12f', *self.p[:12])) - buf.write(struct.pack('>12f', *self.v[:12])) - buf.write(struct.pack('>12f', *self.tau_est[:12])) - buf.write(struct.pack(">qqq", self.timestamp_us, self.id, self.robot_id)) - - def decode(data): - if hasattr(data, 'read'): - buf = data - else: - buf = BytesIO(data) - if buf.read(8) != leg_control_data_lcmt._get_packed_fingerprint(): - raise ValueError("Decode error") - return leg_control_data_lcmt._decode_one(buf) - - decode = staticmethod(decode) - - def _decode_one(buf): - self = leg_control_data_lcmt() - self.q = struct.unpack('>12f', buf.read(48)) - self.qd = struct.unpack('>12f', buf.read(48)) - self.p = struct.unpack('>12f', buf.read(48)) - self.v = struct.unpack('>12f', buf.read(48)) - self.tau_est = struct.unpack('>12f', buf.read(48)) - self.timestamp_us, self.id, self.robot_id = struct.unpack(">qqq", buf.read(24)) - return self - - _decode_one = staticmethod(_decode_one) - - def _get_hash_recursive(parents): - if leg_control_data_lcmt in parents: return 0 - tmphash = (0xa9a928b534bfc487) & 0xffffffffffffffff - tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff - return tmphash - - _get_hash_recursive = staticmethod(_get_hash_recursive) - _packed_fingerprint = None - - def _get_packed_fingerprint(): - if leg_control_data_lcmt._packed_fingerprint is None: - leg_control_data_lcmt._packed_fingerprint = struct.pack(">Q", leg_control_data_lcmt._get_hash_recursive([])) - return leg_control_data_lcmt._packed_fingerprint - - _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) - - def get_hash(self): - """Get the LCM hash of the struct""" - return struct.unpack(">Q", leg_control_data_lcmt._get_packed_fingerprint())[0] diff --git a/go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.lcm b/go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.lcm deleted file mode 100755 index d107752..0000000 --- a/go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.lcm +++ /dev/null @@ -1,12 +0,0 @@ -struct pd_tau_targets_lcmt -{ - double q_des[12]; - double qd_des[12]; - double tau_ff[12]; - double kp[12]; - double kd[12]; - int64_t timestamp_us; - int64_t id; - int64_t robot_id; - double se_contactState[4]; -} \ No newline at end of file diff --git a/go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.py b/go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.py deleted file mode 100755 index 88a3615..0000000 --- a/go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.py +++ /dev/null @@ -1,88 +0,0 @@ -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -try: - import cStringIO.StringIO as BytesIO -except ImportError: - from io import BytesIO -import struct - - -class pd_tau_targets_lcmt(object): - __slots__ = ["q_des", "qd_des", "tau_ff", "kp", "kd", "timestamp_us", "id", "robot_id", "se_contactState"] - - __typenames__ = ["double", "double", "double", "double", "double", "int64_t", "int64_t", "int64_t", "double"] - - __dimensions__ = [[12], [12], [12], [12], [12], None, None, None, [4]] - - def __init__(self): - self.q_des = [0.0 for dim0 in range(12)] - self.qd_des = [0.0 for dim0 in range(12)] - self.tau_ff = [0.0 for dim0 in range(12)] - self.kp = [0.0 for dim0 in range(12)] - self.kd = [0.0 for dim0 in range(12)] - self.timestamp_us = 0 - self.id = 0 - self.robot_id = 0 - self.se_contactState = [0.0 for dim0 in range(4)] - - def encode(self): - buf = BytesIO() - buf.write(pd_tau_targets_lcmt._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf): - buf.write(struct.pack('>12d', *self.q_des[:12])) - buf.write(struct.pack('>12d', *self.qd_des[:12])) - buf.write(struct.pack('>12d', *self.tau_ff[:12])) - buf.write(struct.pack('>12d', *self.kp[:12])) - buf.write(struct.pack('>12d', *self.kd[:12])) - buf.write(struct.pack(">qqq", self.timestamp_us, self.id, self.robot_id)) - buf.write(struct.pack('>4d', *self.se_contactState[:4])) - - def decode(data): - if hasattr(data, 'read'): - buf = data - else: - buf = BytesIO(data) - if buf.read(8) != pd_tau_targets_lcmt._get_packed_fingerprint(): - raise ValueError("Decode error") - return pd_tau_targets_lcmt._decode_one(buf) - - decode = staticmethod(decode) - - def _decode_one(buf): - self = pd_tau_targets_lcmt() - self.q_des = struct.unpack('>12d', buf.read(96)) - self.qd_des = struct.unpack('>12d', buf.read(96)) - self.tau_ff = struct.unpack('>12d', buf.read(96)) - self.kp = struct.unpack('>12d', buf.read(96)) - self.kd = struct.unpack('>12d', buf.read(96)) - self.timestamp_us, self.id, self.robot_id = struct.unpack(">qqq", buf.read(24)) - self.se_contactState = struct.unpack('>4d', buf.read(32)) - return self - - _decode_one = staticmethod(_decode_one) - - def _get_hash_recursive(parents): - if pd_tau_targets_lcmt in parents: return 0 - tmphash = (0x6d88128ef1291cc1) & 0xffffffffffffffff - tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff - return tmphash - - _get_hash_recursive = staticmethod(_get_hash_recursive) - _packed_fingerprint = None - - def _get_packed_fingerprint(): - if pd_tau_targets_lcmt._packed_fingerprint is None: - pd_tau_targets_lcmt._packed_fingerprint = struct.pack(">Q", pd_tau_targets_lcmt._get_hash_recursive([])) - return pd_tau_targets_lcmt._packed_fingerprint - - _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) - - def get_hash(self): - """Get the LCM hash of the struct""" - return struct.unpack(">Q", pd_tau_targets_lcmt._get_packed_fingerprint())[0] diff --git a/go1_gym_deploy/lcm_types/rc_command_lcmt.lcm b/go1_gym_deploy/lcm_types/rc_command_lcmt.lcm deleted file mode 100755 index 5953187..0000000 --- a/go1_gym_deploy/lcm_types/rc_command_lcmt.lcm +++ /dev/null @@ -1,13 +0,0 @@ -struct rc_command_lcmt -{ - int16_t mode; - float left_stick[2]; - float right_stick[2]; - float knobs[2]; - int16_t left_upper_switch; - int16_t left_lower_left_switch; - int16_t left_lower_right_switch; - int16_t right_upper_switch; - int16_t right_lower_left_switch; - int16_t right_lower_right_switch; -} \ No newline at end of file diff --git a/go1_gym_deploy/lcm_types/rc_command_lcmt.py b/go1_gym_deploy/lcm_types/rc_command_lcmt.py deleted file mode 100755 index 0310593..0000000 --- a/go1_gym_deploy/lcm_types/rc_command_lcmt.py +++ /dev/null @@ -1,90 +0,0 @@ -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -try: - import cStringIO.StringIO as BytesIO -except ImportError: - from io import BytesIO -import struct - - -class rc_command_lcmt(object): - __slots__ = ["mode", "left_stick", "right_stick", "knobs", "left_upper_switch", "left_lower_left_switch", - "left_lower_right_switch", "right_upper_switch", "right_lower_left_switch", "right_lower_right_switch"] - - __typenames__ = ["int16_t", "float", "float", "float", "int16_t", "int16_t", "int16_t", "int16_t", "int16_t", - "int16_t"] - - __dimensions__ = [None, [2], [2], [2], None, None, None, None, None, None] - - def __init__(self): - self.mode = 0 - self.left_stick = [0.0 for dim0 in range(2)] - self.right_stick = [0.0 for dim0 in range(2)] - self.knobs = [0.0 for dim0 in range(2)] - self.left_upper_switch = 0 - self.left_lower_left_switch = 0 - self.left_lower_right_switch = 0 - self.right_upper_switch = 0 - self.right_lower_left_switch = 0 - self.right_lower_right_switch = 0 - - def encode(self): - buf = BytesIO() - buf.write(rc_command_lcmt._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf): - buf.write(struct.pack(">h", self.mode)) - buf.write(struct.pack('>2f', *self.left_stick[:2])) - buf.write(struct.pack('>2f', *self.right_stick[:2])) - buf.write(struct.pack('>2f', *self.knobs[:2])) - buf.write( - struct.pack(">hhhhhh", self.left_upper_switch, self.left_lower_left_switch, self.left_lower_right_switch, - self.right_upper_switch, self.right_lower_left_switch, self.right_lower_right_switch)) - - def decode(data): - if hasattr(data, 'read'): - buf = data - else: - buf = BytesIO(data) - if buf.read(8) != rc_command_lcmt._get_packed_fingerprint(): - raise ValueError("Decode error") - return rc_command_lcmt._decode_one(buf) - - decode = staticmethod(decode) - - def _decode_one(buf): - self = rc_command_lcmt() - self.mode = struct.unpack(">h", buf.read(2))[0] - self.left_stick = struct.unpack('>2f', buf.read(8)) - self.right_stick = struct.unpack('>2f', buf.read(8)) - self.knobs = struct.unpack('>2f', buf.read(8)) - self.left_upper_switch, self.left_lower_left_switch, self.left_lower_right_switch, self.right_upper_switch, self.right_lower_left_switch, self.right_lower_right_switch = struct.unpack( - ">hhhhhh", buf.read(12)) - return self - - _decode_one = staticmethod(_decode_one) - - def _get_hash_recursive(parents): - if rc_command_lcmt in parents: return 0 - tmphash = (0xc7726b02ec3e7de2) & 0xffffffffffffffff - tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff - return tmphash - - _get_hash_recursive = staticmethod(_get_hash_recursive) - _packed_fingerprint = None - - def _get_packed_fingerprint(): - if rc_command_lcmt._packed_fingerprint is None: - rc_command_lcmt._packed_fingerprint = struct.pack(">Q", rc_command_lcmt._get_hash_recursive([])) - return rc_command_lcmt._packed_fingerprint - - _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) - - def get_hash(self): - """Get the LCM hash of the struct""" - return struct.unpack(">Q", rc_command_lcmt._get_packed_fingerprint())[0] diff --git a/go1_gym_deploy/lcm_types/state_estimator_lcmt.lcm b/go1_gym_deploy/lcm_types/state_estimator_lcmt.lcm deleted file mode 100755 index a7efc00..0000000 --- a/go1_gym_deploy/lcm_types/state_estimator_lcmt.lcm +++ /dev/null @@ -1,16 +0,0 @@ -struct state_estimator_lcmt -{ - float p[3]; - float vWorld[3]; - float vBody[3]; - float rpy[3]; - float omegaBody[3]; - float omegaWorld[3]; - float quat[4]; - float contact_estimate[4]; - float aBody[3]; - float aWorld[3]; - int64_t timestamp_us; - int64_t id; - int64_t robot_id; -} diff --git a/go1_gym_deploy/lcm_types/state_estimator_lcmt.py b/go1_gym_deploy/lcm_types/state_estimator_lcmt.py deleted file mode 100755 index e8fae52..0000000 --- a/go1_gym_deploy/lcm_types/state_estimator_lcmt.py +++ /dev/null @@ -1,102 +0,0 @@ -"""LCM type definitions -This file automatically generated by lcm. -DO NOT MODIFY BY HAND!!!! -""" - -try: - import cStringIO.StringIO as BytesIO -except ImportError: - from io import BytesIO -import struct - - -class state_estimator_lcmt(object): - __slots__ = ["p", "vWorld", "vBody", "rpy", "omegaBody", "omegaWorld", "quat", "contact_estimate", "aBody", - "aWorld", "timestamp_us", "id", "robot_id"] - - __typenames__ = ["float", "float", "float", "float", "float", "float", "float", "float", "float", "float", - "int64_t", "int64_t", "int64_t"] - - __dimensions__ = [[3], [3], [3], [3], [3], [3], [4], [4], [3], [3], None, None, None] - - def __init__(self): - self.p = [0.0 for dim0 in range(3)] - self.vWorld = [0.0 for dim0 in range(3)] - self.vBody = [0.0 for dim0 in range(3)] - self.rpy = [0.0 for dim0 in range(3)] - self.omegaBody = [0.0 for dim0 in range(3)] - self.omegaWorld = [0.0 for dim0 in range(3)] - self.quat = [0.0 for dim0 in range(4)] - self.contact_estimate = [0.0 for dim0 in range(4)] - self.aBody = [0.0 for dim0 in range(3)] - self.aWorld = [0.0 for dim0 in range(3)] - self.timestamp_us = 0 - self.id = 0 - self.robot_id = 0 - - def encode(self): - buf = BytesIO() - buf.write(state_estimator_lcmt._get_packed_fingerprint()) - self._encode_one(buf) - return buf.getvalue() - - def _encode_one(self, buf): - buf.write(struct.pack('>3f', *self.p[:3])) - buf.write(struct.pack('>3f', *self.vWorld[:3])) - buf.write(struct.pack('>3f', *self.vBody[:3])) - buf.write(struct.pack('>3f', *self.rpy[:3])) - buf.write(struct.pack('>3f', *self.omegaBody[:3])) - buf.write(struct.pack('>3f', *self.omegaWorld[:3])) - buf.write(struct.pack('>4f', *self.quat[:4])) - buf.write(struct.pack('>4f', *self.contact_estimate[:4])) - buf.write(struct.pack('>3f', *self.aBody[:3])) - buf.write(struct.pack('>3f', *self.aWorld[:3])) - buf.write(struct.pack(">qqq", self.timestamp_us, self.id, self.robot_id)) - - def decode(data): - if hasattr(data, 'read'): - buf = data - else: - buf = BytesIO(data) - if buf.read(8) != state_estimator_lcmt._get_packed_fingerprint(): - raise ValueError("Decode error") - return state_estimator_lcmt._decode_one(buf) - - decode = staticmethod(decode) - - def _decode_one(buf): - self = state_estimator_lcmt() - self.p = struct.unpack('>3f', buf.read(12)) - self.vWorld = struct.unpack('>3f', buf.read(12)) - self.vBody = struct.unpack('>3f', buf.read(12)) - self.rpy = struct.unpack('>3f', buf.read(12)) - self.omegaBody = struct.unpack('>3f', buf.read(12)) - self.omegaWorld = struct.unpack('>3f', buf.read(12)) - self.quat = struct.unpack('>4f', buf.read(16)) - self.contact_estimate = struct.unpack('>4f', buf.read(16)) - self.aBody = struct.unpack('>3f', buf.read(12)) - self.aWorld = struct.unpack('>3f', buf.read(12)) - self.timestamp_us, self.id, self.robot_id = struct.unpack(">qqq", buf.read(24)) - return self - - _decode_one = staticmethod(_decode_one) - - def _get_hash_recursive(parents): - if state_estimator_lcmt in parents: return 0 - tmphash = (0xea87c8282effe5b6) & 0xffffffffffffffff - tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff - return tmphash - - _get_hash_recursive = staticmethod(_get_hash_recursive) - _packed_fingerprint = None - - def _get_packed_fingerprint(): - if state_estimator_lcmt._packed_fingerprint is None: - state_estimator_lcmt._packed_fingerprint = struct.pack(">Q", state_estimator_lcmt._get_hash_recursive([])) - return state_estimator_lcmt._packed_fingerprint - - _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) - - def get_hash(self): - """Get the LCM hash of the struct""" - return struct.unpack(">Q", state_estimator_lcmt._get_packed_fingerprint())[0] diff --git a/go1_gym_deploy/scripts/__init__.py b/go1_gym_deploy/scripts/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/go1_gym_deploy/scripts/deploy_policy.py b/go1_gym_deploy/scripts/deploy_policy.py deleted file mode 100644 index 3c203d5..0000000 --- a/go1_gym_deploy/scripts/deploy_policy.py +++ /dev/null @@ -1,77 +0,0 @@ -import glob -import pickle as pkl -import lcm -import sys - -from go1_gym_deploy.utils.deployment_runner import DeploymentRunner -from go1_gym_deploy.envs.lcm_agent import LCMAgent -from go1_gym_deploy.utils.cheetah_state_estimator import StateEstimator -from go1_gym_deploy.utils.command_profile import * - -import pathlib - -lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") - -def load_and_run_policy(label, experiment_name, max_vel=1.0, max_yaw_vel=1.0): - # load agent - dirs = glob.glob(f"../../runs/{label}/*") - logdir = sorted(dirs)[0] - - with open(logdir+"/parameters.pkl", 'rb') as file: - pkl_cfg = pkl.load(file) - print(pkl_cfg.keys()) - cfg = pkl_cfg["Cfg"] - print(cfg.keys()) - - - se = StateEstimator(lc) - - control_dt = 0.02 - command_profile = RCControllerProfile(dt=control_dt, state_estimator=se, x_scale=max_vel, y_scale=0.6, yaw_scale=max_yaw_vel) - - hardware_agent = LCMAgent(cfg, se, command_profile) - se.spin() - - from go1_gym_deploy.envs.history_wrapper import HistoryWrapper - hardware_agent = HistoryWrapper(hardware_agent) - - policy = load_policy(logdir) - - # load runner - root = f"{pathlib.Path(__file__).parent.resolve()}/../../logs/" - pathlib.Path(root).mkdir(parents=True, exist_ok=True) - deployment_runner = DeploymentRunner(experiment_name=experiment_name, se=None, - log_root=f"{root}/{experiment_name}") - deployment_runner.add_control_agent(hardware_agent, "hardware_closed_loop") - deployment_runner.add_policy(policy) - deployment_runner.add_command_profile(command_profile) - - if len(sys.argv) >= 2: - max_steps = int(sys.argv[1]) - else: - max_steps = 10000000 - print(f'max steps {max_steps}') - - deployment_runner.run(max_steps=max_steps, logging=True) - -def load_policy(logdir): - body = torch.jit.load(logdir + '/checkpoints/body_latest.jit') - import os - adaptation_module = torch.jit.load(logdir + '/checkpoints/adaptation_module_latest.jit') - - def policy(obs, info): - i = 0 - latent = adaptation_module.forward(obs["obs_history"].to('cpu')) - action = body.forward(torch.cat((obs["obs_history"].to('cpu'), latent), dim=-1)) - info['latent'] = latent - return action - - return policy - - -if __name__ == '__main__': - label = "gait-conditioned-agility/pretrain-v0/train" - - experiment_name = "example_experiment" - - load_and_run_policy(label, experiment_name=experiment_name, max_vel=3.5, max_yaw_vel=5.0) diff --git a/go1_gym_deploy/scripts/send_to_unitree.sh b/go1_gym_deploy/scripts/send_to_unitree.sh deleted file mode 100755 index ccaa067..0000000 --- a/go1_gym_deploy/scripts/send_to_unitree.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash -# download docker image if it doesn't exist yet -wget --directory-prefix=../docker -nc --load-cookies /tmp/cookies.txt "https://docs.google.com/uc?export=download&confirm=$(wget --quiet --save-cookies /tmp/cookies.txt --keep-session-cookies --no-check-certificate 'https://docs.google.com/uc?export=download&id=1XkVpyYyYqQQ4FcgLIDUxg-GR1WI89-XC' -O- | sed -rn 's/.*confirm=([0-9A-Za-z_]+).*/\1\n/p')&id=1XkVpyYyYqQQ4FcgLIDUxg-GR1WI89-XC" -O deployment_image.tar && rm -rf /tmp/cookies.txt - -#rsync -av -e ssh --exclude=*.pt --exclude=*.mp4 $PWD/../../go1_gym_deploy $PWD/../../runs $PWD/../../setup.py pi@192.168.12.1:/home/pi/go1_gym -rsync -av -e ssh --exclude=*.pt --exclude=*.mp4 $PWD/../../go1_gym_deploy $PWD/../../runs $PWD/../setup.py unitree@192.168.123.15:/home/unitree/go1_gym -#scp -r $PWD/../../runs pi@192.168.12.1:/home/pi/go1_gym -#scp -r $PWD/../../setup.py pi@192.168.12.1:/home/pi/go1_gym diff --git a/go1_gym_deploy/setup.py b/go1_gym_deploy/setup.py deleted file mode 100755 index 2099745..0000000 --- a/go1_gym_deploy/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -from setuptools import find_packages -from distutils.core import setup - -setup( - name='go1_gym', - version='1.0.0', - author='Gabriel Margolis', - license="BSD-3-Clause", - packages=find_packages(), - author_email='gmargo@mit.edu', - description='Toolkit for deployment of sim-to-real RL on the Unitree Go1.' -) diff --git a/go1_gym_deploy/tests/__init__.py b/go1_gym_deploy/tests/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/go1_gym_deploy/tests/check_camera_msgs.py b/go1_gym_deploy/tests/check_camera_msgs.py deleted file mode 100644 index 240a156..0000000 --- a/go1_gym_deploy/tests/check_camera_msgs.py +++ /dev/null @@ -1,154 +0,0 @@ -import lcm -import threading -import time -import select - -import numpy as np - -from go1_gym_deploy.lcm_types.leg_control_data_lcmt import leg_control_data_lcmt -from go1_gym_deploy.lcm_types.rc_command_lcmt import rc_command_lcmt -from go1_gym_deploy.lcm_types.state_estimator_lcmt import state_estimator_lcmt -from go1_gym_deploy.lcm_types.vicon_pose_lcmt import vicon_pose_lcmt -from go1_gym_deploy.lcm_types.camera_message_lcmt import camera_message_lcmt -from go1_gym_deploy.lcm_types.camera_message_rect_wide import camera_message_rect_wide -from go1_gym_deploy.lcm_types.camera_message_rect_wide_mask import camera_message_rect_wide_mask - - -class UnitreeLCMInspector: - def __init__(self, lc): - self.lc = lc - - self.camera_names = ["front", "bottom", "left", "right", "rear"] - for cam_name in self.camera_names: - self.camera_subscription = self.lc.subscribe(f"rect_image_{cam_name}", self._rect_camera_cb) - for cam_name in self.camera_names: - self.camera_subscription = self.lc.subscribe(f"rect_image_{cam_name}_mask", self._mask_camera_cb) - - self.camera_image_left = None - self.camera_image_right = None - self.camera_image_front = None - self.camera_image_bottom = None - self.camera_image_rear = None - - self.ts = [time.time(), time.time(), time.time(), time.time(), time.time(),] - - self.num_low_states = 0 - - def _rect_camera_cb(self, channel, data): - - # message_types = [camera_message_rect_front, camera_message_rect_front_chin, camera_message_rect_left, - # camera_message_rect_right, camera_message_rect_rear_down] - # image_shapes = [(200, 200, 3), (100, 100, 3), (100, 232, 3), (100, 232, 3), (200, 200, 3)] - - message_types = [camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide] - image_shapes = [(116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3)] - - cam_name = channel.split("_")[-1] - - cam_id = self.camera_names.index(cam_name) + 1 - - print(channel, message_types[cam_id - 1]) - msg = message_types[cam_id - 1].decode(data) - - img = np.fromstring(msg.data, dtype=np.uint8) - img = np.flip(np.flip( - img.reshape((image_shapes[cam_id - 1][2], image_shapes[cam_id - 1][1], image_shapes[cam_id - 1][0])), - axis=0), axis=1).transpose(1, 2, 0) - - if cam_id == 1: - self.camera_image_front = img - elif cam_id == 2: - self.camera_image_bottom = img - elif cam_id == 3: - self.camera_image_left = img - elif cam_id == 4: - self.camera_image_right = img - elif cam_id == 5: - self.camera_image_rear = img - else: - print("Image received from camera with unknown ID#!") - - print(f"f{1. / (time.time() - self.ts[cam_id - 1])}: received py from {cam_name}!") - self.ts[cam_id-1] = time.time() - - from PIL import Image - im = Image.fromarray(img) - im.save(f"{cam_name}_image.jpeg") - - def _mask_camera_cb(self, channel, data): - - message_types = [camera_message_rect_wide_mask for i in range(5)] - image_shapes = [(116, 100, 1) for i in range(5)] - - cam_name = channel.split("_")[-2] - - cam_id = self.camera_names.index(cam_name) + 1 - - print(channel, message_types[cam_id - 1]) - msg = message_types[cam_id - 1].decode(data) - - img = np.array(list(msg.data)).astype(np.uint8) - img = np.flip(np.flip( - img.reshape((image_shapes[cam_id - 1][2], image_shapes[cam_id - 1][1], image_shapes[cam_id - 1][0])), - axis=0), axis=1) - - if cam_id == 1: - self.camera_image_front = img - elif cam_id == 2: - self.camera_image_bottom = img - elif cam_id == 3: - self.camera_image_left = img - elif cam_id == 4: - self.camera_image_right = img - elif cam_id == 5: - self.camera_image_rear = img - else: - print("Image received from camera with unknown ID#!") - - print(f"f{1. / (time.time() - self.ts[cam_id - 1])}: received py from {cam_name}!") - self.ts[cam_id-1] = time.time() - - from PIL import Image - # print(img[0]) - im = Image.fromarray(img[0]) - im.save(f"{cam_name}_mask_image.jpeg") - - def publish_30Hz(self): - msg = camera_message_rect_wide() - msg.data = [0] * 34800 - self.lc.publish("rect_image_rear", msg.encode()) - time.sleep(1./30.) - - def poll(self, cb=None): - t = time.time() - try: - while True: - timeout = 0.01 - rfds, wfds, efds = select.select([self.lc.fileno()], [], [], timeout) - if rfds: - # print("message received!") - self.lc.handle() - # print(f'Freq {1. / (time.time() - t)} Hz'); t = time.time() - else: - continue - # print(f'waiting for message... Freq {1. / (time.time() - t)} Hz'); t = time.time() - # if cb is not None: - # cb() - except KeyboardInterrupt: - pass - - def spin(self): - self.run_thread = threading.Thread(target=self.poll, daemon=False) - self.run_thread.start() - -if __name__ == "__main__": - import lcm - - lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") - #check_lcm_msgs() - print("init") - insp = UnitreeLCMInspector(lc) - print("polling") - # insp.poll() - while True: - insp.publish_30Hz() diff --git a/go1_gym_deploy/unitree_legged_sdk_bin/__init__.py b/go1_gym_deploy/unitree_legged_sdk_bin/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/go1_gym_deploy/unitree_legged_sdk_bin/lcm_position b/go1_gym_deploy/unitree_legged_sdk_bin/lcm_position deleted file mode 100755 index 43d1e66b0fc041b79c7e7f047a30202df904ea2b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 95968 zcmeEve_T{m`u~|3ehi|BzmQB}2Fuh+0TH>CISe59BO)MkZ9mPO83w`lB_K++F(RmC zwT`*j_Sv-o%@wv zJ$b)K2d}p62WIaJP(A-b`wZ{Fiv$9Vk=U z9Vl0p(^L2TvW~!BEzN-xt#JAIseLAX&<$XLM~7Dey1Xh9_3AW>B;(} zGLPcxPDtQo?>}>T%3CRS2g(tjKlBOb8MAqP{^XLu<)XZ0DKIvNI(s!^FlhcPIPB=;rf8eh9gYq+O`{%5wNpVLf zy!iCIs``~kvmp>2VqtoCjDBqMkRhgRvFqKb?E!TOd7TTk{o=I^`yPJPon;WOb9b;>F9eoBLpSuAn19??`^3=U`!uNo3lhc;+;UaH?2v)FENysQq||KUCl4VXfu4m3 z1qg)*ixBQbpr;t21fdk6451vM0$~ZlQiS^u?nn3)0zD5PtdL@i)$;c$yw@N+$l-_Z zUW@QBhw1eQkN+C)^*sI<-Wz!QalAJoJb~~e$0@JhBeF>W@OJa~X1uo`JdN-Ngl2?4 zB0Pie90EPt5q2Q#LU<9O1>q%xml5_MbRfKnK+k@J0|;**{276sw-MfzV)CE&@H&L> z7lii_jv&x;6ycZ@W9;vE{{!JegpUw95$HLN(1maU;S+>U5l$kUMxf_Q`Nh7%`)h=6 zIs6@epTnD6#`g$4goCEF2;?SsmnSS62zU@e7=r#@eeEMVhA%F^ZimA<^A9y|-1O}3 zU)<7qNAE2$k1QWB^6y`bUHrYa-CXZ_cJ9i+__tTKJo0tebmyH%W}f=umY!>G$-LdV z=gwXF2TZ?;{&GzI{U6=_$TP7MUp;j3PwPkS`uJ~epV%bkY#U!uKlkbpf7>zWw%K1i zr62o9;;VC4e|-FdPaK+-*9x{Kgq>PdIj3;a)pO@Pm9Twf(laya3g=CEXXQ)J{vvSW zu^ms{zHnSi>*U_w&UP0V*cj`^s&|Uadmh~yZ~NO9o7cTJ@b|C3^5=JV4_|yi|K2y> zEUa@~fAZz`>V;RAu+#dSd4}OvV{sZ=^ zFF$>J^qp%T`@@bG>XvK>{_Dj*wVqwkx%t%dMQ5}ZUAx|{bN_Ah_M3m5`NdP8)qnfj zgx8Oc_+at;(TBR9DSct>}brj4P+6V|^x=)d;d z`Oha8)}Cs(7(RPj?8K+qe{+NWO(Mq#XIjs1)6Z)~`RfW07^xCkwt(cQ?tN)qiv>l$ANu7&)$Ek#NA;OA;ah z4!r~;1B1~oeBloV`Nn?}=o^2ppZtY>@}KgPUjsh-hH@3Z`F`@>) z{uq`1l;|8ka!B!0?>0a9C;Z?m{Ludr6E0tT()j4h-rn&;XR04M<$mNd+fV+_{qX<2 zpZ@rJKl!)#;qxs&{G9V+Kka_-9e(JH@q?e{hfb=Wc6Iq_7tM2g>8s99y@7t@@G<1? zOTQQW@Od@(^p*cDKm3b+`24^R{USg4BmA^$jGuOu_>tRVe(3zx4}PT|eyo1z(|pU9 zUL1bpWAVd(hadd4e)zoLryn)=(bpjGX+ZFPuJO~ZY-IG+F1Me0*NB#T+9k{|MH}_Z4W}O3$im)?U3^sc>aPha{dgj{DD!NJ`F&4w#hH{-e{TsGo1es z9B&)U=^vF5zvTEJO!cT;v5hifKCkz0z!N`iTIk`SwI4n28wr2;yfGs}=CjO;&jr}X zA^EHemI;Hb^gN9AlANG=%yNpuXHzMkXTq4FWHhpZuTyPO>FfFV(S{dyTu$?;EtVd6jbO&K9_ zd@AS=K9UyFcuvXW*j8?z;d&XN*k>Y)pY(Eu%TvL}f*d={8KLN9^H`amEY43P&)+#prqfLuDR|cK zg!iuE{Lsb+9z{MI;e3dnb)5e^IhlP9MUZ^9y&xmzbNq7?<^1iuT?#)MG?45woHict zERbL9UGPtIoDa*0RF1Dfe!}lAml2A7OQL1Gvaiasv!|tgF_<0X_HW<`DLntd4jHk9 z<5yx`AjzNG!y1l%7ZnlyK$DD!)ynS=V26Y+*dW7oJb&VNnV)5^$OuIaOSzoI=VW{m z7sOc3PtvP0J_~-Fp6@}2=)1Yx3=$T5cbuO!obgJ|&nnmn$>#u<+fj~x9rVc#3%vUE zwQ#)DFY-sq8F{tr3@V~}yJ>R@53h#(8T^o6E%5R`Bf*bk=U)D~liT@t-XE`#$*~r6 z1frkutc)1WE4~Brm+a>?8Q;nC{}Xbf{N8?J7wnYsd+YH-?hg&TKW^i6_6?x+Mlu8E zAE5L+%KP=2{W5%%gWNB+bGyZGBRzlP{&4pJ8P4GNKXAWl;{2eSNY6w+^fRD$lIQL}$oUle zOyd32+y5*9|CB$R3zENBG3b!qmwAmlX1&b+Ixqg8f`6m_p7b}l-nsIN1;QQ(zwVff zP{z$wXcyUMJC_f#NzYV_U!=!aj^}31{sDy&KL@zp$MgJ8a=kcd6BUn1`pZ}(+DrMf zIR8ric-R}&yWOkaZ80+bARk8pIGw+8d5&+C5oo^jT+QbdS)7i74}t#4PS)&}^S#RR z2Z8~@@BWL7P~=bzy%RqNIUOg@|A-|2!R!o|+f0ssiQBEn=_~!N0`nOfue&?tj0*pY zc)ij_GLnioozH{i{_gbR-vl{JcFz590?(g`ae?qzEi&Q;u9)9WknP9G{c0J{zu_tw ze~SAzMLriHXJ7XA9G|x=XshC5BM_%Ty zMo9b&Vx62HMGo_!U+UL-Zl6h<&KKy{B>yK4%LoJa|BK*f3_kWnZ-rjS{@DmwA^BV{ zUvWF}p2wbqy%GI`eEiMh^?t?YR|mYt(-}}9<@cVi{$-eK|I57ec!2v`Z~u8CpBH)i z;X5TdmI!CvWh2xAT6j^v5?0a=q=`e}?l42J!x@=i`pT|7qyUm;8INz9YPN zK;|=7ez8w5{z~@A`BD7*eDF#6+c`hVc(DoXrTp8t-v)cq^CA2m*~wAvpRdDufSygL zm+X1XZ)Dghzu0e}cfuz%$%uzJe%MgXCzn5O9{XSTPpX%B)q4m00MYTD5C2ZeKbV=g z{m zXz)pLi~VmI-wpew=ONG`Ihefcf#F=d$X+a;gj;6i-e$>jlsgs{R#ZC5b8ky2DlKv3 zT5UxRX0a?RE-kTCR9ed`Efywa^_daWEEP*^754H%n}ZRQ=cuSGFRhm11=f;0P$P8N z5?fJWMS&EzmpiPLKo{C7D)a2uUr+obva-qo;PWgz)v_RWUS?dZ#bRH!%xWvFii?|u zc#*ZD!ctjX=CBl&!~%YvLr0o)fCmVjgx;LM!Lq{7k)i_Kc$NSIYpSeY7^2E1kA zvSpSsM|nkQiM6P(vf5G=D`hU7QnsWr&5pQNb@8aWqR>~3nR9X~r`m~$S@yKJDVEzT zOAGTHCE%#AlByyW1XN#Ww}6pZ_MFPNID3J$+)`O?Evy70Ep4j3$YCu{LrT157MMk- zB#~UsMV!S_RqUG$l%7()q@=R2*kN&$mzS2O#HVG?$$`2cGb(&DzyV^aV)bt$|?&>OOTAJ5-k=m2@Msap%squ zN}mnOCZ^=J^W3v?ExEClw46js)|{Lv7Gfo*GQk1E_|XJV6wG@yIyGLFIh5j+lCZSE zDl3HAj`l6cvr00MTLT%CE`^!E;;a>wK20Y@G9Ae5GTK~N3zt~S^DN~KXtRQ}*Du?I zMWux$+!{&+*a#O|aavk-PH8Tbq{_?{!JAUDlz7dloMJB|*=ObyCKfI%DJ^$Mc1)_3 zSeNp|2^JZ$6qPQ7!AW+IHHYl^5;IN8E0wKOGWE-)2-A^DNtPqG%4%P{q_7;Vl5qLu zj!R9Vzo~Ik(LC8z$aY0`9lNBYaPbm{rL3|X7H3gR4JiqEj(qEqqDpv*B1ffTR!*EH zEiG$iRTbGGs8Tmywk$3#E?%}H7+qn>LSM#`W2L@C!j*kST6VE4xdeOZl1hnM>D2t9 zB^3pVB8n=~3d!$CE-;bh7nkPc6THxFw^VSqcxTDd!je2owxeQ6vE-zdM&6-3 zKhFXtC<<-LMJ@J%ds#)1!%@Z{V0&?y(*4mIR9EaMMhR9?u%t4tbZH5*OKzc$8u!sC zS>%9?$`%3_TvT`;cw16gK?Lk&)kq@7xpfpnr~8<%uv=B(W8v^2}ywDk0}R7+0koGEvvX2ng* zaa5)jR&bNZh49nz^HCzVD%Mhx5NpYtYAY?RfMe}*wCE9)XlY!mokn6yVR2az`cAAR zPLR|r4SD#p2yH8sdR5#sa$osNO6=sP;m3-K((uS5-hu_0@v&$Px$+9=-iksN48@hx zU>X@#>J(`S1$Yxrv;HMFG@eoq&COA=`ly@)l9;ljqO!CYzLQh#8*k#`HIT2V+Mom*IpF+H0c*xb^kDaCnGrM`+vDvzW226CmsoTO6I zG71+K@M4%>{EU*wEhxbdt;pkNu&G$thjQ)4O_k?%$&%&GO}P!jN9Gh*SKO3rhuu+F zk@==*lUGkxukJl!jG>gkxcVPOPIGtwyP;ByZV_l z#e1gDMoc6BjB!hC*Ozy(D^WIz*yuUqa?eIn<5)FCNYG*%+`4a z^T7x=2)|zJM>(x4HH*3yInx|?T<=~r>vlNoyuuR8k_w09j(;Y_C1R3+xwhN~e2FzK zP8MK}G@~ZTLK{o_p}46Es7TtMG4~A~$#%P=q5>;6NB`jA({kcs^Yd=6!V-?g7?6cG z_r;>@L#f&yDI{OLeXo+LirQ^eYpr!v(^-2@u#ez@K zOD3G|sDy8Ri!6oMKA^b~EGOO~O)gT)ZkOi>{+%CRbo%OM$|NE_t8}TOJgdC45^P`MS1I!U0ZY2S3lx&bZ1r0S)-M$(@xse$Tmv0)#4F0FOCGW!t{aYq7{X}^N zoTN-?amu%*tw2{3mtj6+;9I_li}ue$!d|C z2c}d%sezIubmsz;b&RXXSb3p6+Ys#OKqi{-|XTwXsjI zmyW?cB}D2==<7s&vs}p`a>G&vBY8d~xi*r3UlsqTW?@Gi$GcPL=r zq#b*o?H6p-I7g^hYAs7!RPX~&41V(Yg`_{6A_&K3l-j z6bDP@snXFX@hTsz*(;V{mjefOe)J}JCS`D;eFuR~B;;*LvZE`wpC4~WKDke%j{bwT z0wvvEsguRUFD)-ES%@u_yd~ID0sryRF?;1)>|9&RXO&wL{TAb(QN!!7ElvJqx zhdQG4orTm9{ixMZ(zj~MvS-CjRlaSJ&wMLsC5J978QWz8my+<47C}B$FqMv_(n|0x zNNGv2qeT8D0@eJ;2@#j&CDUKOR;x_BX)SU2#QW#l=u}M&eCkFvvMcP9Ng0xqgKv3` z@$;Y9{;R{3e|OcVtQLIP2lbi&FRf z*$*}4Ps5~76v&g(M;fTsr{M^Bjw(kHZT|VS`Mz<5yBj}Cg1#{fi^l&{7nB{_W!tBT zIN!H8ip%ijiF}TlcAvlYNh5_ahozC?Ul{-uz?8(_u9Ht8PPLcYr{GhgMUn?p=6gS2 ziYCcSfbS}=o{1q_>Xy>J=MUe7@-3qH#H!Ll)i)HSwnZ3FrH>L~|5F>cRcVPbNje4a zwb@KM`OgeZ|Eiyp!h9`$`X>UHpY%%h{vR+@We?b&4-#N~v&ypYJrI41CQWI6(sWF! zCw=ZLbBZN?Qrt9_o|0mjf={<3)cpB~Crp}p2^1f92^bg0Qs&Q3HqWxeO`0-ExFk(b zC0&vbAFl@4WPB29t+W?RURaC|7)mGOb2xlF>9Ana6gyCsioAQJAE~mJl})ym7bhmj zAMP?O{^_KD0r(pz#kBaXE*TdjVYK+Is{TCmOI5+hLI3n99V`X8ER`gBA)KlnzgDFK zP6wF&1>=q^eo+X4Fmf&Q3s^*B0R9pdpTIMGo{gMTXE44tCW@iRCD)8!9g^}tIaT^sC~;}|pi9b6 zQI(hJ2V9c=(q-v7Rl1%%3Oajk zz;6hf$l(X2@_IIn!-xNLFKQ2FX&f$>$_KJ64p&ZCk5BBi%mSDm<@fV+{JupQzX#FI z?^!DN?PYTQ177$u9DmRYAG=u2f7A;{%CFqF+UV;SCFL!$3m3wNZyzt6>z%yQW<@Xl4z3|F?d_BKUskB%59Rq_G zUb)vf-V3kXXOH#5_uU8d!YlXav%=&HR7$)quvXz{9Z@6 z7ryUzV7&0ky-tG{-oWKK-V3kXw~h6}XK_AFUU=pAb27Z}2JUC)d*PLPnxYq;@%#l| zc*W0@dEu4uWtkUVxmW1)!XM=PukpewzcaJW3x9y~^Mn^Zmebkhg;(xDZ}-A0zq8Tq zgPfU&sBD z2J4QYDmrcz9#@t7o+NIElz(a;7QeTx!Yg;fDcP;U-=@mHTZO+X^Rpk%o{e|Q(Q-$9hD*ItA+QqMuOR4QDJd*mJ11dZgdVSA96`o&l zmeP)@@U%~%Je?}Ma_5@TPO0$tNr%4Yj0zvxhedl;c-jN$dj|Hm3s>*^9=!^WpGxR^ z!c}-&neTfdRd~ghP@+MF#})FvXS@oJpG4?;VpaIzeOUZ{y$X+8_kE8^g~v|;^gS6W zJbr?s@5xf(ab>>mnXkg*rwsZYQH3AXhsE!atMK@#gubUtg~v}H^gYW|cy)hqs_^)U zfWBvq3Xd!Hea|`--q44|Z;z|+Mit(z!pEra+f;a5vG04ftMJ$MVX^<9!jDto+g14M zRQLlb{CE}qpbC$l=IDEls_=Y^M@s8d;rUjGgg>Ri-_S>eu`??CjVgS%3O`AOXM_6f zf3ga%SK(t-_;3|IPKA$D;p0_!g9<-Ig&(iND?6r?9IL`7s`4kP@Y7UylL{}W@EI!n z%_@AB3V(|VKVOAc`+re|zg3mLK!u;7!k4M=CKY~}3ZJaPJ5_kI3cp5$Pf_95sqm>P z{1YmCnhNh$;nn_pn+l(<%D-KO&rsoatMJNicTjS>3O`$w|9}cVM}{sAg;&puVpaJ6Qqf6L;qOu5O)9*o!uKrS+tj0Du7NtXp=bHNZS6YC5mhofgY<)p z9UO8j%@f`+$z{++ABi^T-dBQ@_94=`J8qjqk62 zCg2zqJR7iC1!n@bso-qDwJLZX;6@d^0C0y2z6WrZ3bp~(PU){dA8?EcUIf^zf=d9~ zRB$=qS`}OcxKRb)2e?B8KLEH(1=j-BCiK_83UG`Heh9Ex1wRVdrh?Z4u2sR018!8o zPXg{x!J7bgso+My+Nu5ZZv`Bqf|~)GRq%6wZ7TSAz_luP7vM$}ya#ZH3Vs=ImkRCx ztWE5%e?Qi<~Bnk}Q*;%Bui*Tzg9rg3JQ*uqULAfsm1g|fpL z7YYuiUl0$^yfFW8+J&scDHk#hvj9z)VT>-5jS0-`937n5J~|}RJvuZ~96c!0GtAed#}JxV@`X<=$z`(209P*EN7B$w`Zh#zRA%v9Y%T{c-UeI z_5?8Jq3`b2HdQpIdv=`AyUyEnO`dxLnmTpEnonKNdT#3N-AM5R7kf8KG);*T&2P}2 zDLieTlE$XF%sS^GO1pul?Lk^Q>SKRT_xv|-8t~ew8{RxIjBTKIlAdkYbt2G3v^#YX z%|_=m*J~QKfy!(vo!z!)rN*_}9_(Uu>7L`dQO)j}5bze+yyIr(*?B_eY8*VPsc~>n z(~cbG+4~jqj0$HPUVQhiCbyZn+%?S8s~z^o&QYfC4KXbDg%swgxs`prEsEvh_u#%} zMwZ(JUWY}qoiD6po_(m(QZOayMSFnD*qQF>v@uu9ySFzPP0ZD>a!At*-LO9zt~Pyd z8Vy?MGOYs9M|HcuLi(SW$G9flBh~q9<`K1QhZ{KONH$H^U9(=xoY}`?G%hxlx$MxP z$Yva&as~}{yfkbYLmnOQoAJ}s8ssq}f2U5 ze4##HvTBI&l2xh6Bcu>tS4py(55A~vXF;3faz=WOZ2l5&ngf~8pM#O-VsG!r2Gl`) zzzCgSUu2pQIvLA#@~`T-mDe*#>K`MUZ{Ypp0%S`4@Oh*YZQ|itqyoZg&p{- z_cB`mk2@u~kNB+pLT|1CbU)^_y6}db9cmb>b#+26zeid_y2;~yjCmUVn|Zpx$8p_N z&5!l;ZhQgwhQ~~v6kfJdH?sMk=z~94|9efIBN87GpS5@Q=3WHNS3#?B@IBCJM3eiU zsP`r2xdXTuok^fR*7!<*a2;R+Y?ZM%*FR83;#UWLVS|f_-xQRCEi|lF_#N52koX0@ z5p54ZdD0WffMhmnd&Y=F>xvA@23NT)JSA`iV^!@Fns z%aV`%CX{)|zgB29;AL2I1bBfj+kPDJZ$eET>R;?ay2or_uAb!`+dk~+m9|?hD<4B` z8P*(x@>G^+DCMafl_xsy_$zPLx+-i;qVXO0y%LQ!&=>=KMSc4&J+I(xe>L6nD&BQ}N%yP)&65aH`p4-W6a38-$VeH7fIrAOnT!0I5pS${KXa3> zLwV!DbWbw!8(8!a%1`~rJHL^!w~QB39y4A@UW@Qp1M)+5+s&Utc8HsJS#PG1{vV1#4aX~JV}2%LdZLCX+ zv3nJGvPPwP$nF|?dv`jukUjZh@TfzY2YeCzp5-rHLXUVl_mQ?~q=~tlv(r82P+kKc z({LU0V2nMqREzNubtVR~9ptxO1@G>$%<~<}Mc=}{F2Fn_I)Ht>PseiGVdtWm3GT5b zj}`TcLCmuOG-nhTk{Yfvd59+iV{f{(Z09#Q$xY`zGB@oUMzVWNlAYZ5WP4u^p3kEW zqD4H21E~LvfK5ffXN_!L1wKASo7@+AcYKd`qa;(gAAg2;&+=VTJ84exKIBI86>95d z@Do8Y#{6XD(QubEf4QAW^B0=0{PTK-`N7EMQ`ZF*YhyE7sLkd8HpCXMZ}}pUZLq!^ ziHHtT}9Z5TV(i}%cJ(}t%Ww&l-l3mBeu z*cguaT0rt)@uA4-yhkIe?T<%R+crg3Z+nw$3;VXQM2!2{HTMl{ zv1`L!Vs>Qp!sgUAt2WY=|6FQYp4Q;98Zug2R%%_2U8!xmPiS0u=Th4OQ<%#-Af#=T z)_KS(X0(z#?cb-ik&m}&)7q>zg|t~aGg=phg|y}0iuju$ZH{*cH@8(ZBYi01hTB@L zX1v|EwdO^HwAp7Oeki2Pb|j?D=)R-XItuSjyl3MrX17|8g|vxL$TKsoP0UPd%YPJf zGJ*Rrq|G4CYjvzoYs-5QX)%D^^IGl4(%Nhv0?r0(xT`gP9C(db(>mk<7T#5C(djW<7c+n z5|J+-{GUaca|rFP-ZF0RnC_=%kbbXfb~@MIBAU`KI87lJ#6)wCJwLV0R-W1>mIU-z zAHcgk?Sj2o-)7yU2d@D=wl00!%IzO46Io1@_$J~9dHfy3&+zym#KVutd5-AYj>8V& ze-0Yk<6PwbH{Xmg1NF1v&O@6q@4IHWsVQ|hYl2<0B!{z>hS2n;2Fxuj1;(V8{|O%h zyZsjB7FggHdq2|u4X|koODur?mtUXm$*;|5DMWk`!o3IqgEA)+;Vr`NY6EYdu=@VU zocjC2b5_SNp?(|_f-fvPTz|hlr*0Ax08;Ok-oykG!87R%cs3IZ1kaK|ajR;2#{`Bzm@Tragzd@`eD)qThheEll4m+E|!cnHs_e~1YV;OibISf8_U6Y&wA zQ}-kjQh{H&m0*2N?Q_Iac+SdZCX5BXb{E0=oE0xKK?D5ia?q~@?e%D1Bii#k+OdZT zTFTqOgcbWKAM(G6cD%!cG*`}qlm~Ms)PKl?KcKwzxr~-=$h-Oo!JO~sGg?+3LtBBb z|CICnrObE!u8fxY)13EnocD{I_wPCHK^mF&FpbRn0FBIh1i|1XN+a>U<`C-Wl66sk z7W`{9!pfl

~u;=08RwtQbdmQD&k>a8A-lI&sF+n{=-cR^Ox%R^6%*tZ!ztIELIc z!5%VyLVc#DUmr6m&fE14^f4Rxsa^9lvOez7$og2Ik@b;JF!WJEZGb)&X=Hs=@islc z+jJjq(<ELK6IMN^ zflY?z*t#-W4v;>!60FZ5yAnTzKAJVq5A16fZ_mrbKlFDRZQ8@zw4b->9p0uld7F;# zHhsw3bd0y@Q{JX8i9gubX^o&m9ku5OMjyJULEnkYSrZh)rEGQ)d=>E zv98+h<@P(ZTDiXkX$A4ijF#E~T46<)R?;KcY3)$Nb8G|Vu0bilKiCFKUSfxKL2=L_KzRHKf0?~qHR@bPd@lw{7`Do zLcH@HP3~AZc5d z*?4Q1ed4T^K*Vj6X0-$W&oHkt0#AO`j<+>_R*M<$0QlvTc-Lu}uwR194nIwB5c;VE zi-)~=3xv-mUmXBnT|a;codJ<0`w%uG)FK4Jf7cDA^o+wIY=Vt9hB@Kq?0Az8Hxmp$ zXT;ltbSHcr`SO4yT^L)Y3v()i$ne_R3oXxo(9v$!#}CvWRd)KBw| z(@$RFtQNI?NH?Sx(h2EZe8%esNholuXql8o!&XGzB7FG$Aq z@bx5PX*|L>K(bS0To1qRP|3K?=Gz}$Cg-Xj<{#_V>md7p;`;m_<{$HSrS_D)liE|g zC$*;?{bC>b#p~!7l`p6EXkqt(uzMYBec2)C>_}?Q_~7xAX9UMizBM>$@=SJdaiqqc|LcPzDsK`+OHBTEWUrUqqR zM|>Q@11NJo>cy}lQMCqCv0sW!XFS;A=nVo5XK^`L!Ui4-U!<>hLIhS&7>kcjX?Gk zKG7KVivErX+N5CgwcxPpl7i4LgTh?sgZo(agy)mO!^pOBkRF6SNp@vMKI)fsFsJZ!|En#g_Vg~X}p!@KDTOxxci2NN%NnZwDKHcrdFOqWMUivz4~!lf%YwkC?Dnz3$*YJ;bG2uw8A>%d+ed0;#IZ5#VhuQ6x%*c z?OAdf{`t#G`}~Wro4>#|4s)A(3HG!H_N3U7;*+b+ruLBDNxvd=L$W^s`F()=N0C1n zxW9r{BXH!m>pKP%Cy)-I3)1&6DuXnozwbw!>L6W`?iBy8_yN)>`F`^EC!nJf`nx7{ z;7vLry^x-0j!Au;=9tIX{0U0`uKN^nVzvI)fOk%QWNs!GQb| zfiogL3;6smXSFcIyT$}(jtA^GJF8_M;(6z0wY&?S-$5H0&MeH)A(=GiXc!Zc>DsCj z-bUNjHKT2SW1>SdpF!T$&*=o~n9$5$Kd%%1h;ona(g`NO=BPoL&B*`A9-UwuGbnS_ z%R1pfWM=NA~<`5&I7d{yW`6>$<1GUwsV5 z`iIjdXzON9s7u6Hjx`|7y=ktoY9g(R!gFZ-7s%IrwC1bF{34LA`)JKqkNE`68BDU;CN)+Aou@{j%kG0{I?V_t89I)g;VYZo>L#CZ7+?#(ZENU+*ozJm4Na z53pe#kdJx5t#ezf=D95e56!P$_~`uVxuBK5etz{F@a%Yee)Vkdk@w{M>Is1Do90(f zLOR*Pjle&=hzSFKrO#Pg!i3KNKUB_yH}S5mLY;U&dLI)We1Hi}h(A(``c`2*iTJ}0 zF=5T4Ojx@f>!Qb*@X(XsdlT|Dq77S7wi#`B4*8!)J9eS%H_EzxB~aFNJlFLTc+*@r zQkv^VN_+ek;KZ#~!O2*HIAB(r-e8DI#<`9-Ag#xy)w}F_0$q7~2DvJ2fzrMzGufh! zW8Q2!6VWQ(l-5JADRyM5_#}^SNBm7WE{1Db#YG%nHnP>aDXm9*C#}aGmeylLIoqvr znjLX7f_0Oo&02yyjhZ%bsGP^XUejiKT+>GNSf9k%QTWxg|0CC%g?J;6A4L3+92a8) zTg7sYUk7{yj~m9ciZgj!M0_iccOrg7j*FG7O|0U0H}Io)JT|&joXz89h&S{28N`pt zaWN~TRlJYmw@0^HpF>+dM0=xndo$&` zu2szDG}j^iJdcM5w2EDFTr3;ZD%NuRLF5_7<09lRkH_7J@8WSoP^zrA+6#=9DgRb)ix=u$9fOi^Kx1b>63I!db2%= zc5Xr&w<3IrJdes{C|zs@E*`i>xy`0^q1YqiG!G))!Q)v2Tg9_-+_`;3tGJ%y!+}ra zaW~|U&*Pnl@8@wbtW`WG$Hn_$TaR@WAZ^&K{=A89Rt2u=bcJ2kMvd4+!kRKK#U5z}M(QTtb`y8u<^{V4%@YwwfWkBwdwb3bHqg--_omRi#5wI_g`ksv@UA6MGSCGZx3jn-iOmW zr;7uD!#PB|UWE?_ez(mXL2I#C}(9u3m5AHJ6Ll{a3f*=iI{;oPJX zYib%N-Z_smm3Mj*8kaqFC>i5K?ckTrQd(pCv9q;*|N3kUW1bM?DVEBNYNoMfA7I0( z-h?ir>yQy^*{;dYqRgmf^NnwwH7`0XmAP=PcjNdARL`hp>_@KKfilj!QBU!24_UD% z*|qnNQW@=lLub1hzL&~;g))bs8xz;l9K`FP3kI0Zz8aWe+Hp2M(aqCSknZGh6XGj$ zv5D;*z6CJkz7G6vr?U;SA>PGKrQ@93l=S7@R(%Hrlv}LDV?6+(;woh?2qMtQRV;Sh7XLwj*P^$6l_ zlqWv+A>eFroiQohV|Rjw-^t%TXy!8PGmUK4-LGkKkJY%!q%t^*LOmMD#|Rz_`1V1J z$9y=1?X-4GfZW*ju?V91de;`5Tjj+EcvhU#l-A(9j(w{uwPkcnPf6{V9s}F3q2E#d z8TYfM8K`5%{aOSaLI6Tw6UjIlX-30TS2WV2ksgioXrxCY-GFras@mzchia!=-$6Kp za0KBP!iNZ5wbR8<5l$m~iEtL-9K!dt(@k?brrRE`ovz91m@YnAJKaEi z+IfV0EbO(N`&bh{cawbV)pYK5K=P~Pt2=Q{xjL9_*aEw4ha9O*a{o%`>tA1g*Sqm| zNPivgJqQh4H*#OVxi{|%(WsmH!f)|zfNfFwir(HGWRKKWlDW*OpJ?=^rU2+a1!uEP zrdw-cEU~u0-kn@v?N-ip86NFGoVC}Pn*8O{z%GZJ>m5mUKsI26{b^uFTfXZ*k8QxW zLDYXJT{(|c@;!<65YNh4t_C*NgR)e{*xgU>f-ibV_ojE&n;!b^|Dr6N3pawM!i!SQ zd;mTSZ8UC&?Sgk3`V8@A#kr?bu~nQOlC4r%;#J|xio6Qne?c9zKYPV`Apib)qPwMf zK*Lu(qM5zdUr%(mQV;YmopW2l+2Z5SyBj>sFfd`pH8?ZM#d-y@C*NhkSQmov&w}yH zjWNzLkS%Vw*qhsT-Y%WXYMRN1P`gCfsxcL^0FU%7PdogkmYD?0HnvzqJqICIgN7y4 zfL}e%y{Qh^2F_qv6N4QY^m4uU|G4puT5^+eE`OOk_<8Vf6MIjR{g03d?QyGRvQ*Ob z@McXgzFFgSmi!Ps{2TgS;B8;1`wq#z+Ag&(-cIwnbPwss2s^Vw#%2S?51i|h?ip-u ziSxg@8RC`N5o>Fs;F}js?RJvwg{geyCHAM10rFD2zmv}LU)eTA;^XBe(&uIbvUlot z${D;7ypY`OWb-6<@L~oJcGPdzMt7U>Ci}NW__j%J`71IX;jdo)?FH3KeT?e8v`!}X zIkeY`wmW}Bf9wACQvdRQ(cZs(OTG&Brti16L*PU1_m}IR`j%S%E3csc$Dn`AJUlC) zQ_L|vqF?>@`ya}?3p|nj$uITu^~$zq`uOr;4?lxmHb}Bj`Ekf-EqG9W_d@cZxzjG# zB}4j7yjMUb9T+c-&U?C37`}VN{6dF$6V0_YV0};yx-^cFEz@@?w3ab%k$tsxH1j-( zbh7ux@6)9|qxjdZAPx7gnkJ{7&pBX&SMd3ndb@2bI3-uV@)mM0O$0v zB6D@cu$CB;2K@qk75&7JrsZJO%V zP(7&oIO?GKyz}giHhGqWG0#*VH2dn{K0!pC46>s+hp1(18!>O%2V64KylK^`)^^go z<27mCK|Hug_VK0`hISJVTC|z?(V)%b0~p#&W1t9|BwpLS`sN7e=&zVZF!;aYc#|BT z`lfdy>6mQs8hrOg_DFF`(@XX-yqWY#?fVR68XzCa4?g?nD7;;i$GfeksSNb2)ImO! z#^CQz?<2Hd>8dxYK!=eRb`9kk=}rEwcybWnf(x3baPvz{3NH{|E4YOfJ}2?H!CDt#8`0! zTco;wh%Hva7888X?5pG7utnm5Y_Z|ru*Km%Z1KWb_(kZG>~96WX;a&x1A3xZxEiyd4D?{H?zVjzrB>SUz0+pff z1G`V^n!1&51WPs2H-aq#Of4@!&oiQNFJU6f-G%Q7&+D|!d%g;2+Jo|~SZgjQP4`sc z`#@WeNw5zv3Hf0r;r^i}VR?i}u#Pebjwq8r-#BeSe#7-Fv91#S;Fpm(Mg%5Cgm##; zp*NeZ#?@*qHn2vAF4v6?U4ecfu4HRz?YLqb%boF*X7O!!i!sc#OQ&kC4C19*Lb zygnW33qXA{jX|LysPAd1zM#;}qAt{ec3fi&2(`0-P%G-x)U$5b4B8d2_>pMZXV5ku z(CS>a#sJsPQcrZy;@fa1L8_-V0Bs8TdFpv8crpIpda0h*wLz{$jlr%Um7W42H_{X7 zA^>vR$MvLx+^&UANKbW=+yX;up(l%meWru{u%{>K>UbTO+uq>CZ;xQPj;-m!jE{mB zi;d|*UURyz@VRt>dD_Fwol%dcC060eWwO-aDM$5l-(A=pEzqKD+|GyELA+!$I#;(5ule7P~<2 zH0XWF>7C>B&Vt_eoZiJN(7PM_g|Xa%aT!9;L6@CjQi8c(|kd#rYiClzgJT&TewAo|XI8G@}mLs<9#=ujVD8z-1llc^RdrG-z7cxHTIYAJtmE{^sV!$0rI!b z$pf*Ui#r6gpW5(>R(Rnq=847~o#R7Io)e%OgS`xqY1dluvx6UFJUxMLi0RwqhOt_g z6Z1ZI4(1f7Q(E5!v)sd|rxklCyYM~qp084y!mzGY;)KVzbT9yZQHxO5IZ4`&!Q6DH z*)$pVTWBBEgLzCCuby5#7x>#IG%hs(hx!lqCS=5}KC~J>Ib(L~St_?}^Q&j6tlsv| zvs9L7oMOyV2|m2bKY~3~TY;_nIAoEEeVtZ}N9y(+0^ET8Oq$=&csL9^NcS#sn5W@x z`tF+3vKF#TO*95I zKyDiFzH21rO_)2)&<+11?XMbe$7|bltj9fw<HV-66BWzllL|JyZvN4pX8} zG&5jNL{k$Y)3oA@wG#8^^JH(Jw`>w?p*_`_4kpy>XF@a^`B@F-)&&z;n+YGK;(4qCJeVUYdyow&8m|A5&5wZx+BYN_Heg?xzAdD01s^=qyKzToTGMXm zn{0*3Nc)yNKgk5AHhuYjMfpKT3^IB4q72D5;k(`)pCev*zIO-h(YLQMHHoVNn%ZHT z;w0vw`#QfyImT_{s4D(G;UvLw5j0ojR?U19$pX=GQw1TV_E;4R#Nr*-;W!1XL| z>AR1h*i}R)b2Y+_h*rXAHZ&Q1IR*B#8EvMuXG;LvpaUNtpIVu1{$^#iy?wF^>&}UE zC*c9?)xL;33`=o_+;O+_5bO`<6eF9R;3H;<42#FYUH1dFJ|7L)vNI`$Nv@i+I#12% z;1+SHf$q|HmUS6iR=ufZ$5@Ta{A!w~=3H=#?V=9%H}oym7$bZX?ESWmL!x-{Y)zQH zrDi~Yr)Cu1L-CFX@Q9-VplA4?u{hU#3-b( z;e9gu4bto#Mpq5?*R@8@bIdia8tj+XV6VIed*wCQE3d&`c@6f;Ywiz12u9F1)zk)f z(omk-P4{d>^LSS>U|VA>cnIQW9TCkI(6SIMoZtKgcSI%ooYrrj?Tqbw5$BDPkHpzy z&+^x$wP_e=S#c&ny4;8Lt2i#zGZH#kGBFi*ZrEEpA4)mAS3C0F8?ir0X>8fjZl|&~ z#hwn8*RZa z{=UU!M#WW?nROx(xLCHMBQeoFUWUQNv6u4^+|32eJXGN{w-uXfq2AP zihQr)_h{|xgl~}7RoKG;eGTgAThAgd?O!;tezeB37KXh=4SXiu-?D1qD{1cn`x?|< zd7ttX$-aE;AAEwglkVq$5B2^y>6BqF;8#d*M13R=@BODYaUX3n(kPwyr*_-1r(#}) zJ*^$-9*4+Wd9;s&{S*iI$^&0^`2A~87tyEu4ZQyAfjbTy@j&&`cu0E?G#2Kvd%6?Y z@U;uBz{?+gA}<;hFAs2D^uWpXu$%U^4-zlt4ERiWZF(>nd@18E?V+3l-#c}<*9^Yn zKtq-l-5DFv{6B#A)&wyJ(_LhDO<sgV5cz;|tG`%-M{G02tbFkwG| z`Z4WCkRC{8vRs&FlBCZO%_h(we2@f3G_S+E0lZUw$Y%%HnRG_T+pq%j2(h?6k?7~WiiHdLU^6t94uU%(tN7kz;IHpL15 z<(Jg%5zV>COEjtgGdw-ZU+lY&N%Rx+;1Rr%EmL`#ce?EwSK=>CO_Hr+{UXgZEdfn5 zr`+{XVABrF7qYOo6g`@i!{^Xh+`(Oz;HF(4;!JElevbijBF0S5@4#Bd4O=By&^>IC zx07kS_h;XbeAHg#{SLC6173TU@9E={?oJLDO-;lj-I*jl>E0yM4S$R7kKV+FOJmAE zv0vdN8}6<-jLqTowB9KUq;>yQ&6_}Ths}h2BG!BryK3!W?eMpHwWIzhVmuRd(D|a9 zxZiPlAYHcS`s)(Nd{ZM!rF(_xK8>x78r-YW)C>MioM5(N?oyv`9E(BhjXli5Zz@g3|r_O5d zeJYx!uDgM2hpuUV9yYyp#4@aXN8$G%aL3s_m?boX zB@0yV)8HeL>X>9|q55e~a2xg~r0)u_UcmQL()vu|$#AnP4SB7|OFTTu>xG?NQqPi+ zay^|x`|DYYdM+X~Kt>X6tV0;i+l^<^J-x8^3O)!EUAp~InLTJ+|*qH9Z%)?|KMnn6Pre*eS%$=G|%f@yS zeN8yV8}RH7UXl7}5L%V4^|gjO->C*%0MB3B1Vl$zbEdpwB4-u{MJ~Ss+WWLZ42Qi$lIG2#T%u zEdR4)c;n8sC!sn-S41GJFg>x zMXV(|-4|nOF)|iU@=26vj%ePBxWD#@pI_P@3orBAE0mG>=$8TUp|o`b8v#DBpTqfh z3v?bwx(n}bKZg!sgX9Yu)Z-bqMQYn&F|Cv(001&vv4G`2|crFjmY z<7_8dG+#9uZgG(vQQi5VLAEnYx|ct!`9rD~v=!UA2kDCK(05}-@fO(Ju;%%|i+wti zZ3pya+rf7gitRLlo@6(exArW5W!q5jN9E_DykFhnyAHMPXg%UjW)FbQQKYBh9nHU2 zfxNa;yJ^2e`7Va$50Hsx2gXzCXClTb5$&$9(f--6&z$I&%KTmf9+f!l<+#uW+Rvdi zq9RX{CHWlk(KNQ6ie^1eAzuOfswR9=6vm^848KM5B={n~>VD>fLxGy4D7PT{TUwW6 z9GXb}wriyZyg9R<#y6P(@V#|-e=;nn>62l>O$$I%+;0+kacz0rJ-*3bqnYg%Wj^SgO| z_r>0+Bv-1JY)Z0$_I~?0f_!8@hXZ8alL=UDKkoou0b4V0e-2+K`I#4BZ{%m7hdx^w ziM$UXD1L_ac1}f`dX&8#vWb_G?$2&IamJyx8#kI|%ZR zn2U{p&eZd;Z!jl0i#zy#L>Zc+P+r2V1x$VDYw4|-NVXDyZ_DT#Gy1-)p2^>#Iq@Bu z2K$wrxNG;&M7wPKw0%VZ#deq zp>YQGmh=*@AAHukai!K&ZN;1tbDnA&Y;Psb&PY}`6K}HPUPVnNet$&uol|(wnnS(z z*tfK74dxEm6T&#zcb*r@Jl&uv0#EwJ?{>r)FkkEpMZW7<%Z&?%PYtN&LX~)_jKV5UH+CUgl#}nNvQ>h#FTp$`JorW@_tAVegr6$HV+fLzKY~ zgSRmX?IFCh*G~DAeHilTb{>G?N@O#!; zQ?dIr)+qej{}bd3k>6dVyDb0TmHb-#78=iwxh!ZT7nA)fIPAFtXQq2ht__d&)BZE? zH1A#k|DyC~^8X}fr5)G67U=hV$fte`yQ#rEaS-Hg!aSmLvIO7zEc`O&;1k2~O>-c8 z-3eX5#4?62i`L~ug^v=WlCw;0f%xV*WxlB`mFZn3F}>B?nciyG8>43a&eWE?#nfij z8=_JTb6ZXBxvlng1HO$O7d7(~@Q5@0HBsqjOl@}fXd~dd&a755;wO=Z0bV8E(P|&> zKw3=H9SO;8`;q@7l>?l1OLCieygh2A`_9%n!@Sm~^>IjxiQ13!JDkaF#__hOI{nzF zlr_n1PXo7JT+q57>9w5;T5*ps%8vYXl=sh+4t?w6qmuuU+;(!j7)5;60q1mET8+p@ zyt)jQ*8SrbA}uDWk(pYEPvV&&{X{ftd3t<)R61nS7=U>V(un^$@P0;LCh=<@pBF{^ z5?@q*c#IfzExxHV<3539y!=~$Bzw#~TV~A1d;E zoiO8Dgk=H3j0*_PKw(Beknr+pT8}!1D!Oh2Evn~D=+QEqEv7N^2KZ?u9rHv9hqE2x z;||KNcMHWV_fM1Lu8a8vQL}L`{MjhCT9fDuKcM&bB?-F_u{^=ESdid^saq>U$+iaj6 zqh$%bxPwP@dq2cm)VX?b@4WX@ghg7R7k5;9%XLEUIfVNHgx-q?wShu!P>?W{+lb;9 zXq-QdasJeGsko!_n&cyhE^}&K6OotNf_WRQlg}_w>$;ZXNd8+Cyh-bdR`8JZ1_h6O z%BvJS^4H6FM!wcO+O+<}6pT}NcMhhrM_qGXfi1~iU`-0ZZ|CR=Vv;%sn_B$)U+u(- zq3k_nPn`CCY0oJH^=)2MvAg}|joD`KPUE1drLA0KY&L$&Zlet_jj10YPU#dUf9qZ} zWw$i9P<;a%b6T+t5+3BCzy z;Cq}{M>!9@Lh!8&-|C@{AwU)rw-Rn1YcUM#o^}Fj!cGJf{ZnLXB^siryT-Wb-sw~_7M8^x--2C%7O zL~@HExO`V=!-)@^l8fqJX1$(!N_w>pubwX%C&xWu4dqp*8bkBjmZPiO*$;Rvz;H6`{|@ zaWO+>%MoGs9G7xHg}jA;&JrnS9+>L1zsHtdNF4TPsGOL8nCOR6LXCrw)eX`5YWYd6k=M}lFqZ3wjM zUz>K#Hf>x=X}YD`6as{|DQQUy`@`+B+puKo-}lVSk)`Xagiz@3AMVHUx%bSOGiT1s zoH_HjcXE0w@tox6etT~jo=M&Sa6EQ>ko3_)^32@(5QV4op~kKW%s0?@gv*fT8~*dy z==L_yW-G&c5q3IdNMnVsBF=+IyB|6nSMKj-&M!PJ%VG9j%5Szf*k_S3$MRK*qw_Xb z%*Fh|+`X^q`GxO-&o>ah9+1WhpFq920P`|rPpA)_iGKUl6SmJRsI^p1Kz~2MWv~3% zIo*vjXIUP7Jb&4S1ugbU3;g3(2W_9Jg1z_++VCBKM+4oBZ+scN@&U!BvmUSSZhQhT z_rA#&E(nN6Nq_YI#H-l9L3M}fM}Z@+cOm8zs7+FPaYH78PeJG8_xItM`u#fgI zglx1w+R`u9`|(UN=0Qe#pGqSSvQC7YxsdT^m=76+FAp6zkHVL)0-S)b14w@X=za)D zJc;)N@Fkwa?}_=~3!c`y#&}XZJ9y&#w9*6M>41NFK=aH&*izuhr^R@lY~EAf&}q=O z1WA6Li@6=`f%L4`3wuc41>Jda{ePdfL z=;;7{E#H1C3-JYc`4&fWD zM8h@+{T6J38Y3^HwltL2`yAr4ytA;Dw`Bw7tMGkbYDYrb!hYnf0Cef;LtgNjx%U^q zXKOz$gAJrPAfkUA`(7~Dy4TY0T7N$BmxJ=EgU&4|3(A)jJ}=d;^LkwWOa5_#!+&l7 zWmYf`en03d9FBAAP#^FeU-;1UejdJSiSn(QCuDqT7pOM_w|{zUu7~R^*%B-03S1L= zTd^)kdDsS-T2f`EY{}DF2g9fvJP$VfhSCAGgR6A@?>tHVTMYSs8~G=C%AY{~Y-#x` z)cGU7jPvI}{vJR+9P{62zCc&dGAWg>miL*jy=nP6Tj%TQ)65sqr;lsN|FyJKRYLwU z)VYh4{ZL~^>YvGXnJuinbGmJ^fBP0>ARqZ*z*hJo)E24!`_LA?4jlUIizxh8l5Y0i zUjnvZ&Pv#^mn4_w)l?evJ|mAyGUOSu%63Zi|26E*>P7t5un(!Vwfj->_k3u3Ny!`Y zLekd#WY4}gRQpF=>Vcg*Lp+IfVd1JgPrz5UyDA-ae8(aW-zY?#B|8D1zK6>7ygie8 zb6}G;fgXKmqi_%Xony7={Sx9{fVi6ww-9m9M_h_$C4QS|4s*ev5W6m4AZ%}9jj5UW zrssovmzl^5)m!DWTK3)yxo?B~)HnU+%iT{^%Y0z3Lz)kI8}f1taV?mCJo^1R$Ywqz zAYCHG?)g*}j2Bk_Y4Yi$vFgrUMw)(`3>m*3Nc~rF^mUEUg9|>_x^`$(1*W!axeE!RQBv+lRfpp4|@%Bul@$t@z;K$ zY-=a{zSo;<$L~WvT5one+7G)~wb440%B}$8OU%D-_o0s5hIersLkk8e-{(l$ZumE| z_LBXyNFB`HTLTE08pm|;xEz}h4f!wsi#b8FaMDA%-v1MIaHAy&AuJ+ZQy$z__}6seB0Wu zoI&fYwu@?J;rpVnYh+sr=D8ld0`c#3h-O;9C0~32%Kck2L^HKf>`&{leXhIld)3(g zx6yVs+0k;;oqqvDW$iiedGU~qzC!>VAHaCy0LB^zK5xShd%ixOgI_Lw6CR?r-1nGT zN3@<0Gwid(8P%v)m=9lzFC3q7KloH{%$?co?!Nwh@TmsRp@l>D+z)=cH?$tVA3U+A zcW?iOg}AprbU%3C58n4{-hcjdcHX1Y#&{Fod0$xeF7dttd>4EH>*B(;w;O!P7EoXO z9h4i%dzAbx)DhUCr^qj$a!4So8|!)$jhw@Z_{0R5$ z*FTH2`d{#24O{%-0h|*+HmY?t#sO%vjyzZ80{Cn8{OS!Z%;PM8UnT%6U&ObPZK9IC zeL-ssRDWrGn)jq(U;af4Q5QakKBeU>*fGsdU|p=`Eb;CVEgSKt^f(O*+Nrc>F4u8O zDZZ6>Iljj&)={|i^*JD`B1KdJ|$ z!!C^1T4ep7x3`@1S?#XupCU@}4oDBxtGTcX?rdjI_3)LuX8G~VB8yzXI) z@2DK0Umy6?*hN_`jWJ>SH&ML?&F`XU1K>&RfOLPGRg{IHv)S`8mV_=)PQ9c9%J+K- z5BUN;#EZ&{Y&ezKd9cNpudVz7+SM@L3!t%51$fdoD1HeYkqqC%Tno!V>BbO_`BpiO zyBD&&W;_33b7)F$8E|rZDfipY-uo@76W#|gYwslpCw-7TWt}V{o>Cvw zFDQMyfp}))`^0?Z)3(e|{v7c^8);k%`d9Fyx^e;XO}d=}zOC4I(v3L= z_t=}M?%s`XIsVl0z73z5{E&tCd11p}`~9f2*)1s30wL}!8}d@$cu_09sfv9d7g4*k zz#d!y8}nnlzpxN(-PejaXtaX@^sfbC?&RC2in9O4{vp_^wb-xN+%m`Y?zPlzs7>J= zY|>L1@+{DI2*=dk<&cHy(o9)rRC_DLy#xBQV6LJ7`+3N}DsWKS5G6L~whrOl@V6

_LTUYmHd3jI5k z!Toc@qfev!@6>tdw_|=AX;T>wLk^<(1;QD}GJOH`ls2V1h;%7ES++X{>-`0oiuSI?rNI&aG5B(N(g!cGI zKL~4+1@M;zes>(q>wQqT*559~`Ug>WZ->wP;1s|qfat?+cZuT<4gn4UE&|;mv3?=S zSA<2cwc*@~f=#qv_2xlq%ejLKU4Z@P4m!GT-ftbAcdm8#+%Cru?%%?_SkUDV-u>1Q z+&dO@Ep&MYt*!-w3%dck0a4Ga@HNRdmwo7-D=YtTa8hsg!Mw_EV9o0GBYC~}U{mh{ zk6Z~`Yu|g@lX;aEd{6nC16Njl`A}ZvKkd7+@{5P_Dg*jE?}1rW_};taAwTvq*5u>6 zH7zR!TYr-?I2Sr>%!zqr~9E>P&TtLZc1#!zCi2)QES;V_x>|{QLfK!JKS<_C|h<6Qv#+GF2rgzZ{?8`d+sQQUI?Arq4PrM+zy==Lg#kqys)Ke(B5JhEbIsD2OI_*26R*p+8x%x!WO_5z<$7f zz+u2)Kt~P2Z3qW!0qh6t2OI_*26VU)Zbvv^3*e$3SK1d@Dg!9v>K&rj-G5GnzZ?6R z^KTw>51ca?XaF1m?0KcDTP(!4^UmEL7*O{D>xr%yo(~c2xr2dxFK7W9Ue&Y<_6K6G zR=8seDgr2XH_APLa(AQLy&~sLAAF*P7cZ|o|H;X{)+tjlZ#A{|e&FrGI0oFS-A$yjfE#^A1n#eO2VWIl&G3ishC4pvj## zweskf??3Abi~W|sNd2HYHfu0YyrRoJFsrMu`4^S$L$fLhZv-3xTn#u3*a9eKR}==8 zS5hABZ=HGWQIuU_``EL)cy?D|IPJLs&#|=U7@m`9&pmkVN_!r_^TxF2LwN2^dmh2_ zrnKk$IbDUD)1HgxbhTifv88zB30K3M6D-;9r z{Z!)H%ibmX9s!Qp)&L;wi=lZ4*^XdiX|ACdywM+G|L1Foort@!n|&h7o) zZ|3yUxhj3u*}piBy@}Kp)4FEgUuw3{dQCa__F@0=cVYK1fBV!wZb%&e4SWUi6_M6O z3A}&NBCdFr-mNI0cPp^p;+^NznJLGyzwCcMtH!j-R;V*KdCiFA;>#ked@JESchmLg z6$<|T3Ke+#fW~V$4zKA(u2@;t8jU8BmBB85FqRBOBOb*;z;BD)S_=$(t?+b?&qm|Bzt5?=FH+nqYde5?D9=Es2y}YzO8jYPK zyn@1)f|*CGTpadyI7^y3TND0xXiP$dUs7MQVnuCwV4brv7Vsv0o%qGq1d|Cb;wQ^% zIuprghX?#reogXK8H~rHac?Nn7RA3?g8pPQUbnKz>omz480jlJKdq-wM=T6B#a`#> zC7_d7*_13#w#S3MfYa%1O8SyPujc5j@YY4vMb`%7bxp~#WM?cKth;iB&gs&1>pTrj zi@hFsK~^p2VsC4xyfqYoit5%ObEV#nw$S2^w&J>VDIJlpW#LdF>6CJpt@DLDgLMr} zrDNPw=26F}9u3hGmAE4rbp$(N$@LCdr!KpU>jT0~3WcHxsdUHr0Y@O{j|M`KHIDNW z1qB6@WFY=ID@%%;NVj)oAj!-0#^hNwZMaZcQW_ zCoU{cyDt(52Nzw)3{h=Ees3a4M3-IWUE%dd!7bkDPdXx<$O5rMhC=}@08)a=gNbCQ z!-opt4fv8i3OZdmVx6r}X?seLaBvMJos37rDW1eP?gzS~15(qI)LSf!$~#t}75;S7 zPpAge%BpDcG&@$VT2Z&Ms@~DCswq{UYmj(nB;@x|J9dPF>w;lNDB(y1lT^C(tFFC7 z;);|MLR?$bxUz2LvdbJRd~1V_L}xtcNVW$Z@n|cRO~TRMiNffNrRW>t!9>F0i6nz@ zVn`|oMLL5;MVf+j5{3n!G&%z@Zz9OrP|xw8e;ucPy6xj6_DHoELoL6-LssD?2U#Qs z5~V!(!qgtuJK~*@2&$2k{02#Y1{zP^f`QA3TsUIEcqpnL@VuxaaUtOm*WpV# z{IO0&hZ{#{BoPbxLv5j8K*(e-b2LHkzP2{1z7Z`HFDq_8R0*4 zzmqe>{53rAe>YxHcJseZaP4~gn@=oX^4kNg`Kv4U9sI)qm0r%!yi458?-fjREiS5% z@@YNPNBwIZ9l?%hJW~yv|r{p zzm4sW#=|_drK3~%h$djp(DKrg`~)W8+tBS_VHe^ysg<#Rk3KcJQiu&p8S|C@_+I~? zpBVTw*V`s24t0nskgh+9lEbUf7dZUkP%r|b*afj@+?=i^8i_y!DIThKb<{OnmdUTp zA4!HCOTEiH%}~u)jpg(>9QG$8{AzcDqb}W)C#2wQ+;SWaM+0P2jRi^8OUtx0>|2vr zkTqdw5GDuwWLa^#gbxzdu{5m}Q9FVGBMWwu>@r$?{Fn7KyL2Ys^F7ela>CDr4k$jf3 zZJ&A)b#pcDU0)s@-HkfB2N2&d5RR{mj=BL`05NYY`T<>M2yqOXBaR}D<7=a%J5aY5 z0X_m)4CqGuZonxMglO*>9eoLKWDj7T5ET!Njy{ftl#huCoJTK;0Wr5NDga9XR|D1p zwgBD;*aa8^8~_{wJP3FgP+()tJfzbO_)ZQcFahzNw&?oy=;$Ee2;hCdxBN5Y#Pcyg z7d;P*jvfR&0yvAF|Ig@X81Oit0|W1_?~IPt1Ll7Z`2p@4$pfG34(DbORnA z8XfHejQwPE^aS7{Y%r;qh6I89Ps#SNM}0I#Q?7sa30`|fX4wh0UpAH{Ud-aM-dNj4`5zC@PP9H z7yTCb1&jf%1{?;&I-BVK9qJvRcmw$Z#Jk6%`(}XtkE5eU05<~))W_I6C{IATh@39V zhDKrUI@faclm(&?&-3s*aVO-Yz^ZAEo%UU}nyKehZLuzU<>jYAgp1<3@H>I}>HwsR zaLxGP?Xyu-C3z999X})8M&O7JRYG1w*MlF?%~B{S%Wk>{o$MjCEY(xbsW!^;B*?lU zb2`6a;F1R1JHQ*P4;w|7#mImpBF3X07da11m_kL!PMVw7KTLioVzhUqQ0NSrW zL1*Z#DWx}}>q2@*{{i(3_n<4@nY$~eiu7ymvsG`k3Xx znol2o`IV(p&T(wPvn(H^a2R;f;}OK|yIsjb(Mi4`{06}<326UEROAf#`YhM#_!lGm zE&P&LDA-K2pOv&yMp*|S!_G;&CdzWo>zh!$HFsOi^6mEPcG!YPu9@`8%g;UYtW?U9u`Y}Is!f7O$#jtD{g_kr(?u*FEzjX z%8}<)RjitF&T!5a#NN7k+qK)Tq4@VS-P?HIRm9mhNkx3AYI4J~&%N;COI5_Wida|O zca;zgkPGwqViWSS8I9dOTgugM**R<1OcHBr{JT!?!NaaU8^D`Z^RHf4!h8&mC~7~$k+%=&?{efGJ=bgIcr-bjgXopK;f zKjLggoNnCP{|E;{+c(9(&*A|7anKwE&0R$EJZP$wT~+-_pM~T+2AYQb=wqcWB_CBb zk_ZLk**aw#^iKMi1${Nc&)H4<$azU?Ut997f{4`E(I~g(_DyIw{7e-=&{Eskh?^nA z-Gh6?>#^)SW7iBRY<}N#_tt6KPy?pi1Qug$UbsNI5Ali*ppS%2LcErpQ+7>8wCXLB z`X+AA-H}tXHE-L5yY2Vbs_(VN5IwU-D!HG8+~HmD3n-6sGxJ!reNr{$u!bt3^(z?y zQ2KdDzkeg_H}qt;%eX`5cy=DK?Rw58<#^WC_l#}%*2A`KPusRXW!v#<+uet3_x#G1 zQ~U4aj*^bM@`N~i+vw=!xX&!7<*9l=LDYVBgLdA}#@Y|z27#LbTpma6{SO|`OStJO=iic}E)3a}i8?;N< z0zt@V>l>oSFAOp}{9^}|&n+^Js)MpnWCh#~(=*X}Eh6UD${Ip^NFRqG+hBL5Zs9D| zah^dU9Qd68oeREp10mS2B4?rjl*lQ`?m*KEgXR&Ush&2_ZE^4XQvR+l=2vgol;1q* zkg|1%Vhi}2*nZ{%`>y;+J}zXs zuC%#jhebVN^>%x8POUYk&SDLdZm%Sxp3*PLTuN>K$&_!LzZ^1?-s^#14SW*!EQgdC zbpqamtP*5nlc3uJI+vo$-)Y@tSxM63a^u&B@Pi1?x8p(ElWLMrIs-M^?K^DN>mi!z zW+;=gnU6gp8p-n#`1D{*v>W%}!*#CDvKob(7JG=5z!C=!{?o55Ds`}8It`L%=JevH57DRvUK5jYx~p?1rQWbOfOA8^Bj zxhhTO<}sOxZV+@~jNgtBo%W5)FT=%w#TPKlnmcQ`48_7L)MAe&` zt?()C&b=q6?%8LWUU(j=(;?GhBV}Ad$0XI+h?D7 z_QjW$KL0`$5o|;lGo`>4FTQm1v(GH0Ad1FumQIp9;O{;U2d;}$#I!2zqiJ4ZZewZCN-hi&!|o9MBF5R5M=1>9t@KaK#4{g_?6 zWwH0z#cnG_9mArCL132+3fvxCE0y; zx*f8^M1bfl60(7$Lv}Dr1?qh^F`y+P3i?f|w{NzH-4?|-uhdO&* zR@@%6+6Qx#6i4la!0yXYNqZVl@&lH6_{Q#t&5l{F{dW6Mp4gOQKQTcZ&auBULEM*X z-!wrC=Q@t!{{%!koM(Szl6Yq#{ogbxfUXHSno+t3O1{fx$Bfy2JGMz?$!e)RCc@*h zTGVf`?~x@+8n)V>vRZf|IF{-2eMiEmvnK59YV(zxviQYaxB>+{+ zkFSq4@UaFy*1*Ra_*erUYv5xIe5`?wHSnJmTQ?T9e++%oV>H!1(kba(*i!of(jqaHbY zk&b`-Va6!fpz%XrQ_sgVWGdE6k4~@q>l~sWwrt3Y>5gl_^)$FgK~9h3b?JEReJUK&>hj74IbPm2MPKod=Bwo$(R>sZYb`9F z)=Qqw-$DJ{(yJ)@4SLIeg*h1DM5T|0?<;(}hKn?$={~xQ@T&@)!slW-n=YoCV|ZR{ zc>aOm`E9JH(RGdjV(ll@4W^&v6$>heDOY(NJ)+>5`gu^-@3Zx@L&LK)Jfg!f9V4$i z4gc#eqfxr0{R4Dqu|z|Gde&lzV`|AoVEIE{_i6bsr7thcB*|;Gh74%Ej;{X6y4|Zs zF`)U)($D%)$d^ih8<|G#>0X*=1SuiEiE{oJMf5QoMe z)cwkker}kd=#Bkx#oElKG{SaW$M4ed2Q~kge%>=d@msB*9okN}=x6S?2K4igj(H&Kkuw4DlWZHVTGc12U|nf171ge4gO`A`?GRaKDs^pOosq zGVzlI_v4xPDT3`=CVr|obW1u$-P^EvnEU_qP@9-8guzdza2r;ZdtOhcRQEP~R<-4K zX81G2@&7Wz&lK!uq=(wD6NCMWO#Ez9zuP9}2(~Ag^m9{wLM9%)yP^J4xJ{fXDopY0 z^^9?$s%{s5938``JC6k8>szhFPZcpe-r{(Qe!QNSgTRbG^VwtIL;j-`AF*US`$39+ zd_7V4@#RitCVRS*#YXK%u>8#DEp2ZYe?1hGozGOkcAn`uUq(Nh@jQMp(zBm##2d#s z%xC0lOsOE_+3)92#`Ad0h&PVUn4ZT`MtUAc81csTNW%&$N5%(q1m@4(- z&w@Xw$AKJ+<>da=i0^-dF$yw$?YA=V++hN8V?eM`YcGx)Yr}TjKsK>9&pYc4d&BUK6HXHQG<;(c@ zTfQ;FJS3Nkc$-sH@PSj5%b_1;(zD)peSky1uJy)vmkv5T=NoINsqwoFcrKSc z8V|PeQt3_?`LAh&0q@9yUu24(A$tCp$!7!PIaxazGWh>r1U>n0M*NpGe#Ah}`p+}y z?H)~UTn`!mp7d5>s)zC8HGH!T^yBfr)BJfoigT^x89yHUljJiV&*jeYFrOKiAU>Ji z`t|${({njCyvi5_89$r_f6Np=L#!UjCSBWwH{iz}z)^YHyWO~JT+?5LhvJmt;ZKaX2CX?zHH;?MI2r5gVSiPsNl;631HpdWE)d&TQab5THM<$DqE zBoF(aEDz2dKx>&NN;Dr%;?`()3*J?`e9TPhk4_Sa2dfJkDLM z6Q}~7^1I_{g>dMOXOqOU6k`zI>6CnUUZO$=;TxT%`F$-5{vFMS=P?4BPc;-s@)ZA@ zLbPc78w-VS&}x}MKS|KfNSi;jYx+ARAN`O9`hh1suph|v#Eu0)N_XBfiujnWpDQ(f z^AiexMDyyF_%z1)b_?)SF1&uwt?`4v%X+BCm0Vs&fG2r)zKF}~JS?D6zD^7)2Ar=| zz>}N}g9?vFdBt&40D2zZ)oc7?z?-!rdOwKL<#{cKrZ2|&q*=XPE%D>irB~!h9-$R; zqvrGNEPQ?rJf+L)<#%cN%4v!Z&tuR#mUMku;?;fn|1V|1Kb!^sL*Pk2JicZ*&%u;v zKFX`zuzqnq@FXYCgC%w1>w!1Z&p%|r@6>#FK1aU03SKi%0nGT6WWm=1Px2Vsb%&%! zxjd=_lo^=d&maz6SUQ zC|`Phfb;d3#Gfg;v|VETybV0EFzzZq12>a@Iq+0JdE9rsj`>g)`WcrHpYikR|0e0t zz8+T$=$%En{-E)^F3EDzOT&~d&*ObUk&BDag`2gDa^OjyypN_^)9=!Fo{wFv%Xh{c zl5?u){*fYdY5FA^-}MuPSgrAEfhYdF|0kgF2Y@I4CuY#|o4`x^ug5!VCud_r2Gv8J zM{;OBR{>A_d0bzi@t@3+-vQ8@mDj%mPx7ob$n!rsUEa6F^<>68l`hXqu>LOwp7^vF z_*?@#$k5SJTo@p-U*MO(|^124==SfMA`Txh10(NUX{4on3JC>%*^e`_A z{z8d|-S`*9ctG=i6Gn#WgK>T82GEn7ygpK)>A#VM|D##(zXYD@Gtbj9pNVHH{=8np z`d_N?y#Ail>3$PPye{dX=eS}}ule8%Ety}QFQRwt>G}llRK7g# zjbfG8GL7f?YR0eCe0VTIacg1Ez=kL)N1o4jXnZ;FRDXCM1efD-Nk3cgeD5Aj z|1j`WUc67DP1~hIz{~o!O%d`wvQIBi>GHY&N=RPU0&k|D4v9xQ(snGM>30E7?YhCx zt{>O*v7al#{hI#GEc|mFichV9PZ97`E<6uktod9AJn5%pP$3F6extwzad@Oo2)PWTDU-#C7MM)E;@{-Gi) z)%5RaKD-Xad~QGmGfOuDJeAA9^NIn?k-WYt>0u{-q@MG2L4FT-GyVS_!;2M%4KU7e0`Sw^jn%g?|<*s{HJ|V$=`BN zA(}PbE%Evx4cq`c&1)6w_M+7w)&p-^zLGwTfxi6_@Q(3l@dEIq&+Z?nK>1x7++J|8 zxgI{91z!(5$;12iyL4LL%|bsxj+>?m-nYo@Vy?uG7xdyR_`l17zX^D1cf23cqFHtW zPjVXP-~K5JpPxuR|D>!C{lKq_X#Z3EN=v!JYd*&D-Nl!h*N0C5Z{}~T1m3J&bV&SEA%3oe32T|YmWBUA zS@`?_cdEW)hn!I*q;qzVKNlxQ@<6k5_=GA|y;&c7Eutd=t z$B%8mo0W^Wv@;REG#qNZbdBGCsm~Ypx0jb*62`aRyDnK1>AZBwrA0;hZ+u{rKb~|J zMMYsi6n6bpoWLjqMt+=D;Jr2ITkCC${I&cd;OE7O5y|pWZ_qV(18 zapd@ry&}tGgAeWN*ruPzuXmOOQXjsqiDEOghjL_gx(7&osc&1Kg0#XH2*exlJ^zz% zD<9*Avk^|7a`~8$Op78s9Pb_pE9fCDv>_}qR)S#hyBk(a*9uZkMwo{X;-ZE<}0-y7)c=va>^X-}vhp=1VW zLVawW3M?3IyM)4vqD_dkw6SW1$Lm??_ImNOY~|HnPpwAQx*JK7MAWN}wGi%Wi{iny zBCmH%SCq{x?*pn zwAlO6otxrzde)J&P0_`%&ZNhmDS`Spy2g2Nw#X9DUca)W7sZ)WnpOCYzae1>>B!XdlDc>MDl$a8= zT%Bs;(Il#(6qS@v5t><-a&=0Bb8&;VdG(==P}0+g!)(gyqqo*{1hSeESy!EPXlbBw z;~*CqYe_ARXwcD`uNf2U{hhaCDq|KMHn&Hdp;F$?1dh~!!<3}sJ^bx(`)~+|QPz*B zl9c0+DV+9`y$GF7Rai~b1Ia9)>?CM@lf(W@P#dx6^N2W^MIk1?*f>Teqy%+tgguOlQcgT-IEh)xzt}K{uFG z2mGO&dX`>&)^z&f!^d7Kv~$7&Ek#R8=xL zIPkB{ENU}`4*_!buRoQGaC&Bw%ywHmm|1{uF5z<;@<%=7Iyh?4cwK|pk&76OHkIJ~ zKj#v%WPh-ab{$LPsb2bot&A=*vl)cQ&StISr&kmt zmK>?8a{zTsMAt2kE;MG(%-~AnUW%TM8Y~%SaxSEZ*{DH`K9L4PsZq;^V+m73k5U>u zQVOzNNvnw35f0*P(2OP(hsbU5D31n}qnB9&h*O3d=#0G#M5-{#)k&lke7vNyOy1#i zMx2}ITZ02JVa?uOP8W}zbctI&>2ira>C#iK7)lS>>DB4b0jI6nb8cGZUD@RHE-p4m z;Ekfgm1ih=lQ?Wnc34f(N;DqbXmA#3V^g#_(>@hdQjKehmbKx?M~pgk_EUpru8ozP za~#|0sr2C#u{9BIGWwxq+q16O>v1l|k*V=uz`HK#!y#pmY}qxZGf=)}rL$PhcKKRE z>zptgm~erpQXkMlTNFdSG^&&kgN(^gd_<#KIgQ}OTs<9> ziy1+f)sS&yN4CIp{BNR;RPU31$~5F(j&qCsWF9foU4cp1TSI|h#N$T+o(Avc>^<~{ z;Fz7NT{u6Ab}?42W#}+e5vakp2R5=8$Mb%sGw6*aWk1LBX9y_`1lxR_;UrG#rm5>X zbW9#k1GTpd1F9$2K2W_o)qsNLB4w)-m^zJxj2(C&++gU`$TKkZ@@A$;`)<+@d7Z`T zI9EEyRdubW*AmQzUT!sc7P*z36y57$KoRkGn2ZNLkp85!A1J1l@1v{jx@f70pn|cU zSve(UW#V)yH>d^LQT!0oJS~=l|0i`H9L2mY`gh% zM5?RGkfTJN_lsjp>6~pP!&u{bU+1Z3m)VmsN6uY84Mj2ALz92XeHk}u7acUz!3fGw z?a>vODLFl3YV?ZfH-oDzeW!A-H>yUz>|>g&DxP$#+(65WES8>PkREBep>{5YO_G)x z$&mA(X5tUTPfQ{^}{(jZ?-cJXpJbInKB3}pQ=6AOkHXOV}axBAr* zgIvifDg7v?LCWM3rD_&UX2WuUsb4HBVhz<$nM@VSUX#&dYE?aM+1T0hr{&lX+tds! z3YN!prjtj~Z+IUmp*3V3d1t!I=3O*Y=j(`h8rv}Ui5ZWM=(@~V5NTUa zGP5#6Etqz~k~0rXQvr_OpQ=g!J{-2hN@>Nn+`d)QXbVw zm$Xmm=ajIYLz6mzAPjGmmi%eKNqKCf8`w=l>6YHw?laoVu`)B6Ts$q+!26P0MdVDA z-Xb7-%&bl6wAiP2t*EB7=xkd2m96Wsl%BZN7xUcG{yrDY$z8zwMX}d;dLm&Y3Ei7I zXl9jRY}LXnR8|$)t0^DWJ62t?rHH&QVsX7t?JpOFDKjczE+E5WsohMyDUtM z``Tz%54O-mGJSquYc$UNfSSKd_}ei%hA8r;GaP(hov^dy)_63s2D8tBPCr(M$$qGw z5_^s!k#U*JlG0nS(<_NjTfz;mi(tMz77fd}$3Ot9o+%fKZQ6%p3{*e0 zII9(P7GoD!u`qTz%mWK^knA=i4U+_GxwI;)4MMnBc* z^{(qUm7`kMyaHYh@1Ded!f-SaY}V@nrpfbSTN&@uI`w)RjMdeVTSHh1Z44$l<({~j z8tfY^mPQi0UCBhW2FY%G)H|to58-LgdD9Y)GUuMgj?Af~BJ$$LdQs|T1UIpa;ae+I zkby{FUi0}s(;14RRgKNikc~I1PL+Y+x?q@AqOw}3l>5S+LsO_6i%D4bF*cVnq|%gJ z>`%=E^I*uEb(NNCTE*2X>zW%qv~y$GGLPE}pCaXho&GXTR_m+!v~dfA<=D094R*xP zons;!Q<9zy*_0(67p$(P_7PVQ&%*Op}g!@H&f zYm7Wh!4@Vggqc)`SZ6%wMTeH`^dZqx*M~BH+`kxt--51}8nkJ1ZtI8!+AD8I7i)lXO)Uz$&#(VrD`K2ZlVNN`hAmSYqA0Px18drV$+&{;d06_pu(hSAwKEhBToMY1B6^Zr!ixgyBj6__OvY6Rw$IT3D&xV6 zFzk)+5uv_|g_EL)MjN6Cle+jX`-CD44KR%$ih}L>6@vBv;_?%i`mr=Fw=Th|(bBx+ zq24Y~JoujYP@-Td1@v`<{D>b#QYr~7H4D6^(m@`venaT51Nhv5pm*E&y}$~6!;r$$ zFMbb>ad@3ro{_@woAnJt2NP<@@98lbuPbH5PwMy#3z;xWe0pE?zi~zir_b;IGi=CF z5j9Ai%ff6|;$|{_9G}0}&#)bz9H1)~KO27hejUG8b&bYzd_K2^A)iwNGI<&GcQdeb z1`NmN@BcGAR|l~COvi97;?tQj{LJSBFf7&ylKk{z`Ee_+4fs!G!twb$0fyapHIFV% zpW}1>HzADTR}&E~J~xLUpR+^p@ky0b`W*kOz>$a~8(n-p0mE4eo%&B{rC)!KhxGV- zjsZjZ?p^xD?;kVlGsNfbs56`uNsm1yKV$DP#OHGn7;-vHZmX?=&)eGR0WMH>OXoS(E;ljL&0WxMRAC$N#zf`FRL8>GAp828MmiSVth* z^y?`+Ov8`b7G3;Z$H5#$>5$m5f1EzUR}e%Za(q4q;)ssVbj*RD8U6tQloH41@BIwx z_$?fu8E`!2_h;O&Z6F+f-_kWv$={;$&-I(*r^*pz>FM)1RsEB7d_#e8e1;@udi>OP zDe_f(yxy5{aXdy+GKAx7@^?5(bo}B8OsSwTK9NyM#hGOI{)S4w-hfwF6^DZ8*-%Af zjBXvj0`Hg8Mc -#include -#include -#include -#include -#include -#include "state_estimator_lcmt.hpp" -#include "leg_control_data_lcmt.hpp" -#include "pd_tau_targets_lcmt.hpp" -#include "rc_command_lcmt.hpp" - -using namespace std; -using namespace UNITREE_LEGGED_SDK; - -class Custom -{ -public: - Custom(uint8_t level): safe(LeggedType::Go1), udp(level, 8090, "192.168.123.10", 8007) { - udp.InitCmdData(cmd); - } - void UDPRecv(); - void UDPSend(); - void RobotControl(); - void init(); - void handleActionLCM(const lcm::ReceiveBuffer *rbuf, const std::string & chan, const pd_tau_targets_lcmt * msg); - void _simpleLCMThread(); - - Safety safe; - UDP udp; - LowCmd cmd = {0}; - LowState state = {0}; - float qInit[3]={0}; - float qDes[3]={0}; - float sin_mid_q[3] = {0.0, 1.2, -2.0}; - float Kp[3] = {0}; - float Kd[3] = {0}; - double time_consume = 0; - int rate_count = 0; - int sin_count = 0; - int motiontime = 0; - float dt = 0.002; // 0.001~0.01 - - lcm::LCM _simpleLCM; - std::thread _simple_LCM_thread; - bool _firstCommandReceived; - bool _firstRun; - state_estimator_lcmt body_state_simple = {0}; - leg_control_data_lcmt joint_state_simple = {0}; - pd_tau_targets_lcmt joint_command_simple = {0}; - rc_command_lcmt rc_command = {0}; - - xRockerBtnDataStruct _keyData; - int mode = 0; - -}; - -void Custom::init() -{ - _simpleLCM.subscribe("pd_plustau_targets", &Custom::handleActionLCM, this); - _simple_LCM_thread = std::thread(&Custom::_simpleLCMThread, this); - - _firstCommandReceived = false; - _firstRun = true; - - // set nominal pose - - for(int i = 0; i < 12; i++){ - joint_command_simple.qd_des[i] = 0; - joint_command_simple.tau_ff[i] = 0; - joint_command_simple.kp[i] = 20.; - joint_command_simple.kd[i] = 0.5; - } - - joint_command_simple.q_des[0] = -0.3; - joint_command_simple.q_des[1] = 1.2; - joint_command_simple.q_des[2] = -2.721; - joint_command_simple.q_des[3] = 0.3; - joint_command_simple.q_des[4] = 1.2; - joint_command_simple.q_des[5] = -2.721; - joint_command_simple.q_des[6] = -0.3; - joint_command_simple.q_des[7] = 1.2; - joint_command_simple.q_des[8] = -2.721; - joint_command_simple.q_des[9] = 0.3; - joint_command_simple.q_des[10] = 1.2; - joint_command_simple.q_des[11] = -2.721; - - printf("SET NOMINAL POSE"); - - -} - -void Custom::UDPRecv() -{ - udp.Recv(); -} - -void Custom::UDPSend() -{ - udp.Send(); -} - -double jointLinearInterpolation(double initPos, double targetPos, double rate) -{ - double p; - rate = std::min(std::max(rate, 0.0), 1.0); - p = initPos*(1-rate) + targetPos*rate; - return p; -} - -void Custom::handleActionLCM(const lcm::ReceiveBuffer *rbuf, const std::string & chan, const pd_tau_targets_lcmt * msg){ - (void) rbuf; - (void) chan; - - joint_command_simple = *msg; - _firstCommandReceived = true; - -} - -void Custom::_simpleLCMThread(){ - while(true){ - _simpleLCM.handle(); - } -} - -void Custom::RobotControl() -{ - motiontime++; - udp.GetRecv(state); - - memcpy(&_keyData, &state.wirelessRemote[0], 40); - - rc_command.left_stick[0] = _keyData.lx; - rc_command.left_stick[1] = _keyData.ly; - rc_command.right_stick[0] = _keyData.rx; - rc_command.right_stick[1] = _keyData.ry; - rc_command.right_lower_right_switch = _keyData.btn.components.R2; - rc_command.right_upper_switch = _keyData.btn.components.R1; - rc_command.left_lower_left_switch = _keyData.btn.components.L2; - rc_command.left_upper_switch = _keyData.btn.components.L1; - - - if(_keyData.btn.components.A > 0){ - mode = 0; - } else if(_keyData.btn.components.B > 0){ - mode = 1; - }else if(_keyData.btn.components.X > 0){ - mode = 2; - }else if(_keyData.btn.components.Y > 0){ - mode = 3; - }else if(_keyData.btn.components.up > 0){ - mode = 4; - }else if(_keyData.btn.components.right > 0){ - mode = 5; - }else if(_keyData.btn.components.down > 0){ - mode = 6; - }else if(_keyData.btn.components.left > 0){ - mode = 7; - } - - rc_command.mode = mode; - - - // publish state to LCM - for(int i = 0; i < 12; i++){ - joint_state_simple.q[i] = state.motorState[i].q; - joint_state_simple.qd[i] = state.motorState[i].dq; - joint_state_simple.tau_est[i] = state.motorState[i].tauEst; - } - for(int i = 0; i < 4; i++){ - body_state_simple.quat[i] = state.imu.quaternion[i]; - } - for(int i = 0; i < 3; i++){ - body_state_simple.rpy[i] = state.imu.rpy[i]; - body_state_simple.aBody[i] = state.imu.accelerometer[i]; - body_state_simple.omegaBody[i] = state.imu.gyroscope[i]; - } - for(int i = 0; i < 4; i++){ - body_state_simple.contact_estimate[i] = state.footForce[i]; - } - - _simpleLCM.publish("state_estimator_data", &body_state_simple); - _simpleLCM.publish("leg_control_data", &joint_state_simple); - _simpleLCM.publish("rc_command", &rc_command); - - if(_firstRun && joint_state_simple.q[0] != 0){ - for(int i = 0; i < 12; i++){ - joint_command_simple.q_des[i] = joint_state_simple.q[i]; - } - _firstRun = false; - } - - for(int i = 0; i < 12; i++){ - cmd.motorCmd[i].q = joint_command_simple.q_des[i]; - cmd.motorCmd[i].dq = joint_command_simple.qd_des[i]; - cmd.motorCmd[i].Kp = joint_command_simple.kp[i]; - cmd.motorCmd[i].Kd = joint_command_simple.kd[i]; - cmd.motorCmd[i].tau = joint_command_simple.tau_ff[i]; - } - - safe.PositionLimit(cmd); - int res1 = safe.PowerProtect(cmd, state, 9); - udp.SetSend(cmd); - -} - - -int main(void) -{ - std::cout << "Communication level is set to LOW-level." << std::endl - << "WARNING: Make sure the robot is hung up." << std::endl - << "Press Enter to continue..." << std::endl; - std::cin.ignore(); - - Custom custom(LOWLEVEL); - custom.init(); - // InitEnvironment(); - LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); - LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); - LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); - - loop_udpSend.start(); - loop_udpRecv.start(); - loop_control.start(); - - while(1){ - sleep(10); - }; - - return 0; -} diff --git a/go1_gym_deploy/utils/__init__.py b/go1_gym_deploy/utils/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/go1_gym_deploy/utils/cheetah_state_estimator.py b/go1_gym_deploy/utils/cheetah_state_estimator.py deleted file mode 100755 index 44c83d5..0000000 --- a/go1_gym_deploy/utils/cheetah_state_estimator.py +++ /dev/null @@ -1,406 +0,0 @@ -import math -import select -import threading -import time - -import numpy as np - -from go1_gym_deploy.lcm_types.leg_control_data_lcmt import leg_control_data_lcmt -from go1_gym_deploy.lcm_types.rc_command_lcmt import rc_command_lcmt -from go1_gym_deploy.lcm_types.state_estimator_lcmt import state_estimator_lcmt -from go1_gym_deploy.lcm_types.camera_message_lcmt import camera_message_lcmt -from go1_gym_deploy.lcm_types.camera_message_rect_wide import camera_message_rect_wide - - -def get_rpy_from_quaternion(q): - w, x, y, z = q - r = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x ** 2 + y ** 2)) - p = np.arcsin(2 * (w * y - z * x)) - y = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2)) - return np.array([r, p, y]) - - -def get_rotation_matrix_from_rpy(rpy): - """ - Get rotation matrix from the given quaternion. - Args: - q (np.array[float[4]]): quaternion [w,x,y,z] - Returns: - np.array[float[3,3]]: rotation matrix. - """ - r, p, y = rpy - R_x = np.array([[1, 0, 0], - [0, math.cos(r), -math.sin(r)], - [0, math.sin(r), math.cos(r)] - ]) - - R_y = np.array([[math.cos(p), 0, math.sin(p)], - [0, 1, 0], - [-math.sin(p), 0, math.cos(p)] - ]) - - R_z = np.array([[math.cos(y), -math.sin(y), 0], - [math.sin(y), math.cos(y), 0], - [0, 0, 1] - ]) - - rot = np.dot(R_z, np.dot(R_y, R_x)) - return rot - - -class StateEstimator: - def __init__(self, lc, use_cameras=True): - - # reverse legs - self.joint_idxs = [3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8] - self.contact_idxs = [1, 0, 3, 2] - # self.joint_idxs = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11] - - self.lc = lc - - self.joint_pos = np.zeros(12) - self.joint_vel = np.zeros(12) - self.tau_est = np.zeros(12) - self.world_lin_vel = np.zeros(3) - self.world_ang_vel = np.zeros(3) - self.euler = np.zeros(3) - self.R = np.eye(3) - self.buf_idx = 0 - - self.smoothing_length = 12 - self.deuler_history = np.zeros((self.smoothing_length, 3)) - self.dt_history = np.zeros((self.smoothing_length, 1)) - self.euler_prev = np.zeros(3) - self.timuprev = time.time() - - self.body_lin_vel = np.zeros(3) - self.body_ang_vel = np.zeros(3) - self.smoothing_ratio = 0.2 - - self.contact_state = np.ones(4) - - self.mode = 0 - self.ctrlmode_left = 0 - self.ctrlmode_right = 0 - self.left_stick = [0, 0] - self.right_stick = [0, 0] - self.left_upper_switch = 0 - self.left_lower_left_switch = 0 - self.left_lower_right_switch = 0 - self.right_upper_switch = 0 - self.right_lower_left_switch = 0 - self.right_lower_right_switch = 0 - self.left_upper_switch_pressed = 0 - self.left_lower_left_switch_pressed = 0 - self.left_lower_right_switch_pressed = 0 - self.right_upper_switch_pressed = 0 - self.right_lower_left_switch_pressed = 0 - self.right_lower_right_switch_pressed = 0 - - # default trotting gait - self.cmd_freq = 3.0 - self.cmd_phase = 0.5 - self.cmd_offset = 0.0 - self.cmd_duration = 0.5 - - - self.init_time = time.time() - self.received_first_legdata = False - - self.imu_subscription = self.lc.subscribe("state_estimator_data", self._imu_cb) - self.legdata_state_subscription = self.lc.subscribe("leg_control_data", self._legdata_cb) - self.rc_command_subscription = self.lc.subscribe("rc_command", self._rc_command_cb) - - if use_cameras: - for cam_id in [1, 2, 3, 4, 5]: - self.camera_subscription = self.lc.subscribe(f"camera{cam_id}", self._camera_cb) - self.camera_names = ["front", "bottom", "left", "right", "rear"] - for cam_name in self.camera_names: - self.camera_subscription = self.lc.subscribe(f"rect_image_{cam_name}", self._rect_camera_cb) - self.camera_image_left = None - self.camera_image_right = None - self.camera_image_front = None - self.camera_image_bottom = None - self.camera_image_rear = None - - self.body_loc = np.array([0, 0, 0]) - self.body_quat = np.array([0, 0, 0, 1]) - - def get_body_linear_vel(self): - self.body_lin_vel = np.dot(self.R.T, self.world_lin_vel) - return self.body_lin_vel - - def get_body_angular_vel(self): - self.body_ang_vel = self.smoothing_ratio * np.mean(self.deuler_history / self.dt_history, axis=0) + ( - 1 - self.smoothing_ratio) * self.body_ang_vel - return self.body_ang_vel - - def get_gravity_vector(self): - grav = np.dot(self.R.T, np.array([0, 0, -1])) - return grav - - def get_contact_state(self): - return self.contact_state[self.contact_idxs] - - def get_rpy(self): - return self.euler - - def get_command(self): - MODES_LEFT = ["body_height", "lat_vel", "stance_width"] - MODES_RIGHT = ["step_frequency", "footswing_height", "body_pitch"] - - if self.left_upper_switch_pressed: - self.ctrlmode_left = (self.ctrlmode_left + 1) % 3 - self.left_upper_switch_pressed = False - if self.right_upper_switch_pressed: - self.ctrlmode_right = (self.ctrlmode_right + 1) % 3 - self.right_upper_switch_pressed = False - - MODE_LEFT = MODES_LEFT[self.ctrlmode_left] - MODE_RIGHT = MODES_RIGHT[self.ctrlmode_right] - - # always in use - cmd_x = 1 * self.left_stick[1] - cmd_yaw = -1 * self.right_stick[0] - - # default values - cmd_y = 0. # -1 * self.left_stick[0] - cmd_height = 0. - cmd_footswing = 0.08 - cmd_stance_width = 0.33 - cmd_stance_length = 0.40 - cmd_ori_pitch = 0. - cmd_ori_roll = 0. - cmd_freq = 3.0 - - # joystick commands - if MODE_LEFT == "body_height": - cmd_height = 0.3 * self.left_stick[0] - elif MODE_LEFT == "lat_vel": - cmd_y = 0.6 * self.left_stick[0] - elif MODE_LEFT == "stance_width": - cmd_stance_width = 0.275 + 0.175 * self.left_stick[0] - if MODE_RIGHT == "step_frequency": - min_freq = 2.0 - max_freq = 4.0 - cmd_freq = (1 + self.right_stick[1]) / 2 * (max_freq - min_freq) + min_freq - elif MODE_RIGHT == "footswing_height": - cmd_footswing = max(0, self.right_stick[1]) * 0.32 + 0.03 - elif MODE_RIGHT == "body_pitch": - cmd_ori_pitch = -0.4 * self.right_stick[1] - - # gait buttons - if self.mode == 0: - self.cmd_phase = 0.5 - self.cmd_offset = 0.0 - self.cmd_bound = 0.0 - self.cmd_duration = 0.5 - elif self.mode == 1: - self.cmd_phase = 0.0 - self.cmd_offset = 0.0 - self.cmd_bound = 0.0 - self.cmd_duration = 0.5 - elif self.mode == 2: - self.cmd_phase = 0.0 - self.cmd_offset = 0.5 - self.cmd_bound = 0.0 - self.cmd_duration = 0.5 - elif self.mode == 3: - self.cmd_phase = 0.0 - self.cmd_offset = 0.0 - self.cmd_bound = 0.5 - self.cmd_duration = 0.5 - else: - self.cmd_phase = 0.5 - self.cmd_offset = 0.0 - self.cmd_bound = 0.0 - self.cmd_duration = 0.5 - - return np.array([cmd_x, cmd_y, cmd_yaw, cmd_height, cmd_freq, self.cmd_phase, self.cmd_offset, self.cmd_bound, - self.cmd_duration, cmd_footswing, cmd_ori_pitch, cmd_ori_roll, cmd_stance_width, - cmd_stance_length, 0, 0, 0, 0, 0]) - - def get_buttons(self): - return np.array([self.left_lower_left_switch, self.left_upper_switch, self.right_lower_right_switch, self.right_upper_switch]) - - def get_dof_pos(self): - # print("dofposquery", self.joint_pos[self.joint_idxs]) - return self.joint_pos[self.joint_idxs] - - def get_dof_vel(self): - return self.joint_vel[self.joint_idxs] - - def get_tau_est(self): - return self.tau_est[self.joint_idxs] - - def get_yaw(self): - return self.euler[2] - - def get_body_loc(self): - return np.array(self.body_loc) - - def get_body_quat(self): - return np.array(self.body_quat) - - def get_camera_front(self): - return self.camera_image_front - - def get_camera_bottom(self): - return self.camera_image_bottom - - def get_camera_rear(self): - return self.camera_image_rear - - def get_camera_left(self): - return self.camera_image_left - - def get_camera_right(self): - return self.camera_image_right - - def _legdata_cb(self, channel, data): - # print("update legdata") - if not self.received_first_legdata: - self.received_first_legdata = True - print(f"First legdata: {time.time() - self.init_time}") - - msg = leg_control_data_lcmt.decode(data) - # print(msg.q) - self.joint_pos = np.array(msg.q) - self.joint_vel = np.array(msg.qd) - self.tau_est = np.array(msg.tau_est) - # print(f"update legdata {msg.id}") - - def _imu_cb(self, channel, data): - # print("update imu") - msg = state_estimator_lcmt.decode(data) - - self.euler = np.array(msg.rpy) - - self.R = get_rotation_matrix_from_rpy(self.euler) - - self.contact_state = 1.0 * (np.array(msg.contact_estimate) > 200) - - self.deuler_history[self.buf_idx % self.smoothing_length, :] = msg.rpy - self.euler_prev - self.dt_history[self.buf_idx % self.smoothing_length] = time.time() - self.timuprev - - self.timuprev = time.time() - - self.buf_idx += 1 - self.euler_prev = np.array(msg.rpy) - - def _sensor_cb(self, channel, data): - pass - - def _rc_command_cb(self, channel, data): - - msg = rc_command_lcmt.decode(data) - - - self.left_upper_switch_pressed = ((msg.left_upper_switch and not self.left_upper_switch) or self.left_upper_switch_pressed) - self.left_lower_left_switch_pressed = ((msg.left_lower_left_switch and not self.left_lower_left_switch) or self.left_lower_left_switch_pressed) - self.left_lower_right_switch_pressed = ((msg.left_lower_right_switch and not self.left_lower_right_switch) or self.left_lower_right_switch_pressed) - self.right_upper_switch_pressed = ((msg.right_upper_switch and not self.right_upper_switch) or self.right_upper_switch_pressed) - self.right_lower_left_switch_pressed = ((msg.right_lower_left_switch and not self.right_lower_left_switch) or self.right_lower_left_switch_pressed) - self.right_lower_right_switch_pressed = ((msg.right_lower_right_switch and not self.right_lower_right_switch) or self.right_lower_right_switch_pressed) - - self.mode = msg.mode - self.right_stick = msg.right_stick - self.left_stick = msg.left_stick - self.left_upper_switch = msg.left_upper_switch - self.left_lower_left_switch = msg.left_lower_left_switch - self.left_lower_right_switch = msg.left_lower_right_switch - self.right_upper_switch = msg.right_upper_switch - self.right_lower_left_switch = msg.right_lower_left_switch - self.right_lower_right_switch = msg.right_lower_right_switch - - # print(self.right_stick, self.left_stick) - - def _camera_cb(self, channel, data): - msg = camera_message_lcmt.decode(data) - - img = np.fromstring(msg.data, dtype=np.uint8) - img = img.reshape((3, 200, 464)).transpose(1, 2, 0) - - cam_id = int(channel[-1]) - if cam_id == 1: - self.camera_image_front = img - elif cam_id == 2: - self.camera_image_bottom = img - elif cam_id == 3: - self.camera_image_left = img - elif cam_id == 4: - self.camera_image_right = img - elif cam_id == 5: - self.camera_image_rear = img - else: - print("Image received from camera with unknown ID#!") - - #im = Image.fromarray(img).convert('RGB') - - #im.save("test_image_" + channel + ".jpg") - #print(channel) - - def _rect_camera_cb(self, channel, data): - message_types = [camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide, - camera_message_rect_wide, camera_message_rect_wide] - image_shapes = [(116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3)] - - cam_name = channel.split("_")[-1] - # print(f"received py from {cam_name}") - cam_id = self.camera_names.index(cam_name) + 1 - - msg = message_types[cam_id - 1].decode(data) - - img = np.fromstring(msg.data, dtype=np.uint8) - img = np.flip(np.flip( - img.reshape((image_shapes[cam_id - 1][2], image_shapes[cam_id - 1][1], image_shapes[cam_id - 1][0])), - axis=0), axis=1).transpose(1, 2, 0) - # print(img.shape) - # img = np.flip(np.flip(img.reshape(image_shapes[cam_id - 1]), axis=0), axis=1)[:, :, - # [2, 1, 0]] # .transpose(1, 2, 0) - - if cam_id == 1: - self.camera_image_front = img - elif cam_id == 2: - self.camera_image_bottom = img - elif cam_id == 3: - self.camera_image_left = img - elif cam_id == 4: - self.camera_image_right = img - elif cam_id == 5: - self.camera_image_rear = img - else: - print("Image received from camera with unknown ID#!") - - def poll(self, cb=None): - t = time.time() - try: - while True: - timeout = 0.01 - rfds, wfds, efds = select.select([self.lc.fileno()], [], [], timeout) - if rfds: - # print("message received!") - self.lc.handle() - # print(f'Freq {1. / (time.time() - t)} Hz'); t = time.time() - else: - continue - # print(f'waiting for message... Freq {1. / (time.time() - t)} Hz'); t = time.time() - # if cb is not None: - # cb() - except KeyboardInterrupt: - pass - - def spin(self): - self.run_thread = threading.Thread(target=self.poll, daemon=False) - self.run_thread.start() - - def close(self): - self.lc.unsubscribe(self.legdata_state_subscription) - - -if __name__ == "__main__": - import lcm - - lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255") - se = StateEstimator(lc) - se.poll() diff --git a/go1_gym_deploy/utils/command_profile.py b/go1_gym_deploy/utils/command_profile.py deleted file mode 100644 index 9902897..0000000 --- a/go1_gym_deploy/utils/command_profile.py +++ /dev/null @@ -1,231 +0,0 @@ -import torch - - -class CommandProfile: - def __init__(self, dt, max_time_s=10.): - self.dt = dt - self.max_timestep = int(max_time_s / self.dt) - self.commands = torch.zeros((self.max_timestep, 9)) - self.start_time = 0 - - def get_command(self, t): - timestep = int((t - self.start_time) / self.dt) - timestep = min(timestep, self.max_timestep - 1) - return self.commands[timestep, :] - - def get_buttons(self): - return [0, 0, 0, 0] - - def reset(self, reset_time): - self.start_time = reset_time - - -class ConstantAccelerationProfile(CommandProfile): - def __init__(self, dt, max_speed, accel_time, zero_buf_time=0): - super().__init__(dt) - zero_buf_timesteps = int(zero_buf_time / self.dt) - accel_timesteps = int(accel_time / self.dt) - self.commands[:zero_buf_timesteps] = 0 - self.commands[zero_buf_timesteps:zero_buf_timesteps + accel_timesteps, 0] = torch.arange(0, max_speed, - step=max_speed / accel_timesteps) - self.commands[zero_buf_timesteps + accel_timesteps:, 0] = max_speed - - -class ElegantForwardProfile(CommandProfile): - def __init__(self, dt, max_speed, accel_time, duration, deaccel_time, zero_buf_time=0): - import numpy as np - - zero_buf_timesteps = int(zero_buf_time / dt) - accel_timesteps = int(accel_time / dt) - duration_timesteps = int(duration / dt) - deaccel_timesteps = int(deaccel_time / dt) - - total_time_s = zero_buf_time + accel_time + duration + deaccel_time - - super().__init__(dt, total_time_s) - - x_vel_cmds = [0] * zero_buf_timesteps + [*np.linspace(0, max_speed, accel_timesteps)] + \ - [max_speed] * duration_timesteps + [*np.linspace(max_speed, 0, deaccel_timesteps)] - - self.commands[:len(x_vel_cmds), 0] = torch.Tensor(x_vel_cmds) - - -class ElegantYawProfile(CommandProfile): - def __init__(self, dt, max_speed, zero_buf_time, accel_time, duration, deaccel_time, yaw_rate): - import numpy as np - - zero_buf_timesteps = int(zero_buf_time / dt) - accel_timesteps = int(accel_time / dt) - duration_timesteps = int(duration / dt) - deaccel_timesteps = int(deaccel_time / dt) - - total_time_s = zero_buf_time + accel_time + duration + deaccel_time - - super().__init__(dt, total_time_s) - - x_vel_cmds = [0] * zero_buf_timesteps + [*np.linspace(0, max_speed, accel_timesteps)] + \ - [max_speed] * duration_timesteps + [*np.linspace(max_speed, 0, deaccel_timesteps)] - - yaw_vel_cmds = [0] * zero_buf_timesteps + [0] * accel_timesteps + \ - [yaw_rate] * duration_timesteps + [0] * deaccel_timesteps - - self.commands[:len(x_vel_cmds), 0] = torch.Tensor(x_vel_cmds) - self.commands[:len(yaw_vel_cmds), 2] = torch.Tensor(yaw_vel_cmds) - - -class ElegantGaitProfile(CommandProfile): - def __init__(self, dt, filename): - import numpy as np - import json - - with open(f'../command_profiles/{filename}', 'r') as file: - command_sequence = json.load(file) - - len_command_sequence = len(command_sequence["x_vel_cmd"]) - total_time_s = int(len_command_sequence / dt) - - super().__init__(dt, total_time_s) - - self.commands[:len_command_sequence, 0] = torch.Tensor(command_sequence["x_vel_cmd"]) - self.commands[:len_command_sequence, 2] = torch.Tensor(command_sequence["yaw_vel_cmd"]) - self.commands[:len_command_sequence, 3] = torch.Tensor(command_sequence["height_cmd"]) - self.commands[:len_command_sequence, 4] = torch.Tensor(command_sequence["frequency_cmd"]) - self.commands[:len_command_sequence, 5] = torch.Tensor(command_sequence["offset_cmd"]) - self.commands[:len_command_sequence, 6] = torch.Tensor(command_sequence["phase_cmd"]) - self.commands[:len_command_sequence, 7] = torch.Tensor(command_sequence["bound_cmd"]) - self.commands[:len_command_sequence, 8] = torch.Tensor(command_sequence["duration_cmd"]) - -class RCControllerProfile(CommandProfile): - def __init__(self, dt, state_estimator, x_scale=1.0, y_scale=1.0, yaw_scale=1.0, probe_vel_multiplier=1.0): - super().__init__(dt) - self.state_estimator = state_estimator - self.x_scale = x_scale - self.y_scale = y_scale - self.yaw_scale = yaw_scale - - self.probe_vel_multiplier = probe_vel_multiplier - - self.triggered_commands = {i: None for i in range(4)} # command profiles for each action button on the controller - self.currently_triggered = [0, 0, 0, 0] - self.button_states = [0, 0, 0, 0] - - def get_command(self, t, probe=False): - - command = self.state_estimator.get_command() - command[0] = command[0] * self.x_scale - command[1] = command[1] * self.y_scale - command[2] = command[2] * self.yaw_scale - - reset_timer = False - - if probe: - command[0] = command[0] * self.probe_vel_multiplier - command[2] = command[2] * self.probe_vel_multiplier - - # check for action buttons - prev_button_states = self.button_states[:] - self.button_states = self.state_estimator.get_buttons() - for button in range(4): - if self.triggered_commands[button] is not None: - if self.button_states[button] == 1 and prev_button_states[button] == 0: - if not self.currently_triggered[button]: - # reset the triggered action - self.triggered_commands[button].reset(t) - # reset the internal timing variable - reset_timer = True - self.currently_triggered[button] = True - else: - self.currently_triggered[button] = False - # execute the triggered action - if self.currently_triggered[button] and t < self.triggered_commands[button].max_timestep: - command = self.triggered_commands[button].get_command(t) - - - return command, reset_timer - - def add_triggered_command(self, button_idx, command_profile): - self.triggered_commands[button_idx] = command_profile - - def get_buttons(self): - return self.state_estimator.get_buttons() - -class RCControllerProfileAccel(RCControllerProfile): - def __init__(self, dt, state_estimator, x_scale=1.0, y_scale=1.0, yaw_scale=1.0): - super().__init__(dt, state_estimator, x_scale=x_scale, y_scale=y_scale, yaw_scale=yaw_scale) - self.x_scale, self.y_scale, self.yaw_scale = self.x_scale / 100., self.y_scale / 100., self.yaw_scale / 100. - self.velocity_command = torch.zeros(3) - - def get_command(self, t): - - accel_command = self.state_estimator.get_command() - self.velocity_command[0] = self.velocity_command[0] + accel_command[0] * self.x_scale - self.velocity_command[1] = self.velocity_command[1] + accel_command[1] * self.y_scale - self.velocity_command[2] = self.velocity_command[2] + accel_command[2] * self.yaw_scale - - # check for action buttons - prev_button_states = self.button_states[:] - self.button_states = self.state_estimator.get_buttons() - for button in range(4): - if self.button_states[button] == 1 and self.triggered_commands[button] is not None: - if prev_button_states[button] == 0: - # reset the triggered action - self.triggered_commands[button].reset(t) - # execute the triggered action - return self.triggered_commands[button].get_command(t) - - return self.velocity_command[:] - - def add_triggered_command(self, button_idx, command_profile): - self.triggered_commands[button_idx] = command_profile - - def get_buttons(self): - return self.state_estimator.get_buttons() - - - - - -class KeyboardProfile(CommandProfile): - # for control via keyboard inputs to isaac gym visualizer - def __init__(self, dt, isaac_env, x_scale=1.0, y_scale=1.0, yaw_scale=1.0): - super().__init__(dt) - from isaacgym.gymapi import KeyboardInput - self.gym = isaac_env.gym - self.viewer = isaac_env.viewer - self.x_scale = x_scale - self.y_scale = y_scale - self.yaw_scale = yaw_scale - self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_UP, "FORWARD") - self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_DOWN, "REVERSE") - self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_LEFT, "LEFT") - self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_RIGHT, "RIGHT") - - self.keyb_command = [0, 0, 0] - self.command = [0, 0, 0] - - def get_command(self, t): - events = self.gym.query_viewer_action_events(self.viewer) - events_dict = {event.action: event.value for event in events} - print(events_dict) - if "FORWARD" in events_dict and events_dict["FORWARD"] == 1.0: self.keyb_command[0] = 1.0 - if "FORWARD" in events_dict and events_dict["FORWARD"] == 0.0: self.keyb_command[0] = 0.0 - if "REVERSE" in events_dict and events_dict["REVERSE"] == 1.0: self.keyb_command[0] = -1.0 - if "REVERSE" in events_dict and events_dict["REVERSE"] == 0.0: self.keyb_command[0] = 0.0 - if "LEFT" in events_dict and events_dict["LEFT"] == 1.0: self.keyb_command[1] = 1.0 - if "LEFT" in events_dict and events_dict["LEFT"] == 0.0: self.keyb_command[1] = 0.0 - if "RIGHT" in events_dict and events_dict["RIGHT"] == 1.0: self.keyb_command[1] = -1.0 - if "RIGHT" in events_dict and events_dict["RIGHT"] == 0.0: self.keyb_command[1] = 0.0 - - self.command[0] = self.keyb_command[0] * self.x_scale - self.command[1] = self.keyb_command[2] * self.y_scale - self.command[2] = self.keyb_command[1] * self.yaw_scale - - print(self.command) - - return self.command - - -if __name__ == "__main__": - cmdprof = ConstantAccelerationProfile(dt=0.2, max_speed=4, accel_time=3) - print(cmdprof.commands) - print(cmdprof.get_command(2)) diff --git a/go1_gym_deploy/utils/deployment_runner.py b/go1_gym_deploy/utils/deployment_runner.py deleted file mode 100755 index e985f92..0000000 --- a/go1_gym_deploy/utils/deployment_runner.py +++ /dev/null @@ -1,222 +0,0 @@ -import copy -import time -import os - -import numpy as np -import torch - -from go1_gym_deploy.utils.logger import MultiLogger - - -class DeploymentRunner: - def __init__(self, experiment_name="unnamed", se=None, log_root="."): - self.agents = {} - self.policy = None - self.command_profile = None - self.logger = MultiLogger() - self.se = se - self.vision_server = None - - self.log_root = log_root - self.init_log_filename() - self.control_agent_name = None - self.command_agent_name = None - - self.triggered_commands = {i: None for i in range(4)} # command profiles for each action button on the controller - self.button_states = np.zeros(4) - - self.is_currently_probing = False - self.is_currently_logging = [False, False, False, False] - - def init_log_filename(self): - datetime = time.strftime("%Y/%m_%d/%H_%M_%S") - - for i in range(100): - try: - os.makedirs(f"{self.log_root}/{datetime}_{i}") - self.log_filename = f"{self.log_root}/{datetime}_{i}/log.pkl" - return - except FileExistsError: - continue - - - def add_open_loop_agent(self, agent, name): - self.agents[name] = agent - self.logger.add_robot(name, agent.env.cfg) - - def add_control_agent(self, agent, name): - self.control_agent_name = name - self.agents[name] = agent - self.logger.add_robot(name, agent.env.cfg) - - def add_vision_server(self, vision_server): - self.vision_server = vision_server - - def set_command_agents(self, name): - self.command_agent = name - - def add_policy(self, policy): - self.policy = policy - - def add_command_profile(self, command_profile): - self.command_profile = command_profile - - - def calibrate(self, wait=True, low=False): - # first, if the robot is not in nominal pose, move slowly to the nominal pose - for agent_name in self.agents.keys(): - if hasattr(self.agents[agent_name], "get_obs"): - agent = self.agents[agent_name] - agent.get_obs() - joint_pos = agent.dof_pos - if low: - final_goal = np.array([0., 0.3, -0.7, - 0., 0.3, -0.7, - 0., 0.3, -0.7, - 0., 0.3, -0.7,]) - else: - final_goal = np.zeros(12) - nominal_joint_pos = agent.default_dof_pos - - print(f"About to calibrate; the robot will stand [Press R2 to calibrate]") - while wait: - self.button_states = self.command_profile.get_buttons() - if self.command_profile.state_estimator.right_lower_right_switch_pressed: - self.command_profile.state_estimator.right_lower_right_switch_pressed = False - break - - cal_action = np.zeros((agent.num_envs, agent.num_actions)) - target_sequence = [] - target = joint_pos - nominal_joint_pos - while np.max(np.abs(target - final_goal)) > 0.01: - target -= np.clip((target - final_goal), -0.05, 0.05) - target_sequence += [copy.deepcopy(target)] - for target in target_sequence: - next_target = target - if isinstance(agent.cfg, dict): - hip_reduction = agent.cfg["control"]["hip_scale_reduction"] - action_scale = agent.cfg["control"]["action_scale"] - else: - hip_reduction = agent.cfg.control.hip_scale_reduction - action_scale = agent.cfg.control.action_scale - - next_target[[0, 3, 6, 9]] /= hip_reduction - next_target = next_target / action_scale - cal_action[:, 0:12] = next_target - agent.step(torch.from_numpy(cal_action)) - agent.get_obs() - time.sleep(0.05) - - print("Starting pose calibrated [Press R2 to start controller]") - while True: - self.button_states = self.command_profile.get_buttons() - if self.command_profile.state_estimator.right_lower_right_switch_pressed: - self.command_profile.state_estimator.right_lower_right_switch_pressed = False - break - - for agent_name in self.agents.keys(): - obs = self.agents[agent_name].reset() - if agent_name == self.control_agent_name: - control_obs = obs - - return control_obs - - - def run(self, num_log_steps=1000000000, max_steps=100000000, logging=True): - assert self.control_agent_name is not None, "cannot deploy, runner has no control agent!" - assert self.policy is not None, "cannot deploy, runner has no policy!" - assert self.command_profile is not None, "cannot deploy, runner has no command profile!" - - # TODO: add basic test for comms - - for agent_name in self.agents.keys(): - obs = self.agents[agent_name].reset() - if agent_name == self.control_agent_name: - control_obs = obs - - control_obs = self.calibrate(wait=True) - - # now, run control loop - - try: - for i in range(max_steps): - - policy_info = {} - action = self.policy(control_obs, policy_info) - - for agent_name in self.agents.keys(): - obs, ret, done, info = self.agents[agent_name].step(action) - - info.update(policy_info) - info.update({"observation": obs, "reward": ret, "done": done, "timestep": i, - "time": i * self.agents[self.control_agent_name].dt, "action": action, "rpy": self.agents[self.control_agent_name].se.get_rpy(), "torques": self.agents[self.control_agent_name].torques}) - - if logging: self.logger.log(agent_name, info) - - if agent_name == self.control_agent_name: - control_obs, control_ret, control_done, control_info = obs, ret, done, info - - # bad orientation emergency stop - rpy = self.agents[self.control_agent_name].se.get_rpy() - if abs(rpy[0]) > 1.6 or abs(rpy[1]) > 1.6: - self.calibrate(wait=False, low=True) - - # check for logging command - prev_button_states = self.button_states[:] - self.button_states = self.command_profile.get_buttons() - - if self.command_profile.state_estimator.left_lower_left_switch_pressed: - if not self.is_currently_probing: - print("START LOGGING") - self.is_currently_probing = True - self.agents[self.control_agent_name].set_probing(True) - self.init_log_filename() - self.logger.reset() - else: - print("SAVE LOG") - self.is_currently_probing = False - self.agents[self.control_agent_name].set_probing(False) - # calibrate, log, and then resume control - control_obs = self.calibrate(wait=False) - self.logger.save(self.log_filename) - self.init_log_filename() - self.logger.reset() - time.sleep(1) - control_obs = self.agents[self.control_agent_name].reset() - self.command_profile.state_estimator.left_lower_left_switch_pressed = False - - for button in range(4): - if self.command_profile.currently_triggered[button]: - if not self.is_currently_logging[button]: - print("START LOGGING") - self.is_currently_logging[button] = True - self.init_log_filename() - self.logger.reset() - else: - if self.is_currently_logging[button]: - print("SAVE LOG") - self.is_currently_logging[button] = False - # calibrate, log, and then resume control - control_obs = self.calibrate(wait=False) - self.logger.save(self.log_filename) - self.init_log_filename() - self.logger.reset() - time.sleep(1) - control_obs = self.agents[self.control_agent_name].reset() - - if self.command_profile.state_estimator.right_lower_right_switch_pressed: - control_obs = self.calibrate(wait=False) - time.sleep(1) - self.command_profile.state_estimator.right_lower_right_switch_pressed = False - # self.button_states = self.command_profile.get_buttons() - while not self.command_profile.state_estimator.right_lower_right_switch_pressed: - time.sleep(0.01) - # self.button_states = self.command_profile.get_buttons() - self.command_profile.state_estimator.right_lower_right_switch_pressed = False - - # finally, return to the nominal pose - control_obs = self.calibrate(wait=False) - self.logger.save(self.log_filename) - - except KeyboardInterrupt: - self.logger.save(self.log_filename) diff --git a/go1_gym_deploy/utils/logger.py b/go1_gym_deploy/utils/logger.py deleted file mode 100755 index 66a2527..0000000 --- a/go1_gym_deploy/utils/logger.py +++ /dev/null @@ -1,79 +0,0 @@ -import copy -import pickle as pkl - -import numpy as np -import torch - - -def class_to_dict(obj) -> dict: - if not hasattr(obj, "__dict__"): - return obj - result = {} - for key in dir(obj): - if key.startswith("_") or key == "terrain": - continue - element = [] - val = getattr(obj, key) - if isinstance(val, list): - for item in val: - element.append(class_to_dict(item)) - else: - print(key) - element = class_to_dict(val) - result[key] = element - return result - - -class MultiLogger: - def __init__(self): - self.loggers = {} - - def add_robot(self, name, cfg): - print(name, cfg) - self.loggers[name] = EpisodeLogger(cfg) - - def log(self, name, info): - self.loggers[name].log(info) - - def save(self, filename): - with open(filename, 'wb') as file: - logdict = {} - for key in self.loggers.keys(): - logdict[key] = [class_to_dict(self.loggers[key].cfg), self.loggers[key].infos] - pkl.dump(logdict, file) - print(f"Saved log! Number of timesteps: {[len(self.loggers[key].infos) for key in self.loggers.keys()]}; Path: {filename}") - - def read_metric(self, metric, robot_name=None): - if robot_name is None: - robot_name = list(self.loggers.keys())[0] - logger = self.loggers[robot_name] - - metric_arr = [] - for info in logger.infos: - metric_arr += [info[metric]] - return np.array(metric_arr) - - def reset(self): - for key, log in self.loggers.items(): - log.reset() - - -class EpisodeLogger: - def __init__(self, cfg): - self.infos = [] - self.cfg = cfg - - def log(self, info): - for key in info.keys(): - if isinstance(info[key], torch.Tensor): - info[key] = info[key].detach().cpu().numpy() - - if isinstance(info[key], dict): - continue - elif "image" not in key: - info[key] = copy.deepcopy(info[key]) - - self.infos += [dict(info)] - - def reset(self): - self.infos = [] diff --git a/go1_gym_deploy/utils/network_config_unitree.py b/go1_gym_deploy/utils/network_config_unitree.py deleted file mode 100644 index 02f3070..0000000 --- a/go1_gym_deploy/utils/network_config_unitree.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 - -from os.path import expanduser -import netifaces -import sys -import subprocess - -def get_saved_interface_name(): - home = expanduser("~") - name = "" - try: - with open(home + "/.cheetah_network.txt"): - name = f.read().split()[0] - except: - name = "" - return name - -def get_likely_iface(): - ifs = netifaces.interfaces() - print("Found {} interfaces:".format(len(ifs))) - - if_to_addrs = {} - - for i in ifs: - if_to_addrs[i] = [] - if netifaces.AF_INET in netifaces.ifaddresses(i).keys(): - for ad in netifaces.ifaddresses(i)[netifaces.AF_INET]: - if_to_addrs[i].append(ad['addr']) - - for i in range(len(ifs)): - print(" [{}] : {} : {}".format(i, ifs[i], if_to_addrs[ifs[i]])) - - found_10_ip = 0 - selected_if = "" - - for i in ifs: - match_string = "192.168.123." - if len(if_to_addrs[i]) > 0 and if_to_addrs[i][0][:len(match_string)] == match_string: - found_10_ip = found_10_ip + 1 - selected_if = i - - if found_10_ip == 0: - print("None of the network adapters look correct. Make sure you have set a 10.0.0.x static ip!") - return "" - - elif found_10_ip == 1: - print("The adapter {} seems correct".format(selected_if)) - return selected_if - - else: - print("Found {} possible adapters, giving up".format(found_10_ip)) - return "" - -def main(): - name = get_saved_interface_name() - if not name: - print("Didn't find saved interface, searching...") - name = get_likely_iface() - if not name: - sys.exit("Failed to find network adapter name") - else: - print("Found saved interface {}".format(name)) - - print("Setup for interface {}".format(name)) - subprocess.call(['sudo', 'ifconfig', name, 'multicast']) - subprocess.call(['sudo', 'route', 'add', '-net', '224.0.0.0', 'netmask', '240.0.0.0', 'dev', name]) - -if __name__ == "__main__": - main()