diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/README.md b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/README.md new file mode 100644 index 0000000000000000000000000000000000000000..77209b7d8f0126252b49527eef0996d23eb36bc5 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/README.md @@ -0,0 +1,37 @@ +### Compile the robot SDK of Unitree A1 for gazebo robot deployment. +``` +# Unitree A1 Gazebo +cd ./legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk +# After entering the SDK directory, compile the code +mkdir build && cd build +cmake -DPYTHON_EXECUTABLE=/usr/bin/python3 .. +make +cp *.so ../../ +``` +### Start A1 Gazebo + +## Step 1: Set up the ROS environment +To set up the ROS development environment, execute the following command. +change the file to model-control and start. +``` +source ${your_workspace}/devel/setup.bash +``` +**Note that this command must be executed for each new terminal to set up the environment.** + +## Step 2: Start the Gazebo simulator and load a robot +Run the Gazebo simulator +``` +roslaunch qr_gazebo gazebo_startup.launch wname:=earth +``` +The `wname` (optional) parameter specifies the world to be loaded in Gazebo. The default value is `earth`. + +In a new terminal, spawn a robot model and manage controllers in the simulation environment. +Below is for loading a Unitree A1 robot. +``` +roslaunch qr_gazebo model_spawn.launch rname:=a1 use_xacro:=true use_camera:=false +``` +### How to use the gazebo lib +need back to the RL file +``` +python3 legged_gym/legged_gym/scripts/play.py --task wl_real --rl_device cuda:0 --sim_device cuda:0 --load_run pre-trained/ --checkpoint A1.pt +``` \ No newline at end of file diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_config.py b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_config.py new file mode 100644 index 0000000000000000000000000000000000000000..5b51ea20310e8dbb4e9c9170880db35d5b1704f3 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_config.py @@ -0,0 +1,198 @@ +# SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: BSD-3-Clause +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Copyright (c) 2021 ETH Zurich, Nikita Rudin +# Copyright (c) 2023, HUAWEI TECHNOLOGIES + +from legged_gym.envs.base.legged_robot_config import LeggedRobotCfg, LeggedRobotCfgPPO + + +class A1RoughCfg(LeggedRobotCfg): + + class init_state(LeggedRobotCfg.init_state): + pos = [0.0, 0.0, 0.36] # x,y,z [m] + default_joint_angles = { + # = target angles [rad] when action = 0.0 + 'FL_hip_joint': 0., + 'RL_hip_joint': 0., + 'FR_hip_joint': 0., + 'RR_hip_joint': 0., + + 'FL_thigh_joint': 0.9, + 'RL_thigh_joint': 0.9, + 'FR_thigh_joint': 0.9, + 'RR_thigh_joint': 0.9, + + 'FL_calf_joint': -1.8, + 'RL_calf_joint': -1.8, + 'FR_calf_joint': -1.8, + 'RR_calf_joint': -1.8, + } + + class env(LeggedRobotCfg.env): + num_envs = 4096 + num_privileged_obs = 54 # if not None a priviledge_obs_buf will be returned by step() (critic obs for assymetric training). None is returned otherwise + num_observation_history = 40 + episode_length_s = 20 # episode length in seconds + curriculum_factor = 0.8 + + class control(LeggedRobotCfg.control): + # PD Drive parameters: + control_type = 'P' + stiffness = {'joint': 20.} # [N*m/rad] + damping = {'joint': 0.5} # [N*m*s/rad] + # action scale: target angle = actionScale * action + defaultAngle + action_scale = 0.25 + # decimation: Number of control action updates @ sim DT per policy DT + decimation = 4 + use_torch_vel_estimator = False + use_actuator_network = False + + class asset(LeggedRobotCfg.asset): + file = '{LEGGED_GYM_ROOT_DIR}/resources/robots/a1/urdf/a1.urdf' # a1_unitree.urdf + name = "a1" + foot_name = "foot" + shoulder_name = "shoulder" + penalize_contacts_on = ["thigh", "calf"] + terminate_after_contacts_on = ["base", "shoulder"] + self_collisions = 1 # 1 to disable, 0 to enable...bitwise filter + restitution_mean = 0.5 + restitution_offset_range = [-0.1, 0.1] + compliance = 0.5 + + class rewards(LeggedRobotCfg.rewards): + soft_dof_pos_limit = 0.9 + base_height_target = 0.28 + pitch_roll_factor = [1, 1] + still_all = True + only_positive_rewards = False + + class scales(LeggedRobotCfg.rewards.scales): + lin_vel_z = -2.0 + ang_vel_xy = -0.5 + orientation = -5.0 + base_height = -1.0 + torques = -0.0002 + dof_vel = -0.0 + dof_acc = -1.25e-07 + action_rate = -0.0 + target_smoothness = -0.01 + collision = -1.0 + termination = -0.0 + dof_pos_limits = -10.0 + tracking_lin_vel = 2.0 + tracking_ang_vel = 1.0 + feet_air_time = 1.0 + stumble = -0.3 + stand_still = -0.5 + feet_height = -3.0 + delta_phi = -0.1 + residual_angle = -0.1 + feet_velocity = -0.0 + episode_length = 0.1 + + class normalization(LeggedRobotCfg.normalization): + + class obs_scales(LeggedRobotCfg.normalization.obs_scales): + height_measurements = 0.0 + + dof_history_interval = 1 + + class noise(LeggedRobotCfg.noise): + add_noise = True + heights_uniform_noise = False + heights_gaussian_mean_mutable = True + heights_downgrade_frequency = False # heights sample rate: 10 Hz + + class noise_scales(LeggedRobotCfg.noise.noise_scales): + height_measurements = 0.0 + + class commands(LeggedRobotCfg.commands): + curriculum = False + fixed_commands = None # None or [lin_vel_x, lin_vel_y, ang_vel_yaw] + gamepad_commands = True + resampling_time = 10. # time before command are changed[s] + + class ranges: + lin_vel_x = [-0.5, 0.5] # min max [m/s] + lin_vel_y = [-0.5, 0.5] # min max [m/s] + ang_vel_yaw = [-0.5, 0.5] # min max [rad/s] + heading = [-3.14, 3.14] + + class terrain(LeggedRobotCfg.terrain): + mesh_type = 'trimesh' # none, plane, heightfield or trimesh + dummy_normal = True + random_reset = True + curriculum = True + max_init_terrain_level = 2 + # terrain_length = 8. + # terrain_width = 8. + # num_rows = 20 # number of terrain rows (levels) + # num_cols = 20 # number of terrain cols (types) + # terrain types: [smooth slope, rough slope, stairs up, stairs down, discrete, stepping stones, wave] + terrain_proportions = [0.15, 0.15, 0.15, 0.0, 0.2, 0.2, 0.15] + # rough terrain only: + measure_heights = True + + class domain_rand(LeggedRobotCfg.domain_rand): + randomize_friction = True + friction_range = [0.5, 1.25] + randomize_base_mass = True + added_mass_range = [-3., 3.] + randomize_com_offset = True + com_offset_range = [[-0.05, 0.05], [-0.05, 0.05], [-0.05, 0.05]] + randomize_motor_strength = True + motor_strength_range = [0.8, 1.2] + randomize_Kp_factor = True + Kp_factor_range = [0.75, 1.25] + randomize_Kd_factor = True + Kd_factor_range = [0.5, 1.5] + + class pmtg(LeggedRobotCfg.pmtg): + gait_type = 'trot' + duty_factor = 0.6 + base_frequency = 1.5 + max_clearance = 0.15 + body_height = 0.28 + max_horizontal_offset = 0.05 + + +class A1RoughCfgPPO(LeggedRobotCfgPPO): + + class algorithm(LeggedRobotCfgPPO.algorithm): + entropy_coef = 0.01 + num_mini_batches = 4 # mini batch size = num_envs*nsteps / nminibatches + + class runner(LeggedRobotCfgPPO.runner): + run_name = '' + experiment_name = 'rough_a1' + max_iterations = 6000 # number of policy updates + resume = False + resume_path = 'legged_gym/logs/rough_a1' # updated from load_run and ckpt + load_run = '' # -1 = last run + checkpoint = -1 # -1 = last saved model diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_legged_robot.py b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_legged_robot.py new file mode 100644 index 0000000000000000000000000000000000000000..40c0ed87541790600838437d8f62adac174d5baa --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_legged_robot.py @@ -0,0 +1,694 @@ +# SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: BSD-3-Clause +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# +# Copyright (c) 2021 ETH Zurich, Nikita Rudin +# Copyright (c) 2023, HUAWEI TECHNOLOGIES + +import os +import sys +import time +import numpy as np +import torch + +from legged_gym.utils.isaacgym_utils import (to_torch, quat_mul, quat_apply, torch_rand_float, get_axis_params, + quat_rotate_inverse, get_euler_xyz) +from legged_gym.utils.torch_math import quat_apply_yaw, wrap_to_pi, torch_rand_sqrt_float +from legged_gym.utils.helpers import class_to_dict, parse_device_str +from .a1_config import A1RoughCfg +from legged_gym.envs.gazebo.a1_real.robot_utils import STAND_UP_HEIGHT +from legged_gym.envs.gamepad import gamepad_reader +from legged_gym.envs.gazebo.a1_real import a1_robot_real +from legged_gym.envs.gazebo.a1_real.a1_robot_dummy import A1Dummy +from legged_gym.envs.base.motor_config import MotorControlMode +from legged_gym.envs.estimator import robot_velocity_estimator +from legged_gym.envs.estimator.torch_robot_velocity_estimator import VelocityEstimator as torch_velocity_estimator +from legged_gym.envs.base.PMTrajectoryGenerator import PMTrajectoryGenerator + + +class A1LeggedRobot: + """A1leggedRobot environment class for the real-world A1 robot. + + """ + cfg: A1RoughCfg + + def __init__(self, cfg, sim_params, physics_engine, sim_device, headless, **kwargs): + """ Initialize the A1LeggedRobot environment. + + Parses the provided config file, + initilizes pytorch buffers used during training + + Args: + cfg (Dict): Environment config file + sim_params (gymapi.SimParams): simulation parameters + physics_engine (gymapi.SimType): gymapi.SIM_PHYSX (must be PhysX) + device_type (string): 'cuda' or 'cpu' + device_id (int): 0, 1, ... + headless (bool): Run without rendering if True + """ + self.cfg = cfg + self.sim_params = sim_params + self.height_samples = None + self.debug_viz = False + self.init_done = False + self._parse_cfg(self.cfg) + self.base_task_init(cfg, sim_params, physics_engine, sim_device, headless) + + self._init_buffers() + + self.hybrid_action = np.zeros(60, dtype=np.float32) + self.motor_control_mode = "P" + self.last_ctontrol_time = 0 + + self.gamepad = gamepad_reader.Gamepad() + self.command_function = self.gamepad.get_command + + # Construct real robot + self.robot = a1_robot_real.A1Robot(motor_control_mode=MotorControlMode.HYBRID, + enable_action_interpolation=False, + time_step=self.cfg.sim.dt, + action_repeat=1, + device=self.device) + + self.robot_dummy = None + if cfg.control.use_torch_vel_estimator: + self.robot_dummy = A1Dummy(motor_control_mode=MotorControlMode.HYBRID, + enable_action_interpolation=False, + time_step=self.cfg.sim.dt, + action_repeat=1, + device=self.device) + + self._clock = self.robot.GetTimeSinceReset + self._reset_time = self._clock() + self.start_time_wall = self.time_since_reset() + + self._state_estimator = {} + self._state_estimator['robot_state_estimator'] = robot_velocity_estimator.VelocityEstimator( + self.robot, moving_window_filter_size=20) + self._state_estimator['torch_robot_state_estimator'] = torch_velocity_estimator(robotIn=self.robot_dummy, + window_size=20, + device=self.device) + + # self._prepare_reward_function() + self.init_done = True + self.tik = 0.0 + + self.fixed_commands = self.cfg.commands.fixed_commands + self.curriculum_factor = self.cfg.env.curriculum_factor + self.height_noise_mean = 0. + self.pmtg = PMTrajectoryGenerator(robot=self.robot_dummy, + clock=self.robot.GetTimeSinceReset, + num_envs=self.num_envs, + device=self.device, + param=self.cfg.pmtg, + task_name="a1_real") + self.count = 0 + self.reset_flag = False + + def time_since_reset(self): + """Return the time elapsed since the last reset.""" + return self._clock() - self._reset_time + + def base_task_init(self, cfg, sim_params, physics_engine, sim_device, headless): + """Initialize the base task parameters. + + Args: + cfg (Dict): Environment config file + sim_params (gymapi.SimParams): simulation parameters + physics_engine (gymapi.SimType): gymapi.SIM_PHYSX (must be PhysX) + sim_device (string): 'cuda' or 'cpu' + headless (bool): Run without rendering if True + """ + self.physics_engine = physics_engine + self.sim_device = sim_device + sim_device_type, self.sim_device_id = parse_device_str(self.sim_device) + self.headless = headless + if sim_device_type == 'cuda': + self.device = self.sim_device + else: + self.device = 'cpu' + + # graphics device for rendering, -1 for no rendering + self.graphics_device_id = self.sim_device_id + if self.headless: + self.graphics_device_id = -1 + + self.num_envs = cfg.env.num_envs + self.num_obs = cfg.env.num_observations + self.num_privileged_obs = cfg.env.num_privileged_obs + self.num_actions = cfg.env.num_actions + self.num_policy_outputs = cfg.env.num_policy_outputs + + # optimization flags for pytorch JIT + torch._C._jit_set_profiling_mode(False) + torch._C._jit_set_profiling_executor(False) + + # allocate buffers + self.obs_buf = torch.zeros(self.num_envs, self.num_obs, device=self.device, dtype=torch.float) + self.rew_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.float) + self.reset_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + self.episode_length_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long) + self.time_out_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool) + if self.num_privileged_obs is not None: + self.privileged_obs_buf = torch.zeros(self.num_envs, + self.num_privileged_obs, + device=self.device, + dtype=torch.float) + else: + self.privileged_obs_buf = None + + self.extras = {} + + def _init_buffers(self): + """ Initialize torch tensors which will contain simulation states and processed quantities + """ + self.num_dof = 12 + self.up_axis_idx = 2 # 2 for z, 1 for y -> adapt gravity accordingly + # create some wrapper tensors for different slices + self.root_states = to_torch([0., 0., 0., 0., 0., 0., 1, 0., 0., 0., 0., 0., 0.], device=self.device).repeat( + (self.num_envs, 1)) + self.dof_pos = torch.zeros(self.num_envs, + self.num_dof, + dtype=torch.float32, + device=self.device, + requires_grad=False) + self.dof_vel = torch.zeros(self.num_envs, + self.num_dof, + dtype=torch.float32, + device=self.device, + requires_grad=False) + self.base_quat = self.root_states[:, 3:7] # xyzw + # initialize some data used later on + self.common_step_counter = 0 + self.extras = {} + self.gravity_vec = to_torch(get_axis_params(-1., self.up_axis_idx), device=self.device).repeat( + (self.num_envs, 1)) + self.torques = torch.zeros(self.num_envs, + self.num_actions, + dtype=torch.float, + device=self.device, + requires_grad=False) + self.p_gains = torch.zeros(self.num_actions, dtype=torch.float, device=self.device, requires_grad=False) + self.d_gains = torch.zeros(self.num_actions, dtype=torch.float, device=self.device, requires_grad=False) + self.actions = torch.zeros(self.num_envs, + self.num_actions, + dtype=torch.float, + device=self.device, + requires_grad=False) + self.last_actions = torch.zeros(self.num_envs, + self.num_actions, + dtype=torch.float, + device=self.device, + requires_grad=False) + self.last_last_actions = torch.zeros(self.num_envs, + self.num_actions, + dtype=torch.float, + device=self.device, + requires_grad=False) + self.last_dof_vel = torch.zeros_like(self.dof_vel) + self.last_last_dof_vel = torch.zeros_like(self.dof_vel) + self.commands = torch.zeros(self.num_envs, + self.cfg.commands.num_commands, + dtype=torch.float, + device=self.device, + requires_grad=False) # x vel, y vel, yaw vel, heading + self.commands_scale = torch.tensor( + [self.obs_scales.lin_vel, self.obs_scales.lin_vel, self.obs_scales.ang_vel], + device=self.device, + requires_grad=False, + ) + self.feet_names = ['FL_foot', 'FR_foot', 'RL_foot', 'RR_foot'] + self.feet_air_time = torch.zeros(self.num_envs, + len(self.feet_names), + dtype=torch.float, + device=self.device, + requires_grad=False) + self.last_contacts = torch.zeros(self.num_envs, + len(self.feet_names), + dtype=torch.bool, + device=self.device, + requires_grad=False) + self.base_lin_vel = quat_rotate_inverse(self.base_quat, self.root_states[:, 7:10]) + self.base_ang_vel = quat_rotate_inverse(self.base_quat, self.root_states[:, 10:13]) + self.projected_gravity = quat_rotate_inverse(self.base_quat, self.gravity_vec) + self.body_height = torch.tensor([[STAND_UP_HEIGHT]], dtype=torch.float, + device=self.device).repeat(self.num_envs, 1) + + self.measured_heights = 0 + self.last_last_last_pos = torch.zeros((self.num_envs, self.num_dof), + device=self.device, + dtype=torch.float, + requires_grad=False) + self.last_last_pos = torch.zeros((self.num_envs, self.num_dof), + device=self.device, + dtype=torch.float, + requires_grad=False) + self.last_pos = torch.zeros((self.num_envs, self.num_dof), + device=self.device, + dtype=torch.float, + requires_grad=False) + self.cpg_phase_information = torch.ones((self.num_envs, 13), + device=self.device, + dtype=torch.float, + requires_grad=False) + self.delta_phi = torch.zeros(self.num_envs, 4, dtype=torch.float, device=self.device, requires_grad=False) + self.residual_angle = torch.zeros(self.num_envs, 12, dtype=torch.float, device=self.device, requires_grad=False) + + # joint positions offsets and PD gains + self.dof_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 = torch.zeros(self.num_dof, dtype=torch.float, device=self.device, requires_grad=False) + for i in range(self.num_dof): + name = self.dof_names[i] + angle = self.cfg.init_state.default_joint_angles[name] + self.default_dof_pos[i] = angle + found = False + for dof_name in self.cfg.control.stiffness.keys(): + if dof_name in 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 {name} were not defined, setting them to zero") + self.default_dof_pos = self.default_dof_pos.unsqueeze(0) + + self.clip_actions = self.cfg.normalization.clip_actions + self.clip_angles = torch.tensor(self.cfg.normalization.clip_angles, dtype=torch.float, device=self.device) + self.torque_limits = 25 + self.feet_indices = torch.zeros(len(self.feet_names), dtype=torch.long, device=self.device, requires_grad=False) + for f in range(len(self.feet_names)): + self.feet_indices[f] = f + + def reset(self): + """Resets the robot environment. + + This method resets the environment to its initial state and returns the initial observations. + """ + self.reset_flag = True + self.reset_buf.zero_() + zero_action = torch.zeros((self.num_envs, self.num_actions + 4), device=self.device, dtype=torch.float) + self.step(zero_action) + self.reset_robot() + zero_action = torch.zeros((self.num_envs, self.num_actions + 4), device=self.device, dtype=torch.float) + self.step(zero_action) + self.pmtg.reset([0]) + self._reset_time = self._clock() + self.start_time_wall = self.time_since_reset() + self.reset_flag = False + return None, None + + def step(self, policy_outputs): + """Applies actions, simulates the environment, and performs post-physics steps. + + This method applies the given policy outputs as actions, simulates the environment, + and performs additional steps such as refreshing state tensors and clipping observations. + + Args: + policy_outputs (torch.Tensor): Tensor of shape (num_envs, num_actions_per_env) + + Returns: + Tuple: A tuple containing the observation buffer, privileged observation buffer, + reward buffer, reset buffer, and additional information. + """ + for _ in range(self.cfg.control.decimation): + self.delta_phi = policy_outputs[:, :4] + self.residual_angle = policy_outputs[:, 4:] + delta_phi = policy_outputs[:, :4] * self.cfg.control.delta_phi_scale + residual_angle = policy_outputs[:, 4:] * self.cfg.control.residual_angle_scale + residual_xyz = torch.zeros(self.num_envs, self.num_actions).to(self.device) + pmtg_joints = self.pmtg.get_action(delta_phi, + residual_xyz, + residual_angle, + self.base_quat, + command=self.commands) + clip_actions = self.cfg.normalization.clip_actions + self.actions = torch.clip(pmtg_joints, -clip_actions, clip_actions).to(self.device) + self.hybrid_action = self._compute_command(self.actions) + if not self.reset_flag: + self.robot.ApplyAction(self.hybrid_action) + + while self.time_since_reset() - self.start_time_wall < self.cfg.sim.dt: + pass + self.start_time_wall = self.time_since_reset() + + self.robot.ReceiveObservation() # refresh raw state + + self.refresh_dof_state_tensor() + if self.robot_dummy is None: + self.refresh_actor_root_state_tensor() + else: + self.torch_refresh_actor_root_state_tensor() + + self.count += 1 + self.post_physics_step() + clip_obs = self.cfg.normalization.clip_observations + self.obs_buf = torch.clip(self.obs_buf, -clip_obs, clip_obs) + if self.privileged_obs_buf is not None: + self.privileged_obs_buf = torch.clip(self.privileged_obs_buf, -clip_obs, clip_obs) + return self.obs_buf, self.privileged_obs_buf, self.rew_buf, self.reset_buf, self.extras + + def refresh_dof_state_tensor(self): + dof_pos = self._serialize_dof(self.robot.GetTrueMotorAngles()) + dof_vel = self._serialize_dof(self.robot.GetMotorVelocities()) + self.dof_pos[:] = torch.from_numpy(dof_pos).to(self.device).repeat((self.num_envs, 1)) + self.dof_vel[:] = torch.from_numpy(dof_vel).to(self.device).repeat((self.num_envs, 1)) + + def refresh_actor_root_state_tensor(self): + self._state_estimator['robot_state_estimator'].update(robot_state_tick=self.robot.tick, + current_time=self.time_since_reset()) + self.body_height = self._state_estimator['robot_state_estimator'].estimate_robot_height() + + base_orientation = np.array([self.robot.GetBaseOrientation()], dtype=np.float32) + self.base_quat[:] = torch.from_numpy(base_orientation).to(self.device).repeat((self.num_envs, 1)) + + base_lin_vel_world = np.array([self._state_estimator['robot_state_estimator'].com_velocity_world_frame], + dtype=np.float32) # world frame + self.base_lin_vel[:] = quat_rotate_inverse( + self.base_quat, + torch.from_numpy(base_lin_vel_world).to(self.device).repeat((self.num_envs, 1))) + base_ang_vel = np.array([self.robot.GetBaseRollPitchYawRate()], dtype=np.float32) # base frame + self.base_ang_vel[:] = torch.from_numpy(base_ang_vel).to(self.device).repeat((self.num_envs, 1)) + + def torch_refresh_actor_root_state_tensor(self): + torch_se = self._state_estimator['torch_robot_state_estimator'] + self.base_quat[:] = torch.from_numpy(self.robot._base_orientation).to(self.device, + dtype=torch.float).unsqueeze(0) + robot_obs_buf = self.robot.GetRobotObs().to(device=self.device) + self.base_ang_vel[:] = robot_obs_buf[0, 6:9] + self.robot_dummy.set_state(robot_obs_buf) + v, h = torch_se(self.robot.raw_state.tick / 1000.0, robot_obs_buf) + self.base_lin_vel[:] = v + self.body_height[:] = h + + def post_physics_step(self): + """ check terminations, compute observations and rewards + calls self._post_physics_step_callback() for common computations + calls self._draw_debug_vis() if needed + """ + self.episode_length_buf += 1 + self.common_step_counter += 1 + self.projected_gravity[:] = quat_rotate_inverse(self.base_quat, self.gravity_vec) + self.cpg_phase_information = self.pmtg.update_observation() # TODO + + self._post_physics_step_callback() + self.foot_forces = torch.tensor(self.robot.GetFootForce()).to(self.device) + self.compute_observations() + + self.last_last_actions[:] = self.last_actions[:] + self.last_actions[:] = self.actions[:] + self.last_last_dof_vel[:] = self.last_dof_vel[:] + self.last_dof_vel[:] = self.dof_vel[:] + self.last_last_last_pos[:] = self.last_last_pos[:] + self.last_last_pos[:] = self.last_pos[:] + self.last_pos[:] = self.dof_pos[:] + + def check_termination(self): + """ Check if environments need to be reset + """ + self.robot.ReceiveObservation() + self.reset_buf = torch.tensor([False]).to(self.device) + roll, pitch, yaw = self.robot.GetBaseRollPitchYaw() + feet_not_contact = (~self.robot.GetFootContacts()).all() + self.reset_buf |= torch.tensor(feet_not_contact).to(self.device) + self.reset_buf |= torch.tensor([self.body_height < 0.15]).to(self.device) + self.time_out_buf = self.episode_length_buf > self.max_episode_length # no terminal reward for time-outs + self.reset_buf |= self.time_out_buf + if self.reset_buf: + print('feet_not_contact:', feet_not_contact) + print('body_height:', self.body_height) + print('time_out_buf:', self.time_out_buf) + + def reset_robot(self): + """ Reset a1 robot and real world environment. + Resets some buffers + """ + self.robot.Reset() + # reset buffers + self.last_actions[0] = 0. + self.last_dof_vel[0] = 0. + self.feet_air_time[0] = 0. + self.episode_length_buf[0] = 0 + self.reset_buf[0] = 0 + # fill extras + self.extras["episode"] = {} + if self.cfg.terrain.curriculum: + self.extras["episode"]["terrain_level"] = torch.mean(self.terrain_levels.float()) + if self.cfg.commands.curriculum: + self.extras["episode"]["max_command_x"] = self.command_ranges["lin_vel_x"][1] + # send timeout info to the algorithm + if self.cfg.env.send_timeouts: + self.extras["time_outs"] = self.time_out_buf + + def _post_physics_step_callback(self): + """ Callback called before computing terminations, rewards, and observations + Default behaviour: Compute ang vel command based on target and heading, compute measured terrain heights and randomly push robots + """ + env_ids = (self.episode_length_buf % + int(self.cfg.commands.resampling_time / self.dt) == 0).nonzero(as_tuple=False).flatten() + self._resample_commands(env_ids) + + if self.cfg.commands.heading_command: + forward = quat_apply(self.base_quat, self.forward_vec) + heading = torch.atan2(forward[:, 1], forward[:, 0]) + self.commands[:, 2] = torch.clip(0.5 * wrap_to_pi(self.commands[:, 3] - heading), -1., 1.) + if self.cfg.commands.gamepad_commands: + lin_speed, ang_speed, e_stop = self.command_function() + if e_stop: + sys.exit(0) + self.commands[:, 0] = lin_speed[0] + self.commands[:, 1] = lin_speed[1] + self.commands[:, 2] = ang_speed + + if self.cfg.terrain.measure_heights: + self.measured_heights = self._get_heights() + + def _resample_commands(self, env_ids): + """ Randommly select commands of some environments + + Args: + env_ids (List[int]): Environments ids for which new commands are needed + """ + if self.fixed_commands is not None: + self.commands[env_ids, + 0] = torch.tensor([self.fixed_commands[0]]).repeat(len(env_ids)).to(device=self.device) + self.commands[env_ids, + 1] = torch.tensor([self.fixed_commands[1]]).repeat(len(env_ids)).to(device=self.device) + self.commands[env_ids, + 2] = torch.tensor([self.fixed_commands[2]]).repeat(len(env_ids)).to(device=self.device) + else: + self.commands[env_ids, 0] = torch_rand_float(self.command_ranges["lin_vel_x"][0], + self.command_ranges["lin_vel_x"][1], (len(env_ids), 1), + device=self.device).squeeze(1) + self.commands[env_ids, 1] = torch_rand_float(self.command_ranges["lin_vel_y"][0], + self.command_ranges["lin_vel_y"][1], (len(env_ids), 1), + device=self.device).squeeze(1) + if self.cfg.commands.heading_command: + self.commands[env_ids, 3] = torch_rand_float(self.command_ranges["heading"][0], + self.command_ranges["heading"][1], (len(env_ids), 1), + device=self.device).squeeze(1) + else: + self.commands[env_ids, 2] = torch_rand_float(self.command_ranges["ang_vel_yaw"][0], + self.command_ranges["ang_vel_yaw"][1], (len(env_ids), 1), + device=self.device).squeeze(1) + + # set small commands to zero + self.commands[env_ids, :2] *= (torch.norm(self.commands[env_ids, :2], dim=1) > 0.2).unsqueeze(1) + + def _get_heights(self, env_ids=None): + self.num_height_points = 187 + self.measured_heights = torch.zeros(self.num_envs, self.num_height_points, device=self.device) + return self.measured_heights + + def _prepare_reward_function(self): + """ Prepares a list of reward functions, whcih will be called to compute the total reward. + Looks for self._reward_, where are names of all non zero reward scales in the cfg. + """ + # remove zero scales + multiply non-zero ones by dt + for key in list(self.reward_scales.keys()): + scale = self.reward_scales[key] + if scale == 0: + self.reward_scales.pop(key) + else: + self.reward_scales[key] *= self.dt + # prepare list of functions + self.reward_functions = [] + self.reward_names = [] + for name, scale in self.reward_scales.items(): + if name == "termination": + continue + self.reward_names.append(name) + name = '_reward_' + name + self.reward_functions.append(getattr(self, name)) + + # reward episode sums + self.episode_sums = { + name: torch.zeros(self.num_envs, dtype=torch.float, device=self.device, requires_grad=False) + for name in self.reward_scales.keys() + } + + def create_sim(self): + self.up_axis_idx = 2 # 2 for z, 1 for y -> adapt gravity accordingly + self.sim = self.gym.create_sim(self.sim_device_id, self.graphics_device_id, self.physics_engine, + self.sim_params) + + # load robot asset from URDF file + asset_path = self.cfg.asset.file.format(LEGGED_GYM_ROOT_DIR=LEGGED_GYM_ROOT_DIR) + asset_root = os.path.dirname(asset_path) + asset_file = os.path.basename(asset_path) + + asset_options = gymapi.AssetOptions() + asset_options.default_dof_drive_mode = self.cfg.asset.default_dof_drive_mode + asset_options.collapse_fixed_joints = self.cfg.asset.collapse_fixed_joints + asset_options.replace_cylinder_with_capsule = self.cfg.asset.replace_cylinder_with_capsule + asset_options.flip_visual_attachments = self.cfg.asset.flip_visual_attachments + asset_options.fix_base_link = self.cfg.asset.fix_base_link + asset_options.density = self.cfg.asset.density + asset_options.angular_damping = self.cfg.asset.angular_damping + asset_options.linear_damping = self.cfg.asset.linear_damping + asset_options.max_angular_velocity = self.cfg.asset.max_angular_velocity + asset_options.max_linear_velocity = self.cfg.asset.max_linear_velocity + asset_options.armature = self.cfg.asset.armature + asset_options.thickness = self.cfg.asset.thickness + asset_options.disable_gravity = self.cfg.asset.disable_gravity + + robot_asset = self.gym.load_asset(self.sim, asset_root, asset_file, asset_options) + dof_props_asset = self.gym.get_asset_dof_properties(robot_asset) + _ = self._process_dof_props(dof_props_asset, 0) + + self.penalize_joint_ids = [] + for i in range(len(self.dof_names)): + if self.dof_names[i] in ['FL_hip_joint', 'FR_hip_joint', 'RL_hip_joint', 'RR_hip_joint']: + self.penalize_joint_ids.append(i) + + def compute_observations(self): + body_orientation = get_euler_xyz(self.base_quat) + dof_pos_history = torch.cat((self.last_last_last_pos, self.last_last_pos, self.last_pos), dim=-1) + dof_pos_vel_history = torch.cat((self.last_last_dof_vel, self.last_dof_vel), dim=-1) + dof_pos_target_history = torch.cat((self.last_last_actions, self.last_actions), dim=-1) + cpg_phase_information = self.cpg_phase_information + + self.obs_buf = torch.cat( + (self.commands[:, :3] * self.commands_scale, body_orientation * self.obs_scales.orientation, + self.base_lin_vel * self.obs_scales.lin_vel, self.base_ang_vel * self.obs_scales.ang_vel, + self.dof_pos * self.obs_scales.dof_pos, self.dof_vel * self.obs_scales.dof_vel, + dof_pos_history * self.obs_scales.dof_pos, dof_pos_vel_history * self.obs_scales.dof_vel, + dof_pos_target_history * self.obs_scales.dof_pos, cpg_phase_information * self.obs_scales.cpg_phase), + dim=-1).to(self.device) + + # add perceptive inputs if not blind + if self.cfg.terrain.measure_heights: + heights = torch.clip(self.body_height - self.cfg.rewards.base_height_target - self.measured_heights, -1, + 1.) * self.obs_scales.height_measurements + self.obs_buf = torch.cat((self.obs_buf, heights), dim=-1) + + def get_observations(self): + return self.obs_buf + + def get_privileged_observations(self): + return self.privileged_obs_buf + + def _compute_command(self, actions): + # PD controller + control_type = self.cfg.control.control_type + self.torques = self.p_gains * (self.default_dof_pos - self.dof_pos) - self.d_gains * self.dof_vel + if control_type == "T": + torques = self.p_gains * (actions - self.dof_pos) - self.d_gains * self.dof_vel + torques = np.clip(torques.squeeze(0).detach().numpy(), -self.torque_limits, self.torque_limits) + torques = self._serialize_dof(torques) + + for index in range(len(torques)): + self.hybrid_action[5 * index + 4] = torques[index] + elif control_type == "P": + positions = actions.detach().cpu().numpy() + positions = self._serialize_dof(positions[0]) # only the first robot + for index in range(len(positions)): + self.hybrid_action[5 * index + 0] = positions[index] # position + self.hybrid_action[5 * index + 1] = self.cfg.control.stiffness['joint'] # Kp + self.hybrid_action[5 * index + 2] = 0 # velocity + self.hybrid_action[5 * index + 3] = self.cfg.control.damping['joint'] # Kd + self.hybrid_action[5 * index + 4] = 0 # torque + + return self.hybrid_action + + def _serialize_dof(self, dof_data): + serialized_data = np.zeros(12, dtype=np.float32) + serialized_data[0:3] = dof_data[3:6] + serialized_data[3:6] = dof_data[0:3] + serialized_data[6:9] = dof_data[9:12] + serialized_data[9:12] = dof_data[6:9] + return serialized_data + + def _parse_cfg(self, cfg): + self.dt = self.cfg.control.decimation * self.sim_params['sim']['dt'] + self.obs_scales = self.cfg.normalization.obs_scales + self.priv_obs_scales = self.cfg.normalization.priv_obs_scales + self.reward_scales = class_to_dict(self.cfg.rewards.scales) + self.command_ranges = class_to_dict(self.cfg.commands.ranges) + if self.cfg.terrain.mesh_type not in ['heightfield', 'trimesh']: + self.cfg.terrain.curriculum = False + self.max_episode_length_s = self.cfg.env.episode_length_s + self.max_episode_length = np.ceil(self.max_episode_length_s / self.dt) + + self.cfg.domain_rand.push_interval = np.ceil(self.cfg.domain_rand.push_interval_s / self.dt) + + # ------------ reward functions---------------- + def _reward_collision(self): + # Penalize collisions on selected bodies + return 0. + + def _reward_feet_air_time(self): + # Reward long steps + # Need to filter the contacts because the contact reporting of PhysX is unreliable on meshes + contact = torch.tensor(self.robot.GetFootContacts()).to(self.device) + contact_filt = torch.logical_or(contact, self.last_contacts) + self.last_contacts = contact + first_contact = (self.feet_air_time > 0.) * contact_filt + self.feet_air_time += self.dt + rew_airTime = torch.sum((self.feet_air_time - 0.5) * first_contact, + dim=1) # reward only on first contact with the ground + rew_airTime *= torch.norm(self.commands[:, :2], dim=1) > 0.1 # no reward for zero command + self.feet_air_time *= ~contact_filt + return rew_airTime + + def _reward_stumble(self): + # Penalize feet hitting vertical surfaces + return 0. + + def _reward_feet_contact_forces(self): + # penalize high contact forces + return torch.sum((self.foot_forces - self.cfg.rewards.max_contact_force).clip(min=0.), dim=1) + + def _reward_feet_height(self): + # Penalize feet height error + feet_height_error = self.pmtg.foot_target_position_in_base_frame[:, :, 2] - torch.tensor( + self.robot.GetFootPositionsInBaseFrame()).unsqueeze(0).to(self.device)[:, :, 2] + return torch.sum(torch.abs(feet_height_error), dim=1) diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/a1_robot_dummy.py b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/a1_robot_dummy.py new file mode 100644 index 0000000000000000000000000000000000000000..ae8e5db1df9a7a35fdd5811df404e890f7c12a71 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/a1_robot_dummy.py @@ -0,0 +1,189 @@ +# SPDX-FileCopyrightText: Copyright (c) 2023, HUAWEI TECHNOLOGIES. All rights reserved. +# SPDX-License-Identifier: BSD-3-Clause +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import torch +from legged_gym.utils.math import * +from .robot_utils import * + + +class A1Dummy(object): + + def __init__(self, time_step=0.002, **kwargs): + """Initializes the robot class.""" + print("Dummy Robot MPC_BODY_MASS: ", MPC_BODY_MASS) + print("Dummy Robot MPC_BODY_INERTIA: ", MPC_BODY_INERTIA) + print("Dummy Robot MPC_BODY_HEIGHT: ", MPC_BODY_HEIGHT) + + self.device = kwargs.get('device', torch.device('cpu')) + print("A1Dummy use ", self.device) + self.time_step = time_step + self.num_envs = kwargs.get('num_envs', 1) + # Robot state variables + self._base_orientation = None + self.base_rpy = torch.zeros((self.num_envs, 3), + dtype=torch.float, + device=self.device) + self._motor_angles = torch.zeros(12) + self._motor_velocities = torch.zeros(12) + self._joint_states = None + self.foot_contact = None + self.estimated_velocity = torch.zeros((self.num_envs, 3), + dtype=torch.float, + device=self.device) + self.base_rpyrate = torch.zeros((self.num_envs, 3), + dtype=torch.float, + device=self.device) + self.base_lin_acc = None + self.base_quat = None + self._init_complete = True + self.foot_positions_in_base_frame = torch.zeros((self.num_envs, 4, 3), + dtype=torch.float, + device=self.device) + self.foot_positions_in_base_frame[:, :, 2] -= 0.28 + self.footVelocitiesInBaseFrame = torch.zeros_like( + self.foot_positions_in_base_frame) + self.torch_hip_offset = torch.from_numpy(HIP_OFFSETS).to( + dtype=torch.float, device=self.device).unsqueeze(0) # (1,4,3) + self.l_hip = torch.tensor([[0.08505, -0.08505, 0.08505, -0.08505]], + dtype=torch.float, + device=self.device) # (1,4) + self.torch_hip_offset[:, 0:2, :] = self.torch_hip_offset[:, [1, 0], :] + self.torch_hip_offset[:, 2:4, :] = self.torch_hip_offset[:, [3, 2], :] + + def GetBaseRollPitchYaw(self): + return getEulerFromQuaternion(self._base_orientation) + + def GetTrueBaseRollPitchYaw(self): + return getEulerFromQuaternion(self._base_orientation) + + def GetBaseRollPitchYawRate(self): + return self.GetTrueBaseRollPitchYawRate() + + def GetFootPositionsInBaseFrame(self): + """Get the robot's foot position in the base frame.""" + return self.foot_positions_in_base_frame + + def ComputeJacobian(self, leg_id): + """Compute the Jacobian for a given leg.""" + motor_angles = self._motor_angles[:, leg_id * 3:(leg_id + 1) * 3] + return analytical_leg_jacobian(motor_angles.view(-1), leg_id) + + def GetBaseAcc(self): + return self.base_lin_acc + + def GetBaseOrientation(self): + return self._base_orientation + + def GetBaseRPY(self): + return self.base_rpy + + def GetMotorVelocities(self): + return self._motor_velocities + + def GetFootContacts(self): + return self.foot_contact + + def GetFootVelocitiesInBaseFrame(self): + return self.footVelocitiesInBaseFrame + + def set_state(self, original_obs_buf): + self.base_lin_acc = original_obs_buf[:, :3] + self.base_rpy = original_obs_buf[:, 3:6] + self.base_rpyrate = original_obs_buf[:, 6:9] + self._motor_angles = original_obs_buf[:, 9:21] + self._motor_velocities = original_obs_buf[:, 21:33] + self.foot_contact = original_obs_buf[:, 33:37] + jacobians = self.torch_analytical_leg_jacobian(self._motor_angles) + self.footVelocitiesInBaseFrame[:] = torch.matmul( + jacobians, self._motor_velocities.view(-1, 4, 3, 1)).squeeze(-1) + + def torch_analytical_leg_jacobian(self, leg_angles): + # J.shape = (None, 4, 3, 3) + # Q.shape = (None, 4, 9, 3) + # x.shape = (None, 4, 3, 1) + # Q*x = -- > (None,4 9,1)-->(None, 4 3, 3) + N = leg_angles.size(0) + Q = torch.zeros((N, 4, 9, 3), + dtype=torch.float, + device=leg_angles.device) + x = torch.zeros((N, 4, 3, 1), + dtype=torch.float, + device=leg_angles.device) + motor_angles = self._motor_angles.view(N, 4, 3) + t1 = torch.select(motor_angles, -1, 0) # (N, 4) + t2 = torch.select(motor_angles, -1, 1) # (N, 4) + t3 = torch.select(motor_angles, -1, 2) # (N, 4) + t_eff = t2 + t3 / 2 + l_eff = torch.sqrt(0.08 + 0.08 * torch.cos(t3)) # (N, 4) + x[:, :, 0, 0] = self.l_hip + x[:, :, 1, 0] = l_eff + x[:, :, 2, 0] = 0.04 / l_eff + + c_t_eff = torch.cos(t_eff) + s_t_eff = torch.sin(t_eff) + s_t1 = torch.sin(t1) + c_t1 = torch.cos(t1) + s_t3 = torch.sin(t3) + c_t3 = torch.cos(t3) + + Q[:, :, 1, 1] = -c_t_eff + Q[:, :, 2, 1] = c_t_eff * (-0.5) + Q[:, :, 2, 2] = s_t_eff * s_t3 + + Q[:, :, 3, 0] = -s_t1 + Q[:, :, 3, 1] = c_t_eff * c_t1 + Q[:, :, 4, 1] = -s_t1 * s_t_eff + Q[:, :, 5, 1] = s_t_eff * s_t1 * (-0.5) + Q[:, :, 5, 2] = -c_t_eff * s_t3 * s_t1 + + Q[:, :, 6, 0] = c_t1 + Q[:, :, 6, 1] = s_t1 * c_t_eff + Q[:, :, 7, 1] = s_t_eff * c_t1 + Q[:, :, 8, 1] = s_t_eff * c_t1 * 0.5 + Q[:, :, 8, 2] = c_t_eff * s_t3 * c_t1 + + J = torch.matmul(Q, x) + J = J.view(N, 4, 3, 3) + + L = torch.sqrt(c_t3 * 0.08 + 0.08) + H = torch.zeros((N, 4, 2, 1), + dtype=torch.float, + device=leg_angles.device) + S = torch.zeros((N, 4, 3, 2), + dtype=torch.float, + device=leg_angles.device) + S[:, :, 0, 1] = -s_t_eff + S[:, :, 1, 0] = c_t1 + S[:, :, 1, 1] = s_t1 * c_t_eff + S[:, :, 2, 0] = s_t1 + S[:, :, 2, 1] = -c_t1 * c_t_eff + H[:, :, 0, 0] = self.l_hip + H[:, :, 1, 0] = L + self.foot_positions_in_base_frame = torch.matmul( + S, H).squeeze(-1) + self.torch_hip_offset # (N,4,3) + return J \ No newline at end of file diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/a1_robot_real.py b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/a1_robot_real.py new file mode 100644 index 0000000000000000000000000000000000000000..c9b2eb00adb876fd0af69f78dfa34b475627ec07 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/a1_robot_real.py @@ -0,0 +1,429 @@ +# SPDX-FileCopyrightText: Copyright (c) 2023, HUAWEI TECHNOLOGIES. All rights reserved. +# SPDX-License-Identifier: BSD-3-Clause +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import os +import inspect + +currentdir = os.path.dirname( + os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0, parentdir) +import time +from absl import logging +import torch +from legged_gym.envs.base.motor_config import MotorControlMode +from legged_gym.envs.gazebo.a1_real.robot_interface import RobotInterface +from legged_gym.utils.math import * +from .robot_utils import * + + +class A1Robot: + """Interface for real A1 robot.""" + + def __init__(self, time_step=0.002, **kwargs): + """Initializes the robot class.""" + print("Real Robot MPC_BODY_MASS: ", MPC_BODY_MASS) + print("Real Robot MPC_BODY_INERTIA: ", MPC_BODY_INERTIA) + print("Real Robot MPC_BODY_HEIGHT: ", MPC_BODY_HEIGHT) + self.device = kwargs.get('device', torch.device("cpu")) + # Initialize pd gain vector + self.motor_kps = np.array([ABDUCTION_P_GAIN, HIP_P_GAIN, KNEE_P_GAIN] * + 4) + self.motor_kds = np.array([ABDUCTION_D_GAIN, HIP_D_GAIN, KNEE_D_GAIN] * + 4) + self._motor_control_mode = kwargs['motor_control_mode'] + self.stand_up_height = STAND_UP_HEIGHT + self.time_step = time_step + + # Robot state variables + self._init_complete = False + self._base_orientation = None + self._base_rpy = None + self.raw_state = None + self._last_raw_state = None + self._motor_angles = np.zeros(12, dtype=np.float32) + self._motor_velocities = np.zeros(12, dtype=np.float32) + self._joint_states = None + self.drpy = np.zeros(3, dtype=np.float32) + self.acc = np.zeros(3, dtype=np.float32) + self.footForce = np.array([50, 50, 50, 50], dtype=np.float32) + + # Initiate UDP for robot state and actions + self._robot_interface = RobotInterface() + self._robot_interface.send_command(np.zeros(60, dtype=np.float32)) + + self._state_action_counter = 0 + self.estimated_velocity = np.zeros(3) + self._last_reset_time = time.time() + self._init_complete = True + self.clip_angles = np.array( + [[-0.73, 0.73], [-0.85, 2.8], [-2.6, -1.0]], dtype=np.float32) + self.unsafty = False + self.safty_action = np.zeros((12), dtype=np.float32) + self.tick = 0 + + def ReceiveObservation(self): + """Receives observation from robot. + """ + state = self._robot_interface.receive_observation() + self.raw_state = state + + self.tick = state.tick + for i in range(3): + self.drpy[i] = state.imu.gyroscope[i] + self.acc[i] = state.imu.accelerometer[i] + for i in range(4): + self.footForce[i] = state.footForce[i] + + q = state.imu.quaternion + self._base_rpy = np.array([state.imu.rpy], dtype=np.float32) + # Convert quaternion from wxyz to xyzw + self._base_orientation = np.array([q[1], q[2], q[3], q[0]]) + self._motor_angles = np.array( + [motor.q for motor in state.motorState[:12]], dtype=np.float32) + self._motor_velocities = np.array( + [motor.dq for motor in state.motorState[:12]], dtype=np.float32) + self._joint_states = np.array( + list(zip(self._motor_angles, self._motor_velocities))) + + def GetTrueMotorAngles(self): + return self._motor_angles.copy() + + def GetMotorAngles(self): + return self._motor_angles.copy() + + def GetMotorVelocities(self): + return self._motor_velocities.copy() + + def GetBaseRollPitchYaw(self): + return getEulerFromQuaternion(self._base_orientation) + + def GetTrueBaseRollPitchYaw(self): + return getEulerFromQuaternion(self._base_orientation) + + def GetBaseRollPitchYawRate(self): + return self.GetTrueBaseRollPitchYawRate() + + def GetTrueBaseRollPitchYawRate(self): + return self.drpy.copy() + + def GetBaseVelocity(self): + return self.estimated_velocity.copy() + + def GetFootContacts(self): + return self.footForce > 20 + + def GetFootForce(self): + return self.footForce.copy() + + def GetTimeSinceReset(self): + return time.time() - self._last_reset_time + + def GetBaseOrientation(self): + return self._base_orientation.copy() + + def GetTrueBaseOrientation(self): + return self._base_orientation.copy() + + @property + def motor_velocities(self): + return self._motor_velocities.copy() + + def ApplyAction(self, motor_commands, motor_control_mode=None): + """Clips and then apply the motor commands using the motor model. + + Args: + motor_commands: np.array. Can be motor angles, torques, hybrid commands, + or motor pwms (for Minitaur only). + motor_control_mode: A MotorControlMode enum. + """ + if motor_control_mode is None: + motor_control_mode = self._motor_control_mode + + command = np.zeros(60, dtype=np.float32) + if motor_control_mode == MotorControlMode.POSITION: + motor_commands = np.clip(motor_commands.reshape(4, 3), + self.clip_angles[:, 0], + self.clip_angles[:, 1]).reshape(12) + for motor_id in range(NUM_MOTORS): + command[motor_id * 5] = motor_commands[motor_id] + command[motor_id * 5 + 1] = self.motor_kps[motor_id] + command[motor_id * 5 + 3] = self.motor_kds[motor_id] + elif motor_control_mode == MotorControlMode.TORQUE: + for motor_id in range(NUM_MOTORS): + motor_commands = np.clip(motor_commands, -25, 25) + command[motor_id * 5 + 4] = motor_commands[motor_id] + elif motor_control_mode == MotorControlMode.HYBRID: + command = np.array(motor_commands, dtype=np.float32) + else: + raise ValueError( + 'Unknown motor control mode for A1 robot: {}.'.format( + motor_control_mode)) + + self._robot_interface.send_command(command) + + def Reset(self, default_motor_angles=None, reset_time=3.0): + """Reset the robot to default motor angles.""" + self._reset() + self._state_action_counter = 0 + + def Step(self, action, motor_control_mode=None): + if not self.unsafty: + self.ApplyAction(action, motor_control_mode) + self.ReceiveObservation() + self._state_action_counter += 1 + + def check_safty(self): + reshaped_motor_angles = self._motor_angles.reshape(4, 3) + for leg_id in range(4): + if reshaped_motor_angles[leg_id, 1] < self.clip_angles[1, 0]: + self.safty_action[leg_id * 3 + 1] = 1.0 / ( + (self.clip_angles[1, 0] - reshaped_motor_angles[leg_id, 1]) + + 0.1) + self.unsafty = True + elif reshaped_motor_angles[leg_id, 1] > self.clip_angles[1, 1]: + self.safty_action[leg_id * 3 + 1] = 1.0 / ( + (self.clip_angles[1, 1] - reshaped_motor_angles[leg_id, 1]) + + 0.1) + self.unsafty = True + + return self.unsafty + + def ComputeMotorAnglesFromFootLocalPosition(self, leg_id, + foot_local_position): + """Use IK to compute the motor angles, given the foot link's local position. + + Args: + leg_id: The leg index. + foot_local_position: The foot link's position in the base frame. + + Returns: + A tuple. The position indices and the angles for all joints along the + leg. The position indices is consistent with the joint orders as returned + by GetMotorAngles API. + """ + # assert len(self._foot_link_ids) == self.num_legs + motors_per_leg = self.num_motors // self.num_legs + joint_position_idxs = list( + range(leg_id * motors_per_leg, + leg_id * motors_per_leg + motors_per_leg)) + + joint_angles = foot_position_in_hip_frame_to_joint_angle( + foot_local_position - HIP_OFFSETS[leg_id], + l_hip_sign=(-1)**(leg_id + 1)) + + # Joint offset is necessary for Laikago. + joint_angles = np.multiply( + np.asarray(joint_angles) - + np.asarray(self._motor_offset)[joint_position_idxs], + self._motor_direction[joint_position_idxs]) + + # Return the joing index (the same as when calling GetMotorAngles) as well + # as the angles. + return joint_position_idxs, joint_angles.tolist() + + def GetFootPositionsInBaseFrame(self): + """Get the robot's foot position in the base frame.""" + motor_angles = self.GetMotorAngles() + return foot_positions_in_base_frame(motor_angles) + + def ComputeJacobian(self, leg_id): + """Compute the Jacobian for a given leg.""" + motor_angles = self.GetMotorAngles()[leg_id * 3:(leg_id + 1) * 3] + return analytical_leg_jacobian(motor_angles, leg_id) + + def stand_up(self, + standup_time=1.5, + reset_time=5, + default_motor_angles=None): + logging.warning( + "About to reset the robot, make sure the robot is hang-up.") + + if default_motor_angles is None: + default_motor_angles = INIT_MOTOR_ANGLES + + for _ in range(20): + self.ReceiveObservation() + + current_motor_angles = self.GetMotorAngles() + tik = time.time() + count = 0 + for t in np.arange(0, reset_time, self.time_step): + count += 1 + stand_up_last_t = time.time() + blend_ratio = min(t / standup_time, 1) + action = blend_ratio * default_motor_angles + ( + 1 - blend_ratio) * current_motor_angles + self.Step(action, MotorControlMode.POSITION) + while time.time() - stand_up_last_t < self.time_step: + pass + tok = time.time() + print("stand up cost time = ", -tik + tok) + + def sit_down(self, + sitdown_time=1.5, + reset_time=2, + default_motor_angles=None): + if default_motor_angles is None: + default_motor_angles = SITDOWN_MOTOR_ANGLES + + self.ReceiveObservation() + current_motor_angles = self.GetMotorAngles() + sitdown_time = max(sitdown_time, 1.5) + tik = time.time() + count = 0 + for t in np.arange(0, reset_time, self.time_step): + count += 1 + sitdown_last_t = time.time() + blend_ratio = min(t / sitdown_time, 1) + action = blend_ratio * default_motor_angles + ( + 1 - blend_ratio) * current_motor_angles + self.Step(action, MotorControlMode.POSITION) + while time.time() - sitdown_last_t < self.time_step: + pass + tok = time.time() + print("sit down cost time = ", -tik + tok) + + def _reset(self): + for _ in range(5): + self.ReceiveObservation() + roll = self._base_rpy[0][0] + # make sure the angle is located in range [0, 2*pi] + if roll < 0.: + roll += 2 * PI + print("roll = ", roll) + shrinked_motor_angles = np.array( + [0, 2.0, -2.6, 0, 2.0, -2.6, 0, 2.0, -2.6, 0, 2.0, -2.6], + dtype=np.float32) + + if roll < 0.5 * PI or roll > 1.5 * PI: # the robot is not fliped + t1 = 2.0 # time for shrink legs + t2 = 2.0 # time for stand up + current_motor_angles = self.GetMotorAngles() + self.linearly_transform_angles(t1, current_motor_angles, + shrinked_motor_angles) + self.stand_up(t2) + else: + t1 = 2.0 # time for shrink1 legs + t2 = 1.0 # time for stand up + + shrinked1_motor_angles = np.array( + [0, 1.57, -0.9, 0, 1.57, -0.9, 0, 1.57, -0.9, 0, 1.57, -0.9], + dtype=np.float32) + shrinked2_motor_angles = np.array( + [0, 1.57, -2.5, 0, 1.57, -2.5, 0, 1.57, -2.5, 0, 1.57, -2.5], + dtype=np.float32) + + shrinked3_motor_angles = np.array([ + -0.7, 2.7, -2.5, 0, 1.57, -2.5, -0.7, 2.8, -2.5, 0, 1.57, -2.5 + ], + dtype=np.float32) + + shrinked4_motor_angles = np.array([ + -0.7, 2.7, -2.5, 0.6, 2.5, -2.5, -0.8, 2.8, -2.5, 0.6, 2.5, + -2.5 + ], + dtype=np.float32) + + shrinked5_motor_angles = np.array([ + -0.1, 1.7, -2.5, -0.6, 2.5, -2.5, -0.1, 1.7, -2.5, -0.6, 2.5, + -2.5 + ], + dtype=np.float32) + + shrinked6_motor_angles = np.array( + [0, 0.9, -2.5, 0, 0.9, -2.5, 0, 0.9, -2.5, 0, 0.9, -2.5], + dtype=np.float32) + + current_motor_angles = self.GetMotorAngles() + self.linearly_transform_angles(t1, current_motor_angles, + shrinked1_motor_angles) + + current_motor_angles = self.GetMotorAngles() + self.linearly_transform_angles(t2, current_motor_angles, + shrinked2_motor_angles) + + current_motor_angles = self.GetMotorAngles() + self.linearly_transform_angles(0.5, shrinked2_motor_angles, + shrinked3_motor_angles) + + current_motor_angles = self.GetMotorAngles() + self.linearly_transform_angles(0.3, shrinked3_motor_angles, + shrinked4_motor_angles) + + current_motor_angles = self.GetMotorAngles() + self.linearly_transform_angles(0.5, current_motor_angles, + shrinked5_motor_angles, 0.5) + + current_motor_angles = self.GetMotorAngles() + self.linearly_transform_angles(t1, current_motor_angles, + shrinked_motor_angles, 0.5) + self.stand_up(t2) + + def linearly_transform_angles(self, t1, souce_angles, dest_angles, t2=0): + for t in np.arange(0, t1, self.time_step): + stand_up_last_t = time.time() + blend_ratio = min(t / t1, 1.0) + action = blend_ratio * dest_angles + (1 - + blend_ratio) * souce_angles + self.Step(action, MotorControlMode.POSITION) + + while time.time() - stand_up_last_t < self.time_step: + pass + + for t in np.arange(0, t2, self.time_step): + stand_up_last_t = time.time() + while time.time() - stand_up_last_t < self.time_step: + pass + + def GetRobotObs(self): + base_acc_in_base_frame = torch.from_numpy(self.acc).unsqueeze(0).to( + device=self.device, dtype=torch.float) + is_contact = self.GetFootContacts() + obs = torch.cat( + (base_acc_in_base_frame, torch.from_numpy(self._base_rpy).to( + self.device), torch.from_numpy( + self.GetBaseRollPitchYawRate()).unsqueeze(0).to( + self.device), + torch.from_numpy(self._serialize_dof( + self._motor_angles)).unsqueeze(0).to(self.device), + torch.from_numpy(self._serialize_dof( + self._motor_velocities)).unsqueeze(0).to(self.device), + torch.tensor(np.array([[is_contact[i] for i in [1, 0, 3, 2]]]), + dtype=torch.float, + device=self.device)), + dim=-1) # 3+3+3+12+12+4=37 + return obs + + def _serialize_dof(self, dof_data): + serialized_data = np.zeros(12, dtype=np.float32) + serialized_data[0:3] = dof_data[3:6] + serialized_data[3:6] = dof_data[0:3] + serialized_data[6:9] = dof_data[9:12] + serialized_data[9:12] = dof_data[6:9] + return serialized_data \ No newline at end of file diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/robot_utils.py b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/robot_utils.py new file mode 100644 index 0000000000000000000000000000000000000000..2da94a9042e0b9da15919556b30487e5f278a75f --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/robot_utils.py @@ -0,0 +1,175 @@ +# SPDX-FileCopyrightText: Copyright (c) 2023, HUAWEI TECHNOLOGIES. All rights reserved. +# SPDX-License-Identifier: BSD-3-Clause +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# 3. Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import math +import numpy as np + +NUM_MOTORS = 12 +NUM_LEGS = 4 +MOTOR_NAMES = [ + "FR_hip_joint", + "FR_upper_joint", + "FR_lower_joint", + "FL_hip_joint", + "FL_upper_joint", + "FL_lower_joint", + "RR_hip_joint", + "RR_upper_joint", + "RR_lower_joint", + "RL_hip_joint", + "RL_upper_joint", + "RL_lower_joint", +] + +# position mode PD +ABDUCTION_P_GAIN = 100.0 +ABDUCTION_D_GAIN = 1.0 +HIP_P_GAIN = 100.0 +HIP_D_GAIN = 2.0 +KNEE_P_GAIN = 100.0 +KNEE_D_GAIN = 2.0 + +INIT_RACK_POSITION = [0, 0, 1] +INIT_POSITION = [0, 0, 0.48] +JOINT_DIRECTIONS = np.ones(12) +HIP_JOINT_OFFSET = 0.0 +UPPER_LEG_JOINT_OFFSET = 0.0 +KNEE_JOINT_OFFSET = 0.0 +DOFS_PER_LEG = 3 +PI = math.pi + +JOINT_OFFSETS = np.array( + [HIP_JOINT_OFFSET, UPPER_LEG_JOINT_OFFSET, KNEE_JOINT_OFFSET] * 4) +# COM_OFFSET = -np.array([0.005, 0.0028, 0.000515]) # x++ back++ y++ right++ +COM_OFFSET = -np.array([0.0, 0.0, 0.0]) # x++ back++ y++ right++ + +HIP_OFFSETS = np.array([[0.1805, -0.047, 0.], [0.1805, 0.047, 0.], + [-0.1805, -0.047, 0.], [-0.1805, 0.047, 0.] + ]) + COM_OFFSET + +HIP_POSITION = np.array([[0.185, -0.135, 0], [0.185, 0.135, 0], + [-0.185, -0.135, 0], [-0.185, 0.135, 0]]) + +# unitree a1_robot +MPC_BODY_MASS = 10 +MPC_BODY_INERTIA = np.array((0.24, 0, 0, 0, 0.80, 0, 0, 0, 1.00)) # google +MPC_BODY_HEIGHT = 0.28 + +STAND_UP_HEIGHT = MPC_BODY_HEIGHT +LEG_LENGTH = 0.20 +UPPER_LEG_LENGTH = 0.20 +LOWER_LEG_LENGTH = 0.20 +HIP_LENGTH = 0.08505 +STANDUP_ABDUCTION_ANGLE = 0.0 +STANDUP_HIP_ANGLE = np.arccos(STAND_UP_HEIGHT / 2.0 / LEG_LENGTH) +STANDUP_KNEE_ANGLE = -2.0 * STANDUP_HIP_ANGLE +INIT_MOTOR_ANGLES = np.array( + [STANDUP_ABDUCTION_ANGLE, STANDUP_HIP_ANGLE, STANDUP_KNEE_ANGLE] * + NUM_LEGS) +SITDOWN_MOTOR_ANGLES = np.array([0.0, 0.93, -2.6] * NUM_LEGS) + + +def foot_position_in_hip_frame_to_joint_angle(foot_position, l_hip_sign=1): + l_up = 0.2 + l_low = 0.2 + l_hip = 0.08505 * l_hip_sign + x, y, z = foot_position[0], foot_position[1], foot_position[2] + theta_knee = -np.arccos( + np.clip((x**2 + y**2 + z**2 - l_hip**2 - l_low**2 - l_up**2) / + (2 * l_low * l_up), -1.0, 1.0)) + l = np.sqrt(l_up**2 + l_low**2 + 2 * l_up * l_low * np.cos(theta_knee)) + theta_hip = np.arcsin(np.clip(-x / l, -1.0, 1.0)) - theta_knee / 2 + c1 = l_hip * y - l * np.cos(theta_hip + theta_knee / 2) * z + s1 = l * np.cos(theta_hip + theta_knee / 2) * y + l_hip * z + theta_ab = np.arctan2(s1, c1) + return np.array([theta_ab, theta_hip, theta_knee]) + + +def foot_position_in_hip_frame(angles, l_hip_sign=1): + theta_ab, theta_hip, theta_knee = angles[0], angles[1], angles[2] + l_up = 0.2 + l_low = 0.2 + l_hip = 0.08505 * l_hip_sign + leg_distance = np.sqrt(l_up**2 + l_low**2 + + 2 * l_up * l_low * np.cos(theta_knee)) + eff_swing = theta_hip + theta_knee / 2 + + off_x_hip = -leg_distance * np.sin(eff_swing) + off_z_hip = -leg_distance * np.cos(eff_swing) + off_y_hip = l_hip + + off_x = off_x_hip + off_y = np.cos(theta_ab) * off_y_hip - np.sin(theta_ab) * off_z_hip + off_z = np.sin(theta_ab) * off_y_hip + np.cos(theta_ab) * off_z_hip + return np.array([off_x, off_y, off_z]) + + +def analytical_leg_jacobian(leg_angles, leg_id): + """ + Computes the analytical Jacobian. + Args: + ` leg_angles: a list of 3 numbers for current abduction, hip and knee angle. + l_hip_sign: whether it's a left (1) or right(-1) leg. + """ + l_up = 0.2 + l_low = 0.2 + l_hip = 0.08505 * (-1)**(leg_id + 1) + + t1, t2, t3 = leg_angles[0], leg_angles[1], leg_angles[2] + l_eff = np.sqrt(l_up**2 + l_low**2 + 2 * l_up * l_low * np.cos(t3)) + t_eff = t2 + t3 / 2 + + J = np.zeros((3, 3)) + J[0, 0] = 0 + J[0, 1] = -l_eff * np.cos(t_eff) + J[0, 2] = l_low * l_up * np.sin(t3) * np.sin( + t_eff) / l_eff - l_eff * np.cos(t_eff) / 2 + J[1, 0] = -l_hip * np.sin(t1) + l_eff * np.cos(t1) * np.cos(t_eff) + J[1, 1] = -l_eff * np.sin(t1) * np.sin(t_eff) + J[1, 2] = -l_low * l_up * np.sin(t1) * np.sin(t3) * np.cos( + t_eff) / l_eff - l_eff * np.sin(t1) * np.sin(t_eff) / 2 + J[2, 0] = l_hip * np.cos(t1) + l_eff * np.sin(t1) * np.cos(t_eff) + J[2, 1] = l_eff * np.sin(t_eff) * np.cos(t1) + J[2, 2] = l_low * l_up * np.sin(t3) * np.cos(t1) * np.cos( + t_eff) / l_eff + l_eff * np.sin(t_eff) * np.cos(t1) / 2 + return J + + +# For JIT compilation +foot_position_in_hip_frame_to_joint_angle(np.random.uniform(size=3), 1) +foot_position_in_hip_frame_to_joint_angle(np.random.uniform(size=3), -1) + + +def foot_positions_in_base_frame(foot_angles): + foot_angles = foot_angles.reshape((4, 3)) + foot_positions = np.zeros((4, 3)) + for i in range(4): + foot_positions[i] = foot_position_in_hip_frame(foot_angles[i], + l_hip_sign=(-1)**(i + + 1)) + return foot_positions + HIP_OFFSETS diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/.gitignore b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..378eac25d311703f3f2cd456d8036da525cd0366 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/.gitignore @@ -0,0 +1 @@ +build diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/CMakeLists.txt b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..578138e79072145fb886124591c5a64818713883 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 2.8.3) +project(unitree_legged_sdk) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + sensor_msgs +) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +link_directories(lib) + +add_compile_options(-std=c++14) + +EXECUTE_PROCESS( COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE ) +message( STATUS "Architecture: ${ARCHITECTURE}") # x86_64, amd64, arm64, aarch64 +if((${ARCHITECTURE} STREQUAL "x86_64") OR (${ARCHITECTURE} STREQUAL "amd64")) + set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm ${catkin_LIBRARIES}) +elseif((${ARCHITECTURE} STREQUAL "arm64") OR (${ARCHITECTURE} STREQUAL "aarch64")) + set(EXTRA_LIBS -pthread libunitree_legged_sdk_arm64.so lcm ${catkin_LIBRARIES}) +else() + message(FATAL_ERROR "unrecongnized arch.") +endif() + +set(CMAKE_CXX_FLAGS "-O3") + +add_subdirectory(pybind11) + +pybind11_add_module(robot_interface python_interface.cpp) +target_link_libraries(robot_interface ${EXTRA_LIBS}) + diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/ChangeLog.md b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/ChangeLog.md new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/LICENSE b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/LICENSE new file mode 100644 index 0000000000000000000000000000000000000000..a612ad9813b006ce81d1ee438dd784da99a54007 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/LICENSE @@ -0,0 +1,373 @@ +Mozilla Public License Version 2.0 +================================== + +1. Definitions +-------------- + +1.1. "Contributor" + means each individual or legal entity that creates, contributes to + the creation of, or owns Covered Software. + +1.2. "Contributor Version" + means the combination of the Contributions of others (if any) used + by a Contributor and that particular Contributor's Contribution. + +1.3. "Contribution" + means Covered Software of a particular Contributor. + +1.4. "Covered Software" + means Source Code Form to which the initial Contributor has attached + the notice in Exhibit A, the Executable Form of such Source Code + Form, and Modifications of such Source Code Form, in each case + including portions thereof. + +1.5. "Incompatible With Secondary Licenses" + means + + (a) that the initial Contributor has attached the notice described + in Exhibit B to the Covered Software; or + + (b) that the Covered Software was made available under the terms of + version 1.1 or earlier of the License, but not also under the + terms of a Secondary License. + +1.6. "Executable Form" + means any form of the work other than Source Code Form. + +1.7. "Larger Work" + means a work that combines Covered Software with other material, in + a separate file or files, that is not Covered Software. + +1.8. "License" + means this document. + +1.9. "Licensable" + means having the right to grant, to the maximum extent possible, + whether at the time of the initial grant or subsequently, any and + all of the rights conveyed by this License. + +1.10. "Modifications" + means any of the following: + + (a) any file in Source Code Form that results from an addition to, + deletion from, or modification of the contents of Covered + Software; or + + (b) any new file in Source Code Form that contains any Covered + Software. + +1.11. "Patent Claims" of a Contributor + means any patent claim(s), including without limitation, method, + process, and apparatus claims, in any patent Licensable by such + Contributor that would be infringed, but for the grant of the + License, by the making, using, selling, offering for sale, having + made, import, or transfer of either its Contributions or its + Contributor Version. + +1.12. "Secondary License" + means either the GNU General Public License, Version 2.0, the GNU + Lesser General Public License, Version 2.1, the GNU Affero General + Public License, Version 3.0, or any later versions of those + licenses. + +1.13. "Source Code Form" + means the form of the work preferred for making modifications. + +1.14. "You" (or "Your") + means an individual or a legal entity exercising rights under this + License. For legal entities, "You" includes any entity that + controls, is controlled by, or is under common control with You. For + purposes of this definition, "control" means (a) the power, direct + or indirect, to cause the direction or management of such entity, + whether by contract or otherwise, or (b) ownership of more than + fifty percent (50%) of the outstanding shares or beneficial + ownership of such entity. + +2. License Grants and Conditions +-------------------------------- + +2.1. Grants + +Each Contributor hereby grants You a world-wide, royalty-free, +non-exclusive license: + +(a) under intellectual property rights (other than patent or trademark) + Licensable by such Contributor to use, reproduce, make available, + modify, display, perform, distribute, and otherwise exploit its + Contributions, either on an unmodified basis, with Modifications, or + as part of a Larger Work; and + +(b) under Patent Claims of such Contributor to make, use, sell, offer + for sale, have made, import, and otherwise transfer either its + Contributions or its Contributor Version. + +2.2. Effective Date + +The licenses granted in Section 2.1 with respect to any Contribution +become effective for each Contribution on the date the Contributor first +distributes such Contribution. + +2.3. Limitations on Grant Scope + +The licenses granted in this Section 2 are the only rights granted under +this License. No additional rights or licenses will be implied from the +distribution or licensing of Covered Software under this License. +Notwithstanding Section 2.1(b) above, no patent license is granted by a +Contributor: + +(a) for any code that a Contributor has removed from Covered Software; + or + +(b) for infringements caused by: (i) Your and any other third party's + modifications of Covered Software, or (ii) the combination of its + Contributions with other software (except as part of its Contributor + Version); or + +(c) under Patent Claims infringed by Covered Software in the absence of + its Contributions. + +This License does not grant any rights in the trademarks, service marks, +or logos of any Contributor (except as may be necessary to comply with +the notice requirements in Section 3.4). + +2.4. Subsequent Licenses + +No Contributor makes additional grants as a result of Your choice to +distribute the Covered Software under a subsequent version of this +License (see Section 10.2) or under the terms of a Secondary License (if +permitted under the terms of Section 3.3). + +2.5. Representation + +Each Contributor represents that the Contributor believes its +Contributions are its original creation(s) or it has sufficient rights +to grant the rights to its Contributions conveyed by this License. + +2.6. Fair Use + +This License is not intended to limit any rights You have under +applicable copyright doctrines of fair use, fair dealing, or other +equivalents. + +2.7. Conditions + +Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted +in Section 2.1. + +3. Responsibilities +------------------- + +3.1. Distribution of Source Form + +All distribution of Covered Software in Source Code Form, including any +Modifications that You create or to which You contribute, must be under +the terms of this License. You must inform recipients that the Source +Code Form of the Covered Software is governed by the terms of this +License, and how they can obtain a copy of this License. You may not +attempt to alter or restrict the recipients' rights in the Source Code +Form. + +3.2. Distribution of Executable Form + +If You distribute Covered Software in Executable Form then: + +(a) such Covered Software must also be made available in Source Code + Form, as described in Section 3.1, and You must inform recipients of + the Executable Form how they can obtain a copy of such Source Code + Form by reasonable means in a timely manner, at a charge no more + than the cost of distribution to the recipient; and + +(b) You may distribute such Executable Form under the terms of this + License, or sublicense it under different terms, provided that the + license for the Executable Form does not attempt to limit or alter + the recipients' rights in the Source Code Form under this License. + +3.3. Distribution of a Larger Work + +You may create and distribute a Larger Work under terms of Your choice, +provided that You also comply with the requirements of this License for +the Covered Software. If the Larger Work is a combination of Covered +Software with a work governed by one or more Secondary Licenses, and the +Covered Software is not Incompatible With Secondary Licenses, this +License permits You to additionally distribute such Covered Software +under the terms of such Secondary License(s), so that the recipient of +the Larger Work may, at their option, further distribute the Covered +Software under the terms of either this License or such Secondary +License(s). + +3.4. Notices + +You may not remove or alter the substance of any license notices +(including copyright notices, patent notices, disclaimers of warranty, +or limitations of liability) contained within the Source Code Form of +the Covered Software, except that You may alter any license notices to +the extent required to remedy known factual inaccuracies. + +3.5. Application of Additional Terms + +You may choose to offer, and to charge a fee for, warranty, support, +indemnity or liability obligations to one or more recipients of Covered +Software. However, You may do so only on Your own behalf, and not on +behalf of any Contributor. You must make it absolutely clear that any +such warranty, support, indemnity, or liability obligation is offered by +You alone, and You hereby agree to indemnify every Contributor for any +liability incurred by such Contributor as a result of warranty, support, +indemnity or liability terms You offer. You may include additional +disclaimers of warranty and limitations of liability specific to any +jurisdiction. + +4. Inability to Comply Due to Statute or Regulation +--------------------------------------------------- + +If it is impossible for You to comply with any of the terms of this +License with respect to some or all of the Covered Software due to +statute, judicial order, or regulation then You must: (a) comply with +the terms of this License to the maximum extent possible; and (b) +describe the limitations and the code they affect. Such description must +be placed in a text file included with all distributions of the Covered +Software under this License. Except to the extent prohibited by statute +or regulation, such description must be sufficiently detailed for a +recipient of ordinary skill to be able to understand it. + +5. Termination +-------------- + +5.1. The rights granted under this License will terminate automatically +if You fail to comply with any of its terms. However, if You become +compliant, then the rights granted under this License from a particular +Contributor are reinstated (a) provisionally, unless and until such +Contributor explicitly and finally terminates Your grants, and (b) on an +ongoing basis, if such Contributor fails to notify You of the +non-compliance by some reasonable means prior to 60 days after You have +come back into compliance. Moreover, Your grants from a particular +Contributor are reinstated on an ongoing basis if such Contributor +notifies You of the non-compliance by some reasonable means, this is the +first time You have received notice of non-compliance with this License +from such Contributor, and You become compliant prior to 30 days after +Your receipt of the notice. + +5.2. If You initiate litigation against any entity by asserting a patent +infringement claim (excluding declaratory judgment actions, +counter-claims, and cross-claims) alleging that a Contributor Version +directly or indirectly infringes any patent, then the rights granted to +You by any and all Contributors for the Covered Software under Section +2.1 of this License shall terminate. + +5.3. In the event of termination under Sections 5.1 or 5.2 above, all +end user license agreements (excluding distributors and resellers) which +have been validly granted by You or Your distributors under this License +prior to termination shall survive termination. + +************************************************************************ +* * +* 6. Disclaimer of Warranty * +* ------------------------- * +* * +* Covered Software is provided under this License on an "as is" * +* basis, without warranty of any kind, either expressed, implied, or * +* statutory, including, without limitation, warranties that the * +* Covered Software is free of defects, merchantable, fit for a * +* particular purpose or non-infringing. The entire risk as to the * +* quality and performance of the Covered Software is with You. * +* Should any Covered Software prove defective in any respect, You * +* (not any Contributor) assume the cost of any necessary servicing, * +* repair, or correction. This disclaimer of warranty constitutes an * +* essential part of this License. No use of any Covered Software is * +* authorized under this License except under this disclaimer. * +* * +************************************************************************ + +************************************************************************ +* * +* 7. Limitation of Liability * +* -------------------------- * +* * +* Under no circumstances and under no legal theory, whether tort * +* (including negligence), contract, or otherwise, shall any * +* Contributor, or anyone who distributes Covered Software as * +* permitted above, be liable to You for any direct, indirect, * +* special, incidental, or consequential damages of any character * +* including, without limitation, damages for lost profits, loss of * +* goodwill, work stoppage, computer failure or malfunction, or any * +* and all other commercial damages or losses, even if such party * +* shall have been informed of the possibility of such damages. This * +* limitation of liability shall not apply to liability for death or * +* personal injury resulting from such party's negligence to the * +* extent applicable law prohibits such limitation. Some * +* jurisdictions do not allow the exclusion or limitation of * +* incidental or consequential damages, so this exclusion and * +* limitation may not apply to You. * +* * +************************************************************************ + +8. Litigation +------------- + +Any litigation relating to this License may be brought only in the +courts of a jurisdiction where the defendant maintains its principal +place of business and such litigation shall be governed by laws of that +jurisdiction, without reference to its conflict-of-law provisions. +Nothing in this Section shall prevent a party's ability to bring +cross-claims or counter-claims. + +9. Miscellaneous +---------------- + +This License represents the complete agreement concerning the subject +matter hereof. If any provision of this License is held to be +unenforceable, such provision shall be reformed only to the extent +necessary to make it enforceable. Any law or regulation which provides +that the language of a contract shall be construed against the drafter +shall not be used to construe this License against a Contributor. + +10. Versions of the License +--------------------------- + +10.1. New Versions + +Mozilla Foundation is the license steward. Except as provided in Section +10.3, no one other than the license steward has the right to modify or +publish new versions of this License. Each version will be given a +distinguishing version number. + +10.2. Effect of New Versions + +You may distribute the Covered Software under the terms of the version +of the License under which You originally received the Covered Software, +or under the terms of any subsequent version published by the license +steward. + +10.3. Modified Versions + +If you create software not governed by this License, and you want to +create a new license for such software, you may create and use a +modified version of this License if you rename the license and remove +any references to the name of the license steward (except to note that +such modified license differs from this License). + +10.4. Distributing Source Code Form that is Incompatible With Secondary +Licenses + +If You choose to distribute Source Code Form that is Incompatible With +Secondary Licenses under the terms of this version of the License, the +notice described in Exhibit B of this License must be attached. + +Exhibit A - Source Code Form License Notice +------------------------------------------- + + This Source Code Form is subject to the terms of the Mozilla Public + License, v. 2.0. If a copy of the MPL was not distributed with this + file, You can obtain one at http://mozilla.org/MPL/2.0/. + +If it is not possible or desirable to put the notice in a particular +file, then You may include the notice in a location (such as a LICENSE +file in a relevant directory) where a recipient would be likely to look +for such a notice. + +You may add additional accurate notices of copyright ownership. + +Exhibit B - "Incompatible With Secondary Licenses" Notice +--------------------------------------------------------- + + This Source Code Form is "Incompatible With Secondary Licenses", as + defined by the Mozilla Public License, v. 2.0. diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/BmsCmd.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/BmsCmd.h new file mode 100644 index 0000000000000000000000000000000000000000..7384053a1960846d33bc433500fb533e677a9374 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/BmsCmd.h @@ -0,0 +1,211 @@ +// Generated by gencpp from file unitree_legged_msgs/BmsCmd.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_BMSCMD_H +#define UNITREE_LEGGED_MSGS_MESSAGE_BMSCMD_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct BmsCmd_ +{ + typedef BmsCmd_ Type; + + BmsCmd_() + : off(0) + , reserve() { + reserve.assign(0); + } + BmsCmd_(const ContainerAllocator& _alloc) + : off(0) + , reserve() { + (void)_alloc; + reserve.assign(0); + } + + + + typedef uint8_t _off_type; + _off_type off; + + typedef boost::array _reserve_type; + _reserve_type reserve; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::BmsCmd_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::BmsCmd_ const> ConstPtr; + +}; // struct BmsCmd_ + +typedef ::unitree_legged_msgs::BmsCmd_ > BmsCmd; + +typedef boost::shared_ptr< ::unitree_legged_msgs::BmsCmd > BmsCmdPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::BmsCmd const> BmsCmdConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::BmsCmd_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::BmsCmd_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::BmsCmd_ & lhs, const ::unitree_legged_msgs::BmsCmd_ & rhs) +{ + return lhs.off == rhs.off && + lhs.reserve == rhs.reserve; +} + +template +bool operator!=(const ::unitree_legged_msgs::BmsCmd_ & lhs, const ::unitree_legged_msgs::BmsCmd_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::BmsCmd_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::BmsCmd_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::BmsCmd_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::BmsCmd_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::BmsCmd_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::BmsCmd_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::BmsCmd_ > +{ + static const char* value() + { + return "c09195f3200bd8917201f8805a3fe1d1"; + } + + static const char* value(const ::unitree_legged_msgs::BmsCmd_&) { return value(); } + static const uint64_t static_value1 = 0xc09195f3200bd891ULL; + static const uint64_t static_value2 = 0x7201f8805a3fe1d1ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::BmsCmd_ > +{ + static const char* value() + { + return "unitree_legged_msgs/BmsCmd"; + } + + static const char* value(const ::unitree_legged_msgs::BmsCmd_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::BmsCmd_ > +{ + static const char* value() + { + return "uint8 off # off 0xA5\n" +"uint8[3] reserve\n" +; + } + + static const char* value(const ::unitree_legged_msgs::BmsCmd_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::BmsCmd_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.off); + stream.next(m.reserve); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct BmsCmd_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::BmsCmd_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::BmsCmd_& v) + { + s << indent << "off: "; + Printer::stream(s, indent + " ", v.off); + s << indent << "reserve[]" << std::endl; + for (size_t i = 0; i < v.reserve.size(); ++i) + { + s << indent << " reserve[" << i << "]: "; + Printer::stream(s, indent + " ", v.reserve[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_BMSCMD_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/BmsState.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/BmsState.h new file mode 100644 index 0000000000000000000000000000000000000000..fb5fa0b595c196a3e4ac4167bb29ae2e8e0f9e16 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/BmsState.h @@ -0,0 +1,297 @@ +// Generated by gencpp from file unitree_legged_msgs/BmsState.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_BMSSTATE_H +#define UNITREE_LEGGED_MSGS_MESSAGE_BMSSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct BmsState_ +{ + typedef BmsState_ Type; + + BmsState_() + : version_h(0) + , version_l(0) + , bms_status(0) + , SOC(0) + , current(0) + , cycle(0) + , BQ_NTC() + , MCU_NTC() + , cell_vol() { + BQ_NTC.assign(0); + + MCU_NTC.assign(0); + + cell_vol.assign(0); + } + BmsState_(const ContainerAllocator& _alloc) + : version_h(0) + , version_l(0) + , bms_status(0) + , SOC(0) + , current(0) + , cycle(0) + , BQ_NTC() + , MCU_NTC() + , cell_vol() { + (void)_alloc; + BQ_NTC.assign(0); + + MCU_NTC.assign(0); + + cell_vol.assign(0); + } + + + + typedef uint8_t _version_h_type; + _version_h_type version_h; + + typedef uint8_t _version_l_type; + _version_l_type version_l; + + typedef uint8_t _bms_status_type; + _bms_status_type bms_status; + + typedef uint8_t _SOC_type; + _SOC_type SOC; + + typedef int32_t _current_type; + _current_type current; + + typedef uint16_t _cycle_type; + _cycle_type cycle; + + typedef boost::array _BQ_NTC_type; + _BQ_NTC_type BQ_NTC; + + typedef boost::array _MCU_NTC_type; + _MCU_NTC_type MCU_NTC; + + typedef boost::array _cell_vol_type; + _cell_vol_type cell_vol; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::BmsState_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::BmsState_ const> ConstPtr; + +}; // struct BmsState_ + +typedef ::unitree_legged_msgs::BmsState_ > BmsState; + +typedef boost::shared_ptr< ::unitree_legged_msgs::BmsState > BmsStatePtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::BmsState const> BmsStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::BmsState_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::BmsState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::BmsState_ & lhs, const ::unitree_legged_msgs::BmsState_ & rhs) +{ + return lhs.version_h == rhs.version_h && + lhs.version_l == rhs.version_l && + lhs.bms_status == rhs.bms_status && + lhs.SOC == rhs.SOC && + lhs.current == rhs.current && + lhs.cycle == rhs.cycle && + lhs.BQ_NTC == rhs.BQ_NTC && + lhs.MCU_NTC == rhs.MCU_NTC && + lhs.cell_vol == rhs.cell_vol; +} + +template +bool operator!=(const ::unitree_legged_msgs::BmsState_ & lhs, const ::unitree_legged_msgs::BmsState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::BmsState_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::BmsState_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::BmsState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::BmsState_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::BmsState_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::BmsState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::BmsState_ > +{ + static const char* value() + { + return "8e007c660c590388bca7c2464d80df54"; + } + + static const char* value(const ::unitree_legged_msgs::BmsState_&) { return value(); } + static const uint64_t static_value1 = 0x8e007c660c590388ULL; + static const uint64_t static_value2 = 0xbca7c2464d80df54ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::BmsState_ > +{ + static const char* value() + { + return "unitree_legged_msgs/BmsState"; + } + + static const char* value(const ::unitree_legged_msgs::BmsState_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::BmsState_ > +{ + static const char* value() + { + return "uint8 version_h\n" +"uint8 version_l\n" +"uint8 bms_status\n" +"uint8 SOC # SOC 0-100%\n" +"int32 current # mA\n" +"uint16 cycle\n" +"int8[2] BQ_NTC # x1 degrees centigrade\n" +"int8[2] MCU_NTC # x1 degrees centigrade\n" +"uint16[10] cell_vol # cell voltage mV\n" +; + } + + static const char* value(const ::unitree_legged_msgs::BmsState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::BmsState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.version_h); + stream.next(m.version_l); + stream.next(m.bms_status); + stream.next(m.SOC); + stream.next(m.current); + stream.next(m.cycle); + stream.next(m.BQ_NTC); + stream.next(m.MCU_NTC); + stream.next(m.cell_vol); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct BmsState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::BmsState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::BmsState_& v) + { + s << indent << "version_h: "; + Printer::stream(s, indent + " ", v.version_h); + s << indent << "version_l: "; + Printer::stream(s, indent + " ", v.version_l); + s << indent << "bms_status: "; + Printer::stream(s, indent + " ", v.bms_status); + s << indent << "SOC: "; + Printer::stream(s, indent + " ", v.SOC); + s << indent << "current: "; + Printer::stream(s, indent + " ", v.current); + s << indent << "cycle: "; + Printer::stream(s, indent + " ", v.cycle); + s << indent << "BQ_NTC[]" << std::endl; + for (size_t i = 0; i < v.BQ_NTC.size(); ++i) + { + s << indent << " BQ_NTC[" << i << "]: "; + Printer::stream(s, indent + " ", v.BQ_NTC[i]); + } + s << indent << "MCU_NTC[]" << std::endl; + for (size_t i = 0; i < v.MCU_NTC.size(); ++i) + { + s << indent << " MCU_NTC[" << i << "]: "; + Printer::stream(s, indent + " ", v.MCU_NTC[i]); + } + s << indent << "cell_vol[]" << std::endl; + for (size_t i = 0; i < v.cell_vol.size(); ++i) + { + s << indent << " cell_vol[" << i << "]: "; + Printer::stream(s, indent + " ", v.cell_vol[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_BMSSTATE_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/Cartesian.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/Cartesian.h new file mode 100644 index 0000000000000000000000000000000000000000..1c367e3953f18fee595a5ce4a3b0f0a655417eee --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/Cartesian.h @@ -0,0 +1,215 @@ +// Generated by gencpp from file unitree_legged_msgs/Cartesian.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H +#define UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct Cartesian_ +{ + typedef Cartesian_ Type; + + Cartesian_() + : x(0.0) + , y(0.0) + , z(0.0) { + } + Cartesian_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) { + (void)_alloc; + } + + + + typedef float _x_type; + _x_type x; + + typedef float _y_type; + _y_type y; + + typedef float _z_type; + _z_type z; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian_ const> ConstPtr; + +}; // struct Cartesian_ + +typedef ::unitree_legged_msgs::Cartesian_ > Cartesian; + +typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian > CartesianPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::Cartesian const> CartesianConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::Cartesian_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::Cartesian_ & lhs, const ::unitree_legged_msgs::Cartesian_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.z == rhs.z; +} + +template +bool operator!=(const ::unitree_legged_msgs::Cartesian_ & lhs, const ::unitree_legged_msgs::Cartesian_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::Cartesian_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::Cartesian_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::Cartesian_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::Cartesian_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::Cartesian_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::Cartesian_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::Cartesian_ > +{ + static const char* value() + { + return "cc153912f1453b708d221682bc23d9ac"; + } + + static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } + static const uint64_t static_value1 = 0xcc153912f1453b70ULL; + static const uint64_t static_value2 = 0x8d221682bc23d9acULL; +}; + +template +struct DataType< ::unitree_legged_msgs::Cartesian_ > +{ + static const char* value() + { + return "unitree_legged_msgs/Cartesian"; + } + + static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::Cartesian_ > +{ + static const char* value() + { + return "float32 x\n" +"float32 y\n" +"float32 z\n" +; + } + + static const char* value(const ::unitree_legged_msgs::Cartesian_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::Cartesian_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Cartesian_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::Cartesian_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::Cartesian_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_CARTESIAN_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/HighCmd.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/HighCmd.h new file mode 100644 index 0000000000000000000000000000000000000000..37aea9656ba0e7a850336ebee4982188e5128b91 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/HighCmd.h @@ -0,0 +1,462 @@ +// Generated by gencpp from file unitree_legged_msgs/HighCmd.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H +#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace unitree_legged_msgs +{ +template +struct HighCmd_ +{ + typedef HighCmd_ Type; + + HighCmd_() + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , mode(0) + , gaitType(0) + , speedLevel(0) + , footRaiseHeight(0.0) + , bodyHeight(0.0) + , position() + , euler() + , velocity() + , yawSpeed(0.0) + , bms() + , led() + , wirelessRemote() + , reserve(0) + , crc(0) { + head.assign(0); + + SN.assign(0); + + version.assign(0); + + position.assign(0.0); + + euler.assign(0.0); + + velocity.assign(0.0); + + wirelessRemote.assign(0); + } + HighCmd_(const ContainerAllocator& _alloc) + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , mode(0) + , gaitType(0) + , speedLevel(0) + , footRaiseHeight(0.0) + , bodyHeight(0.0) + , position() + , euler() + , velocity() + , yawSpeed(0.0) + , bms(_alloc) + , led() + , wirelessRemote() + , reserve(0) + , crc(0) { + (void)_alloc; + head.assign(0); + + SN.assign(0); + + version.assign(0); + + position.assign(0.0); + + euler.assign(0.0); + + velocity.assign(0.0); + + led.assign( ::unitree_legged_msgs::LED_ (_alloc)); + + wirelessRemote.assign(0); + } + + + + typedef boost::array _head_type; + _head_type head; + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint8_t _frameReserve_type; + _frameReserve_type frameReserve; + + typedef boost::array _SN_type; + _SN_type SN; + + typedef boost::array _version_type; + _version_type version; + + typedef uint16_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef uint8_t _gaitType_type; + _gaitType_type gaitType; + + typedef uint8_t _speedLevel_type; + _speedLevel_type speedLevel; + + typedef float _footRaiseHeight_type; + _footRaiseHeight_type footRaiseHeight; + + typedef float _bodyHeight_type; + _bodyHeight_type bodyHeight; + + typedef boost::array _position_type; + _position_type position; + + typedef boost::array _euler_type; + _euler_type euler; + + typedef boost::array _velocity_type; + _velocity_type velocity; + + typedef float _yawSpeed_type; + _yawSpeed_type yawSpeed; + + typedef ::unitree_legged_msgs::BmsCmd_ _bms_type; + _bms_type bms; + + typedef boost::array< ::unitree_legged_msgs::LED_ , 4> _led_type; + _led_type led; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef uint32_t _crc_type; + _crc_type crc; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd_ const> ConstPtr; + +}; // struct HighCmd_ + +typedef ::unitree_legged_msgs::HighCmd_ > HighCmd; + +typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd > HighCmdPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::HighCmd const> HighCmdConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighCmd_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::HighCmd_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::HighCmd_ & lhs, const ::unitree_legged_msgs::HighCmd_ & rhs) +{ + return lhs.head == rhs.head && + lhs.levelFlag == rhs.levelFlag && + lhs.frameReserve == rhs.frameReserve && + lhs.SN == rhs.SN && + lhs.version == rhs.version && + lhs.bandWidth == rhs.bandWidth && + lhs.mode == rhs.mode && + lhs.gaitType == rhs.gaitType && + lhs.speedLevel == rhs.speedLevel && + lhs.footRaiseHeight == rhs.footRaiseHeight && + lhs.bodyHeight == rhs.bodyHeight && + lhs.position == rhs.position && + lhs.euler == rhs.euler && + lhs.velocity == rhs.velocity && + lhs.yawSpeed == rhs.yawSpeed && + lhs.bms == rhs.bms && + lhs.led == rhs.led && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc; +} + +template +bool operator!=(const ::unitree_legged_msgs::HighCmd_ & lhs, const ::unitree_legged_msgs::HighCmd_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::HighCmd_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::HighCmd_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::HighCmd_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::HighCmd_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighCmd_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighCmd_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::HighCmd_ > +{ + static const char* value() + { + return "b4825051cf66ae8183b54c57dba16b66"; + } + + static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } + static const uint64_t static_value1 = 0xb4825051cf66ae81ULL; + static const uint64_t static_value2 = 0x83b54c57dba16b66ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::HighCmd_ > +{ + static const char* value() + { + return "unitree_legged_msgs/HighCmd"; + } + + static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::HighCmd_ > +{ + static const char* value() + { + return "uint8[2] head\n" +"uint8 levelFlag\n" +"uint8 frameReserve\n" +"\n" +"uint32[2] SN\n" +"uint32[2] version\n" +"uint16 bandWidth\n" +"uint8 mode \n" +"\n" +"uint8 gaitType \n" +"uint8 speedLevel \n" +"float32 footRaiseHeight \n" +"float32 bodyHeight \n" +"float32[2] position \n" +"float32[3] euler \n" +"float32[2] velocity \n" +"float32 yawSpeed \n" +"BmsCmd bms\n" +"LED[4] led\n" +"uint8[40] wirelessRemote\n" +"uint32 reserve\n" +"\n" +"uint32 crc\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/BmsCmd\n" +"uint8 off # off 0xA5\n" +"uint8[3] reserve\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/LED\n" +"uint8 r\n" +"uint8 g\n" +"uint8 b\n" +; + } + + static const char* value(const ::unitree_legged_msgs::HighCmd_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::HighCmd_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.head); + stream.next(m.levelFlag); + stream.next(m.frameReserve); + stream.next(m.SN); + stream.next(m.version); + stream.next(m.bandWidth); + stream.next(m.mode); + stream.next(m.gaitType); + stream.next(m.speedLevel); + stream.next(m.footRaiseHeight); + stream.next(m.bodyHeight); + stream.next(m.position); + stream.next(m.euler); + stream.next(m.velocity); + stream.next(m.yawSpeed); + stream.next(m.bms); + stream.next(m.led); + stream.next(m.wirelessRemote); + stream.next(m.reserve); + stream.next(m.crc); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct HighCmd_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::HighCmd_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighCmd_& v) + { + s << indent << "head[]" << std::endl; + for (size_t i = 0; i < v.head.size(); ++i) + { + s << indent << " head[" << i << "]: "; + Printer::stream(s, indent + " ", v.head[i]); + } + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "frameReserve: "; + Printer::stream(s, indent + " ", v.frameReserve); + s << indent << "SN[]" << std::endl; + for (size_t i = 0; i < v.SN.size(); ++i) + { + s << indent << " SN[" << i << "]: "; + Printer::stream(s, indent + " ", v.SN[i]); + } + s << indent << "version[]" << std::endl; + for (size_t i = 0; i < v.version.size(); ++i) + { + s << indent << " version[" << i << "]: "; + Printer::stream(s, indent + " ", v.version[i]); + } + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "gaitType: "; + Printer::stream(s, indent + " ", v.gaitType); + s << indent << "speedLevel: "; + Printer::stream(s, indent + " ", v.speedLevel); + s << indent << "footRaiseHeight: "; + Printer::stream(s, indent + " ", v.footRaiseHeight); + s << indent << "bodyHeight: "; + Printer::stream(s, indent + " ", v.bodyHeight); + s << indent << "position[]" << std::endl; + for (size_t i = 0; i < v.position.size(); ++i) + { + s << indent << " position[" << i << "]: "; + Printer::stream(s, indent + " ", v.position[i]); + } + s << indent << "euler[]" << std::endl; + for (size_t i = 0; i < v.euler.size(); ++i) + { + s << indent << " euler[" << i << "]: "; + Printer::stream(s, indent + " ", v.euler[i]); + } + s << indent << "velocity[]" << std::endl; + for (size_t i = 0; i < v.velocity.size(); ++i) + { + s << indent << " velocity[" << i << "]: "; + Printer::stream(s, indent + " ", v.velocity[i]); + } + s << indent << "yawSpeed: "; + Printer::stream(s, indent + " ", v.yawSpeed); + s << indent << "bms: "; + s << std::endl; + Printer< ::unitree_legged_msgs::BmsCmd_ >::stream(s, indent + " ", v.bms); + s << indent << "led[]" << std::endl; + for (size_t i = 0; i < v.led.size(); ++i) + { + s << indent << " led[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::LED_ >::stream(s, indent + " ", v.led[i]); + } + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHCMD_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/HighState.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/HighState.h new file mode 100644 index 0000000000000000000000000000000000000000..c44c1520764a4e9cc8e6c0169f1348fbe73f2d9f --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/HighState.h @@ -0,0 +1,574 @@ +// Generated by gencpp from file unitree_legged_msgs/HighState.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H +#define UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace unitree_legged_msgs +{ +template +struct HighState_ +{ + typedef HighState_ Type; + + HighState_() + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , imu() + , motorState() + , bms() + , footForce() + , footForceEst() + , mode(0) + , progress(0.0) + , gaitType(0) + , footRaiseHeight(0.0) + , position() + , bodyHeight(0.0) + , velocity() + , yawSpeed(0.0) + , rangeObstacle() + , footPosition2Body() + , footSpeed2Body() + , wirelessRemote() + , reserve(0) + , crc(0) { + head.assign(0); + + SN.assign(0); + + version.assign(0); + + footForce.assign(0); + + footForceEst.assign(0); + + position.assign(0.0); + + velocity.assign(0.0); + + rangeObstacle.assign(0.0); + + wirelessRemote.assign(0); + } + HighState_(const ContainerAllocator& _alloc) + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , imu(_alloc) + , motorState() + , bms(_alloc) + , footForce() + , footForceEst() + , mode(0) + , progress(0.0) + , gaitType(0) + , footRaiseHeight(0.0) + , position() + , bodyHeight(0.0) + , velocity() + , yawSpeed(0.0) + , rangeObstacle() + , footPosition2Body() + , footSpeed2Body() + , wirelessRemote() + , reserve(0) + , crc(0) { + (void)_alloc; + head.assign(0); + + SN.assign(0); + + version.assign(0); + + motorState.assign( ::unitree_legged_msgs::MotorState_ (_alloc)); + + footForce.assign(0); + + footForceEst.assign(0); + + position.assign(0.0); + + velocity.assign(0.0); + + rangeObstacle.assign(0.0); + + footPosition2Body.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + + footSpeed2Body.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + + wirelessRemote.assign(0); + } + + + + typedef boost::array _head_type; + _head_type head; + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint8_t _frameReserve_type; + _frameReserve_type frameReserve; + + typedef boost::array _SN_type; + _SN_type SN; + + typedef boost::array _version_type; + _version_type version; + + typedef uint16_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef ::unitree_legged_msgs::IMU_ _imu_type; + _imu_type imu; + + typedef boost::array< ::unitree_legged_msgs::MotorState_ , 20> _motorState_type; + _motorState_type motorState; + + typedef ::unitree_legged_msgs::BmsState_ _bms_type; + _bms_type bms; + + typedef boost::array _footForce_type; + _footForce_type footForce; + + typedef boost::array _footForceEst_type; + _footForceEst_type footForceEst; + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef float _progress_type; + _progress_type progress; + + typedef uint8_t _gaitType_type; + _gaitType_type gaitType; + + typedef float _footRaiseHeight_type; + _footRaiseHeight_type footRaiseHeight; + + typedef boost::array _position_type; + _position_type position; + + typedef float _bodyHeight_type; + _bodyHeight_type bodyHeight; + + typedef boost::array _velocity_type; + _velocity_type velocity; + + typedef float _yawSpeed_type; + _yawSpeed_type yawSpeed; + + typedef boost::array _rangeObstacle_type; + _rangeObstacle_type rangeObstacle; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _footPosition2Body_type; + _footPosition2Body_type footPosition2Body; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _footSpeed2Body_type; + _footSpeed2Body_type footSpeed2Body; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef uint32_t _crc_type; + _crc_type crc; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::HighState_ const> ConstPtr; + +}; // struct HighState_ + +typedef ::unitree_legged_msgs::HighState_ > HighState; + +typedef boost::shared_ptr< ::unitree_legged_msgs::HighState > HighStatePtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::HighState const> HighStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::HighState_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::HighState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::HighState_ & lhs, const ::unitree_legged_msgs::HighState_ & rhs) +{ + return lhs.head == rhs.head && + lhs.levelFlag == rhs.levelFlag && + lhs.frameReserve == rhs.frameReserve && + lhs.SN == rhs.SN && + lhs.version == rhs.version && + lhs.bandWidth == rhs.bandWidth && + lhs.imu == rhs.imu && + lhs.motorState == rhs.motorState && + lhs.bms == rhs.bms && + lhs.footForce == rhs.footForce && + lhs.footForceEst == rhs.footForceEst && + lhs.mode == rhs.mode && + lhs.progress == rhs.progress && + lhs.gaitType == rhs.gaitType && + lhs.footRaiseHeight == rhs.footRaiseHeight && + lhs.position == rhs.position && + lhs.bodyHeight == rhs.bodyHeight && + lhs.velocity == rhs.velocity && + lhs.yawSpeed == rhs.yawSpeed && + lhs.rangeObstacle == rhs.rangeObstacle && + lhs.footPosition2Body == rhs.footPosition2Body && + lhs.footSpeed2Body == rhs.footSpeed2Body && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc; +} + +template +bool operator!=(const ::unitree_legged_msgs::HighState_ & lhs, const ::unitree_legged_msgs::HighState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::HighState_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::HighState_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::HighState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::HighState_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighState_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::HighState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::HighState_ > +{ + static const char* value() + { + return "7904f43019dadf7640d573b4c14b9066"; + } + + static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } + static const uint64_t static_value1 = 0x7904f43019dadf76ULL; + static const uint64_t static_value2 = 0x40d573b4c14b9066ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::HighState_ > +{ + static const char* value() + { + return "unitree_legged_msgs/HighState"; + } + + static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::HighState_ > +{ + static const char* value() + { + return "\n" +"uint8[2] head\n" +"uint8 levelFlag\n" +"uint8 frameReserve\n" +"\n" +"uint32[2] SN\n" +"uint32[2] version\n" +"uint16 bandWidth\n" +"IMU imu\n" +"MotorState[20] motorState\n" +"BmsState bms\n" +"int16[4] footForce\n" +"int16[4] footForceEst\n" +"uint8 mode\n" +"float32 progress\n" +"uint8 gaitType \n" +"float32 footRaiseHeight \n" +"float32[3] position \n" +"float32 bodyHeight \n" +"float32[3] velocity \n" +"float32 yawSpeed \n" +"float32[4] rangeObstacle\n" +"Cartesian[4] footPosition2Body \n" +"Cartesian[4] footSpeed2Body \n" +"uint8[40] wirelessRemote\n" +"uint32 reserve\n" +"\n" +"uint32 crc\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/IMU\n" +"float32[4] quaternion\n" +"float32[3] gyroscope\n" +"float32[3] accelerometer\n" +"float32[3] rpy\n" +"int8 temperature\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/MotorState\n" +"uint8 mode # motor current mode \n" +"float32 q # motor current position（rad）\n" +"float32 dq # motor current speed（rad/s）\n" +"float32 ddq # motor current speed（rad/s）\n" +"float32 tauEst # current estimated output torque（N*m）\n" +"float32 q_raw # motor current position（rad）\n" +"float32 dq_raw # motor current speed（rad/s）\n" +"float32 ddq_raw # motor current speed（rad/s）\n" +"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n" +"uint32[2] reserve\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/BmsState\n" +"uint8 version_h\n" +"uint8 version_l\n" +"uint8 bms_status\n" +"uint8 SOC # SOC 0-100%\n" +"int32 current # mA\n" +"uint16 cycle\n" +"int8[2] BQ_NTC # x1 degrees centigrade\n" +"int8[2] MCU_NTC # x1 degrees centigrade\n" +"uint16[10] cell_vol # cell voltage mV\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/Cartesian\n" +"float32 x\n" +"float32 y\n" +"float32 z\n" +; + } + + static const char* value(const ::unitree_legged_msgs::HighState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::HighState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.head); + stream.next(m.levelFlag); + stream.next(m.frameReserve); + stream.next(m.SN); + stream.next(m.version); + stream.next(m.bandWidth); + stream.next(m.imu); + stream.next(m.motorState); + stream.next(m.bms); + stream.next(m.footForce); + stream.next(m.footForceEst); + stream.next(m.mode); + stream.next(m.progress); + stream.next(m.gaitType); + stream.next(m.footRaiseHeight); + stream.next(m.position); + stream.next(m.bodyHeight); + stream.next(m.velocity); + stream.next(m.yawSpeed); + stream.next(m.rangeObstacle); + stream.next(m.footPosition2Body); + stream.next(m.footSpeed2Body); + stream.next(m.wirelessRemote); + stream.next(m.reserve); + stream.next(m.crc); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct HighState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::HighState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::HighState_& v) + { + s << indent << "head[]" << std::endl; + for (size_t i = 0; i < v.head.size(); ++i) + { + s << indent << " head[" << i << "]: "; + Printer::stream(s, indent + " ", v.head[i]); + } + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "frameReserve: "; + Printer::stream(s, indent + " ", v.frameReserve); + s << indent << "SN[]" << std::endl; + for (size_t i = 0; i < v.SN.size(); ++i) + { + s << indent << " SN[" << i << "]: "; + Printer::stream(s, indent + " ", v.SN[i]); + } + s << indent << "version[]" << std::endl; + for (size_t i = 0; i < v.version.size(); ++i) + { + s << indent << " version[" << i << "]: "; + Printer::stream(s, indent + " ", v.version[i]); + } + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "imu: "; + s << std::endl; + Printer< ::unitree_legged_msgs::IMU_ >::stream(s, indent + " ", v.imu); + s << indent << "motorState[]" << std::endl; + for (size_t i = 0; i < v.motorState.size(); ++i) + { + s << indent << " motorState[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::MotorState_ >::stream(s, indent + " ", v.motorState[i]); + } + s << indent << "bms: "; + s << std::endl; + Printer< ::unitree_legged_msgs::BmsState_ >::stream(s, indent + " ", v.bms); + s << indent << "footForce[]" << std::endl; + for (size_t i = 0; i < v.footForce.size(); ++i) + { + s << indent << " footForce[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForce[i]); + } + s << indent << "footForceEst[]" << std::endl; + for (size_t i = 0; i < v.footForceEst.size(); ++i) + { + s << indent << " footForceEst[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForceEst[i]); + } + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "progress: "; + Printer::stream(s, indent + " ", v.progress); + s << indent << "gaitType: "; + Printer::stream(s, indent + " ", v.gaitType); + s << indent << "footRaiseHeight: "; + Printer::stream(s, indent + " ", v.footRaiseHeight); + s << indent << "position[]" << std::endl; + for (size_t i = 0; i < v.position.size(); ++i) + { + s << indent << " position[" << i << "]: "; + Printer::stream(s, indent + " ", v.position[i]); + } + s << indent << "bodyHeight: "; + Printer::stream(s, indent + " ", v.bodyHeight); + s << indent << "velocity[]" << std::endl; + for (size_t i = 0; i < v.velocity.size(); ++i) + { + s << indent << " velocity[" << i << "]: "; + Printer::stream(s, indent + " ", v.velocity[i]); + } + s << indent << "yawSpeed: "; + Printer::stream(s, indent + " ", v.yawSpeed); + s << indent << "rangeObstacle[]" << std::endl; + for (size_t i = 0; i < v.rangeObstacle.size(); ++i) + { + s << indent << " rangeObstacle[" << i << "]: "; + Printer::stream(s, indent + " ", v.rangeObstacle[i]); + } + s << indent << "footPosition2Body[]" << std::endl; + for (size_t i = 0; i < v.footPosition2Body.size(); ++i) + { + s << indent << " footPosition2Body[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.footPosition2Body[i]); + } + s << indent << "footSpeed2Body[]" << std::endl; + for (size_t i = 0; i < v.footSpeed2Body.size(); ++i) + { + s << indent << " footSpeed2Body[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.footSpeed2Body[i]); + } + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_HIGHSTATE_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/IMU.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/IMU.h new file mode 100644 index 0000000000000000000000000000000000000000..01a87971425716831795296c6980df6e6a37fd38 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/IMU.h @@ -0,0 +1,265 @@ +// Generated by gencpp from file unitree_legged_msgs/IMU.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_IMU_H +#define UNITREE_LEGGED_MSGS_MESSAGE_IMU_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct IMU_ +{ + typedef IMU_ Type; + + IMU_() + : quaternion() + , gyroscope() + , accelerometer() + , rpy() + , temperature(0) { + quaternion.assign(0.0); + + gyroscope.assign(0.0); + + accelerometer.assign(0.0); + + rpy.assign(0.0); + } + IMU_(const ContainerAllocator& _alloc) + : quaternion() + , gyroscope() + , accelerometer() + , rpy() + , temperature(0) { + (void)_alloc; + quaternion.assign(0.0); + + gyroscope.assign(0.0); + + accelerometer.assign(0.0); + + rpy.assign(0.0); + } + + + + typedef boost::array _quaternion_type; + _quaternion_type quaternion; + + typedef boost::array _gyroscope_type; + _gyroscope_type gyroscope; + + typedef boost::array _accelerometer_type; + _accelerometer_type accelerometer; + + typedef boost::array _rpy_type; + _rpy_type rpy; + + typedef int8_t _temperature_type; + _temperature_type temperature; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::IMU_ const> ConstPtr; + +}; // struct IMU_ + +typedef ::unitree_legged_msgs::IMU_ > IMU; + +typedef boost::shared_ptr< ::unitree_legged_msgs::IMU > IMUPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::IMU const> IMUConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::IMU_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::IMU_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::IMU_ & lhs, const ::unitree_legged_msgs::IMU_ & rhs) +{ + return lhs.quaternion == rhs.quaternion && + lhs.gyroscope == rhs.gyroscope && + lhs.accelerometer == rhs.accelerometer && + lhs.rpy == rhs.rpy && + lhs.temperature == rhs.temperature; +} + +template +bool operator!=(const ::unitree_legged_msgs::IMU_ & lhs, const ::unitree_legged_msgs::IMU_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::IMU_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::IMU_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::IMU_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::IMU_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::IMU_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::IMU_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::IMU_ > +{ + static const char* value() + { + return "c2bcd83820da00fff261b67d6490da58"; + } + + static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } + static const uint64_t static_value1 = 0xc2bcd83820da00ffULL; + static const uint64_t static_value2 = 0xf261b67d6490da58ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::IMU_ > +{ + static const char* value() + { + return "unitree_legged_msgs/IMU"; + } + + static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::IMU_ > +{ + static const char* value() + { + return "float32[4] quaternion\n" +"float32[3] gyroscope\n" +"float32[3] accelerometer\n" +"float32[3] rpy\n" +"int8 temperature\n" +; + } + + static const char* value(const ::unitree_legged_msgs::IMU_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::IMU_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.quaternion); + stream.next(m.gyroscope); + stream.next(m.accelerometer); + stream.next(m.rpy); + stream.next(m.temperature); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct IMU_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::IMU_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::IMU_& v) + { + s << indent << "quaternion[]" << std::endl; + for (size_t i = 0; i < v.quaternion.size(); ++i) + { + s << indent << " quaternion[" << i << "]: "; + Printer::stream(s, indent + " ", v.quaternion[i]); + } + s << indent << "gyroscope[]" << std::endl; + for (size_t i = 0; i < v.gyroscope.size(); ++i) + { + s << indent << " gyroscope[" << i << "]: "; + Printer::stream(s, indent + " ", v.gyroscope[i]); + } + s << indent << "accelerometer[]" << std::endl; + for (size_t i = 0; i < v.accelerometer.size(); ++i) + { + s << indent << " accelerometer[" << i << "]: "; + Printer::stream(s, indent + " ", v.accelerometer[i]); + } + s << indent << "rpy[]" << std::endl; + for (size_t i = 0; i < v.rpy.size(); ++i) + { + s << indent << " rpy[" << i << "]: "; + Printer::stream(s, indent + " ", v.rpy[i]); + } + s << indent << "temperature: "; + Printer::stream(s, indent + " ", v.temperature); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_IMU_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LED.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LED.h new file mode 100644 index 0000000000000000000000000000000000000000..b7150fb3a2e30cea9e073e3c64daea8d4e057f0f --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LED.h @@ -0,0 +1,215 @@ +// Generated by gencpp from file unitree_legged_msgs/LED.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LED_H +#define UNITREE_LEGGED_MSGS_MESSAGE_LED_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct LED_ +{ + typedef LED_ Type; + + LED_() + : r(0) + , g(0) + , b(0) { + } + LED_(const ContainerAllocator& _alloc) + : r(0) + , g(0) + , b(0) { + (void)_alloc; + } + + + + typedef uint8_t _r_type; + _r_type r; + + typedef uint8_t _g_type; + _g_type g; + + typedef uint8_t _b_type; + _b_type b; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::LED_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::LED_ const> ConstPtr; + +}; // struct LED_ + +typedef ::unitree_legged_msgs::LED_ > LED; + +typedef boost::shared_ptr< ::unitree_legged_msgs::LED > LEDPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::LED const> LEDConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LED_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::LED_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::LED_ & lhs, const ::unitree_legged_msgs::LED_ & rhs) +{ + return lhs.r == rhs.r && + lhs.g == rhs.g && + lhs.b == rhs.b; +} + +template +bool operator!=(const ::unitree_legged_msgs::LED_ & lhs, const ::unitree_legged_msgs::LED_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::LED_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LED_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LED_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LED_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LED_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LED_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::LED_ > +{ + static const char* value() + { + return "353891e354491c51aabe32df673fb446"; + } + + static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } + static const uint64_t static_value1 = 0x353891e354491c51ULL; + static const uint64_t static_value2 = 0xaabe32df673fb446ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::LED_ > +{ + static const char* value() + { + return "unitree_legged_msgs/LED"; + } + + static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::LED_ > +{ + static const char* value() + { + return "uint8 r\n" +"uint8 g\n" +"uint8 b\n" +; + } + + static const char* value(const ::unitree_legged_msgs::LED_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::LED_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.r); + stream.next(m.g); + stream.next(m.b); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LED_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::LED_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LED_& v) + { + s << indent << "r: "; + Printer::stream(s, indent + " ", v.r); + s << indent << "g: "; + Printer::stream(s, indent + " ", v.g); + s << indent << "b: "; + Printer::stream(s, indent + " ", v.b); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_LED_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LowCmd.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LowCmd.h new file mode 100644 index 0000000000000000000000000000000000000000..32a533a1be3cc4e24dfbd8e736449d5dcd448055 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LowCmd.h @@ -0,0 +1,352 @@ +// Generated by gencpp from file unitree_legged_msgs/LowCmd.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H +#define UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace unitree_legged_msgs +{ +template +struct LowCmd_ +{ + typedef LowCmd_ Type; + + LowCmd_() + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , motorCmd() + , bms() + , wirelessRemote() + , reserve(0) + , crc(0) { + head.assign(0); + + SN.assign(0); + + version.assign(0); + + wirelessRemote.assign(0); + } + LowCmd_(const ContainerAllocator& _alloc) + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , motorCmd() + , bms(_alloc) + , wirelessRemote() + , reserve(0) + , crc(0) { + (void)_alloc; + head.assign(0); + + SN.assign(0); + + version.assign(0); + + motorCmd.assign( ::unitree_legged_msgs::MotorCmd_ (_alloc)); + + wirelessRemote.assign(0); + } + + + + typedef boost::array _head_type; + _head_type head; + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint8_t _frameReserve_type; + _frameReserve_type frameReserve; + + typedef boost::array _SN_type; + _SN_type SN; + + typedef boost::array _version_type; + _version_type version; + + typedef uint16_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef boost::array< ::unitree_legged_msgs::MotorCmd_ , 20> _motorCmd_type; + _motorCmd_type motorCmd; + + typedef ::unitree_legged_msgs::BmsCmd_ _bms_type; + _bms_type bms; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef uint32_t _crc_type; + _crc_type crc; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd_ const> ConstPtr; + +}; // struct LowCmd_ + +typedef ::unitree_legged_msgs::LowCmd_ > LowCmd; + +typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd > LowCmdPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::LowCmd const> LowCmdConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowCmd_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::LowCmd_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::LowCmd_ & lhs, const ::unitree_legged_msgs::LowCmd_ & rhs) +{ + return lhs.head == rhs.head && + lhs.levelFlag == rhs.levelFlag && + lhs.frameReserve == rhs.frameReserve && + lhs.SN == rhs.SN && + lhs.version == rhs.version && + lhs.bandWidth == rhs.bandWidth && + lhs.motorCmd == rhs.motorCmd && + lhs.bms == rhs.bms && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc; +} + +template +bool operator!=(const ::unitree_legged_msgs::LowCmd_ & lhs, const ::unitree_legged_msgs::LowCmd_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::LowCmd_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LowCmd_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LowCmd_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LowCmd_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowCmd_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowCmd_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::LowCmd_ > +{ + static const char* value() + { + return "7a33b01032a894134df9b65b5cd356d1"; + } + + static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } + static const uint64_t static_value1 = 0x7a33b01032a89413ULL; + static const uint64_t static_value2 = 0x4df9b65b5cd356d1ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::LowCmd_ > +{ + static const char* value() + { + return "unitree_legged_msgs/LowCmd"; + } + + static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::LowCmd_ > +{ + static const char* value() + { + return "\n" +"uint8[2] head\n" +"uint8 levelFlag\n" +"uint8 frameReserve\n" +"\n" +"uint32[2] SN\n" +"uint32[2] version\n" +"uint16 bandWidth\n" +"MotorCmd[20] motorCmd\n" +"BmsCmd bms\n" +"uint8[40] wirelessRemote\n" +"uint32 reserve\n" +"\n" +"uint32 crc\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/MotorCmd\n" +"uint8 mode # motor target mode\n" +"float32 q # motor target position\n" +"float32 dq # motor target velocity\n" +"float32 tau # motor target torque\n" +"float32 Kp # motor spring stiffness coefficient\n" +"float32 Kd # motor damper coefficient\n" +"uint32[3] reserve # motor target torque\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/BmsCmd\n" +"uint8 off # off 0xA5\n" +"uint8[3] reserve\n" +; + } + + static const char* value(const ::unitree_legged_msgs::LowCmd_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::LowCmd_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.head); + stream.next(m.levelFlag); + stream.next(m.frameReserve); + stream.next(m.SN); + stream.next(m.version); + stream.next(m.bandWidth); + stream.next(m.motorCmd); + stream.next(m.bms); + stream.next(m.wirelessRemote); + stream.next(m.reserve); + stream.next(m.crc); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LowCmd_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::LowCmd_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowCmd_& v) + { + s << indent << "head[]" << std::endl; + for (size_t i = 0; i < v.head.size(); ++i) + { + s << indent << " head[" << i << "]: "; + Printer::stream(s, indent + " ", v.head[i]); + } + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "frameReserve: "; + Printer::stream(s, indent + " ", v.frameReserve); + s << indent << "SN[]" << std::endl; + for (size_t i = 0; i < v.SN.size(); ++i) + { + s << indent << " SN[" << i << "]: "; + Printer::stream(s, indent + " ", v.SN[i]); + } + s << indent << "version[]" << std::endl; + for (size_t i = 0; i < v.version.size(); ++i) + { + s << indent << " version[" << i << "]: "; + Printer::stream(s, indent + " ", v.version[i]); + } + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "motorCmd[]" << std::endl; + for (size_t i = 0; i < v.motorCmd.size(); ++i) + { + s << indent << " motorCmd[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::MotorCmd_ >::stream(s, indent + " ", v.motorCmd[i]); + } + s << indent << "bms: "; + s << std::endl; + Printer< ::unitree_legged_msgs::BmsCmd_ >::stream(s, indent + " ", v.bms); + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWCMD_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LowState.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LowState.h new file mode 100644 index 0000000000000000000000000000000000000000..5033aadd5972361c2ca1c9c7899083c7be37aca7 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/LowState.h @@ -0,0 +1,509 @@ +// Generated by gencpp from file unitree_legged_msgs/LowState.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H +#define UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace unitree_legged_msgs +{ +template +struct LowState_ +{ + typedef LowState_ Type; + + LowState_() + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , imu() + , motorState() + , bms() + , footForce() + , footForceEst() + , tick(0) + , wirelessRemote() + , reserve(0) + , crc(0) + , eeForceRaw() + , eeForce() + , position() + , velocity() + , velocity_w() { + head.assign(0); + + SN.assign(0); + + version.assign(0); + + footForce.assign(0); + + footForceEst.assign(0); + + wirelessRemote.assign(0); + } + LowState_(const ContainerAllocator& _alloc) + : head() + , levelFlag(0) + , frameReserve(0) + , SN() + , version() + , bandWidth(0) + , imu(_alloc) + , motorState() + , bms(_alloc) + , footForce() + , footForceEst() + , tick(0) + , wirelessRemote() + , reserve(0) + , crc(0) + , eeForceRaw() + , eeForce() + , position(_alloc) + , velocity(_alloc) + , velocity_w(_alloc) { + (void)_alloc; + head.assign(0); + + SN.assign(0); + + version.assign(0); + + motorState.assign( ::unitree_legged_msgs::MotorState_ (_alloc)); + + footForce.assign(0); + + footForceEst.assign(0); + + wirelessRemote.assign(0); + + eeForceRaw.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + + eeForce.assign( ::unitree_legged_msgs::Cartesian_ (_alloc)); + } + + + + typedef boost::array _head_type; + _head_type head; + + typedef uint8_t _levelFlag_type; + _levelFlag_type levelFlag; + + typedef uint8_t _frameReserve_type; + _frameReserve_type frameReserve; + + typedef boost::array _SN_type; + _SN_type SN; + + typedef boost::array _version_type; + _version_type version; + + typedef uint16_t _bandWidth_type; + _bandWidth_type bandWidth; + + typedef ::unitree_legged_msgs::IMU_ _imu_type; + _imu_type imu; + + typedef boost::array< ::unitree_legged_msgs::MotorState_ , 20> _motorState_type; + _motorState_type motorState; + + typedef ::unitree_legged_msgs::BmsState_ _bms_type; + _bms_type bms; + + typedef boost::array _footForce_type; + _footForce_type footForce; + + typedef boost::array _footForceEst_type; + _footForceEst_type footForceEst; + + typedef uint32_t _tick_type; + _tick_type tick; + + typedef boost::array _wirelessRemote_type; + _wirelessRemote_type wirelessRemote; + + typedef uint32_t _reserve_type; + _reserve_type reserve; + + typedef uint32_t _crc_type; + _crc_type crc; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _eeForceRaw_type; + _eeForceRaw_type eeForceRaw; + + typedef boost::array< ::unitree_legged_msgs::Cartesian_ , 4> _eeForce_type; + _eeForce_type eeForce; + + typedef ::unitree_legged_msgs::Cartesian_ _position_type; + _position_type position; + + typedef ::unitree_legged_msgs::Cartesian_ _velocity_type; + _velocity_type velocity; + + typedef ::unitree_legged_msgs::Cartesian_ _velocity_w_type; + _velocity_w_type velocity_w; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::LowState_ const> ConstPtr; + +}; // struct LowState_ + +typedef ::unitree_legged_msgs::LowState_ > LowState; + +typedef boost::shared_ptr< ::unitree_legged_msgs::LowState > LowStatePtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::LowState const> LowStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::LowState_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::LowState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::LowState_ & lhs, const ::unitree_legged_msgs::LowState_ & rhs) +{ + return lhs.head == rhs.head && + lhs.levelFlag == rhs.levelFlag && + lhs.frameReserve == rhs.frameReserve && + lhs.SN == rhs.SN && + lhs.version == rhs.version && + lhs.bandWidth == rhs.bandWidth && + lhs.imu == rhs.imu && + lhs.motorState == rhs.motorState && + lhs.bms == rhs.bms && + lhs.footForce == rhs.footForce && + lhs.footForceEst == rhs.footForceEst && + lhs.tick == rhs.tick && + lhs.wirelessRemote == rhs.wirelessRemote && + lhs.reserve == rhs.reserve && + lhs.crc == rhs.crc && + lhs.eeForceRaw == rhs.eeForceRaw && + lhs.eeForce == rhs.eeForce && + lhs.position == rhs.position && + lhs.velocity == rhs.velocity && + lhs.velocity_w == rhs.velocity_w; +} + +template +bool operator!=(const ::unitree_legged_msgs::LowState_ & lhs, const ::unitree_legged_msgs::LowState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::LowState_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::LowState_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LowState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::LowState_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowState_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::LowState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::LowState_ > +{ + static const char* value() + { + return "481f8a83f729a25e7cc170da192f80f9"; + } + + static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } + static const uint64_t static_value1 = 0x481f8a83f729a25eULL; + static const uint64_t static_value2 = 0x7cc170da192f80f9ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::LowState_ > +{ + static const char* value() + { + return "unitree_legged_msgs/LowState"; + } + + static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::LowState_ > +{ + static const char* value() + { + return "\n" +"uint8[2] head\n" +"uint8 levelFlag\n" +"uint8 frameReserve\n" +"\n" +"uint32[2] SN\n" +"uint32[2] version\n" +"uint16 bandWidth\n" +"IMU imu\n" +"MotorState[20] motorState\n" +"BmsState bms\n" +"int16[4] footForce \n" +"int16[4] footForceEst \n" +"uint32 tick \n" +"uint8[40] wirelessRemote \n" +"uint32 reserve\n" +"uint32 crc\n" +"\n" +"# Old version Aliengo does not have:\n" +"Cartesian[4] eeForceRaw\n" +"Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization \n" +"Cartesian position # will delete\n" +"Cartesian velocity # will delete\n" +"Cartesian velocity_w # will delete\n" +"\n" +"\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/IMU\n" +"float32[4] quaternion\n" +"float32[3] gyroscope\n" +"float32[3] accelerometer\n" +"float32[3] rpy\n" +"int8 temperature\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/MotorState\n" +"uint8 mode # motor current mode \n" +"float32 q # motor current position（rad）\n" +"float32 dq # motor current speed（rad/s）\n" +"float32 ddq # motor current speed（rad/s）\n" +"float32 tauEst # current estimated output torque（N*m）\n" +"float32 q_raw # motor current position（rad）\n" +"float32 dq_raw # motor current speed（rad/s）\n" +"float32 ddq_raw # motor current speed（rad/s）\n" +"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n" +"uint32[2] reserve\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/BmsState\n" +"uint8 version_h\n" +"uint8 version_l\n" +"uint8 bms_status\n" +"uint8 SOC # SOC 0-100%\n" +"int32 current # mA\n" +"uint16 cycle\n" +"int8[2] BQ_NTC # x1 degrees centigrade\n" +"int8[2] MCU_NTC # x1 degrees centigrade\n" +"uint16[10] cell_vol # cell voltage mV\n" +"================================================================================\n" +"MSG: unitree_legged_msgs/Cartesian\n" +"float32 x\n" +"float32 y\n" +"float32 z\n" +; + } + + static const char* value(const ::unitree_legged_msgs::LowState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::LowState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.head); + stream.next(m.levelFlag); + stream.next(m.frameReserve); + stream.next(m.SN); + stream.next(m.version); + stream.next(m.bandWidth); + stream.next(m.imu); + stream.next(m.motorState); + stream.next(m.bms); + stream.next(m.footForce); + stream.next(m.footForceEst); + stream.next(m.tick); + stream.next(m.wirelessRemote); + stream.next(m.reserve); + stream.next(m.crc); + stream.next(m.eeForceRaw); + stream.next(m.eeForce); + stream.next(m.position); + stream.next(m.velocity); + stream.next(m.velocity_w); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct LowState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::LowState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::LowState_& v) + { + s << indent << "head[]" << std::endl; + for (size_t i = 0; i < v.head.size(); ++i) + { + s << indent << " head[" << i << "]: "; + Printer::stream(s, indent + " ", v.head[i]); + } + s << indent << "levelFlag: "; + Printer::stream(s, indent + " ", v.levelFlag); + s << indent << "frameReserve: "; + Printer::stream(s, indent + " ", v.frameReserve); + s << indent << "SN[]" << std::endl; + for (size_t i = 0; i < v.SN.size(); ++i) + { + s << indent << " SN[" << i << "]: "; + Printer::stream(s, indent + " ", v.SN[i]); + } + s << indent << "version[]" << std::endl; + for (size_t i = 0; i < v.version.size(); ++i) + { + s << indent << " version[" << i << "]: "; + Printer::stream(s, indent + " ", v.version[i]); + } + s << indent << "bandWidth: "; + Printer::stream(s, indent + " ", v.bandWidth); + s << indent << "imu: "; + s << std::endl; + Printer< ::unitree_legged_msgs::IMU_ >::stream(s, indent + " ", v.imu); + s << indent << "motorState[]" << std::endl; + for (size_t i = 0; i < v.motorState.size(); ++i) + { + s << indent << " motorState[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::MotorState_ >::stream(s, indent + " ", v.motorState[i]); + } + s << indent << "bms: "; + s << std::endl; + Printer< ::unitree_legged_msgs::BmsState_ >::stream(s, indent + " ", v.bms); + s << indent << "footForce[]" << std::endl; + for (size_t i = 0; i < v.footForce.size(); ++i) + { + s << indent << " footForce[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForce[i]); + } + s << indent << "footForceEst[]" << std::endl; + for (size_t i = 0; i < v.footForceEst.size(); ++i) + { + s << indent << " footForceEst[" << i << "]: "; + Printer::stream(s, indent + " ", v.footForceEst[i]); + } + s << indent << "tick: "; + Printer::stream(s, indent + " ", v.tick); + s << indent << "wirelessRemote[]" << std::endl; + for (size_t i = 0; i < v.wirelessRemote.size(); ++i) + { + s << indent << " wirelessRemote[" << i << "]: "; + Printer::stream(s, indent + " ", v.wirelessRemote[i]); + } + s << indent << "reserve: "; + Printer::stream(s, indent + " ", v.reserve); + s << indent << "crc: "; + Printer::stream(s, indent + " ", v.crc); + s << indent << "eeForceRaw[]" << std::endl; + for (size_t i = 0; i < v.eeForceRaw.size(); ++i) + { + s << indent << " eeForceRaw[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.eeForceRaw[i]); + } + s << indent << "eeForce[]" << std::endl; + for (size_t i = 0; i < v.eeForce.size(); ++i) + { + s << indent << " eeForce[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.eeForce[i]); + } + s << indent << "position: "; + s << std::endl; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.position); + s << indent << "velocity: "; + s << std::endl; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.velocity); + s << indent << "velocity_w: "; + s << std::endl; + Printer< ::unitree_legged_msgs::Cartesian_ >::stream(s, indent + " ", v.velocity_w); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_LOWSTATE_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/MotorCmd.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/MotorCmd.h new file mode 100644 index 0000000000000000000000000000000000000000..e89f5754e286fc7d35fe15064ac1037f34e7bb2e --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/MotorCmd.h @@ -0,0 +1,261 @@ +// Generated by gencpp from file unitree_legged_msgs/MotorCmd.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H +#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct MotorCmd_ +{ + typedef MotorCmd_ Type; + + MotorCmd_() + : mode(0) + , q(0.0) + , dq(0.0) + , tau(0.0) + , Kp(0.0) + , Kd(0.0) + , reserve() { + reserve.assign(0); + } + MotorCmd_(const ContainerAllocator& _alloc) + : mode(0) + , q(0.0) + , dq(0.0) + , tau(0.0) + , Kp(0.0) + , Kd(0.0) + , reserve() { + (void)_alloc; + reserve.assign(0); + } + + + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef float _q_type; + _q_type q; + + typedef float _dq_type; + _dq_type dq; + + typedef float _tau_type; + _tau_type tau; + + typedef float _Kp_type; + _Kp_type Kp; + + typedef float _Kd_type; + _Kd_type Kd; + + typedef boost::array _reserve_type; + _reserve_type reserve; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd_ const> ConstPtr; + +}; // struct MotorCmd_ + +typedef ::unitree_legged_msgs::MotorCmd_ > MotorCmd; + +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd > MotorCmdPtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorCmd const> MotorCmdConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorCmd_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::MotorCmd_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::MotorCmd_ & lhs, const ::unitree_legged_msgs::MotorCmd_ & rhs) +{ + return lhs.mode == rhs.mode && + lhs.q == rhs.q && + lhs.dq == rhs.dq && + lhs.tau == rhs.tau && + lhs.Kp == rhs.Kp && + lhs.Kd == rhs.Kd && + lhs.reserve == rhs.reserve; +} + +template +bool operator!=(const ::unitree_legged_msgs::MotorCmd_ & lhs, const ::unitree_legged_msgs::MotorCmd_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::MotorCmd_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::MotorCmd_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorCmd_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorCmd_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorCmd_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::MotorCmd_ > +{ + static const char* value() + { + return "bbb3b7d91319c3a1b99055f0149ba221"; + } + + static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } + static const uint64_t static_value1 = 0xbbb3b7d91319c3a1ULL; + static const uint64_t static_value2 = 0xb99055f0149ba221ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::MotorCmd_ > +{ + static const char* value() + { + return "unitree_legged_msgs/MotorCmd"; + } + + static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::MotorCmd_ > +{ + static const char* value() + { + return "uint8 mode # motor target mode\n" +"float32 q # motor target position\n" +"float32 dq # motor target velocity\n" +"float32 tau # motor target torque\n" +"float32 Kp # motor spring stiffness coefficient\n" +"float32 Kd # motor damper coefficient\n" +"uint32[3] reserve # motor target torque\n" +; + } + + static const char* value(const ::unitree_legged_msgs::MotorCmd_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::MotorCmd_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.mode); + stream.next(m.q); + stream.next(m.dq); + stream.next(m.tau); + stream.next(m.Kp); + stream.next(m.Kd); + stream.next(m.reserve); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MotorCmd_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::MotorCmd_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorCmd_& v) + { + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "q: "; + Printer::stream(s, indent + " ", v.q); + s << indent << "dq: "; + Printer::stream(s, indent + " ", v.dq); + s << indent << "tau: "; + Printer::stream(s, indent + " ", v.tau); + s << indent << "Kp: "; + Printer::stream(s, indent + " ", v.Kp); + s << indent << "Kd: "; + Printer::stream(s, indent + " ", v.Kd); + s << indent << "reserve[]" << std::endl; + for (size_t i = 0; i < v.reserve.size(); ++i) + { + s << indent << " reserve[" << i << "]: "; + Printer::stream(s, indent + " ", v.reserve[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORCMD_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/MotorState.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/MotorState.h new file mode 100644 index 0000000000000000000000000000000000000000..788a7f7b5f70189980c0b550bc153e1d5653de10 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_msgs/MotorState.h @@ -0,0 +1,291 @@ +// Generated by gencpp from file unitree_legged_msgs/MotorState.msg +// DO NOT EDIT! + + +#ifndef UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H +#define UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace unitree_legged_msgs +{ +template +struct MotorState_ +{ + typedef MotorState_ Type; + + MotorState_() + : mode(0) + , q(0.0) + , dq(0.0) + , ddq(0.0) + , tauEst(0.0) + , q_raw(0.0) + , dq_raw(0.0) + , ddq_raw(0.0) + , temperature(0) + , reserve() { + reserve.assign(0); + } + MotorState_(const ContainerAllocator& _alloc) + : mode(0) + , q(0.0) + , dq(0.0) + , ddq(0.0) + , tauEst(0.0) + , q_raw(0.0) + , dq_raw(0.0) + , ddq_raw(0.0) + , temperature(0) + , reserve() { + (void)_alloc; + reserve.assign(0); + } + + + + typedef uint8_t _mode_type; + _mode_type mode; + + typedef float _q_type; + _q_type q; + + typedef float _dq_type; + _dq_type dq; + + typedef float _ddq_type; + _ddq_type ddq; + + typedef float _tauEst_type; + _tauEst_type tauEst; + + typedef float _q_raw_type; + _q_raw_type q_raw; + + typedef float _dq_raw_type; + _dq_raw_type dq_raw; + + typedef float _ddq_raw_type; + _ddq_raw_type ddq_raw; + + typedef int8_t _temperature_type; + _temperature_type temperature; + + typedef boost::array _reserve_type; + _reserve_type reserve; + + + + + + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_ > Ptr; + typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState_ const> ConstPtr; + +}; // struct MotorState_ + +typedef ::unitree_legged_msgs::MotorState_ > MotorState; + +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState > MotorStatePtr; +typedef boost::shared_ptr< ::unitree_legged_msgs::MotorState const> MotorStateConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::unitree_legged_msgs::MotorState_ & v) +{ +ros::message_operations::Printer< ::unitree_legged_msgs::MotorState_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::unitree_legged_msgs::MotorState_ & lhs, const ::unitree_legged_msgs::MotorState_ & rhs) +{ + return lhs.mode == rhs.mode && + lhs.q == rhs.q && + lhs.dq == rhs.dq && + lhs.ddq == rhs.ddq && + lhs.tauEst == rhs.tauEst && + lhs.q_raw == rhs.q_raw && + lhs.dq_raw == rhs.dq_raw && + lhs.ddq_raw == rhs.ddq_raw && + lhs.temperature == rhs.temperature && + lhs.reserve == rhs.reserve; +} + +template +bool operator!=(const ::unitree_legged_msgs::MotorState_ & lhs, const ::unitree_legged_msgs::MotorState_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace unitree_legged_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::unitree_legged_msgs::MotorState_ > + : TrueType + { }; + +template +struct IsMessage< ::unitree_legged_msgs::MotorState_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorState_ > + : TrueType + { }; + +template +struct IsFixedSize< ::unitree_legged_msgs::MotorState_ const> + : TrueType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorState_ > + : FalseType + { }; + +template +struct HasHeader< ::unitree_legged_msgs::MotorState_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::unitree_legged_msgs::MotorState_ > +{ + static const char* value() + { + return "94c55ee3b7852be2bd437b20ce12a254"; + } + + static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } + static const uint64_t static_value1 = 0x94c55ee3b7852be2ULL; + static const uint64_t static_value2 = 0xbd437b20ce12a254ULL; +}; + +template +struct DataType< ::unitree_legged_msgs::MotorState_ > +{ + static const char* value() + { + return "unitree_legged_msgs/MotorState"; + } + + static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } +}; + +template +struct Definition< ::unitree_legged_msgs::MotorState_ > +{ + static const char* value() + { + return "uint8 mode # motor current mode \n" +"float32 q # motor current position（rad）\n" +"float32 dq # motor current speed（rad/s）\n" +"float32 ddq # motor current speed（rad/s）\n" +"float32 tauEst # current estimated output torque（N*m）\n" +"float32 q_raw # motor current position（rad）\n" +"float32 dq_raw # motor current speed（rad/s）\n" +"float32 ddq_raw # motor current speed（rad/s）\n" +"int8 temperature # motor temperature（slow conduction of temperature leads to lag）\n" +"uint32[2] reserve\n" +; + } + + static const char* value(const ::unitree_legged_msgs::MotorState_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::unitree_legged_msgs::MotorState_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.mode); + stream.next(m.q); + stream.next(m.dq); + stream.next(m.ddq); + stream.next(m.tauEst); + stream.next(m.q_raw); + stream.next(m.dq_raw); + stream.next(m.ddq_raw); + stream.next(m.temperature); + stream.next(m.reserve); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MotorState_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::unitree_legged_msgs::MotorState_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::unitree_legged_msgs::MotorState_& v) + { + s << indent << "mode: "; + Printer::stream(s, indent + " ", v.mode); + s << indent << "q: "; + Printer::stream(s, indent + " ", v.q); + s << indent << "dq: "; + Printer::stream(s, indent + " ", v.dq); + s << indent << "ddq: "; + Printer::stream(s, indent + " ", v.ddq); + s << indent << "tauEst: "; + Printer::stream(s, indent + " ", v.tauEst); + s << indent << "q_raw: "; + Printer::stream(s, indent + " ", v.q_raw); + s << indent << "dq_raw: "; + Printer::stream(s, indent + " ", v.dq_raw); + s << indent << "ddq_raw: "; + Printer::stream(s, indent + " ", v.ddq_raw); + s << indent << "temperature: "; + Printer::stream(s, indent + " ", v.temperature); + s << indent << "reserve[]" << std::endl; + for (size_t i = 0; i < v.reserve.size(); ++i) + { + s << indent << " reserve[" << i << "]: "; + Printer::stream(s, indent + " ", v.reserve[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // UNITREE_LEGGED_MSGS_MESSAGE_MOTORSTATE_H diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/a1_const.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/a1_const.h new file mode 100644 index 0000000000000000000000000000000000000000..a588d0417a2876097b83aa01259759196f5d72e6 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/a1_const.h @@ -0,0 +1,19 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_A1_H_ +#define _UNITREE_LEGGED_A1_H_ + +namespace UNITREE_LEGGED_SDK +{ + constexpr double a1_Hip_max = 0.802; // unit:radian ( = 46 degree) + constexpr double a1_Hip_min = -0.802; // unit:radian ( = -46 degree) + constexpr double a1_Thigh_max = 4.19; // unit:radian ( = 240 degree) + constexpr double a1_Thigh_min = -1.05; // unit:radian ( = -60 degree) + constexpr double a1_Calf_max = -0.916; // unit:radian ( = -52.5 degree) + constexpr double a1_Calf_min = -2.7; // unit:radian ( = -154.5 degree) +} + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/aliengo_const.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/aliengo_const.h new file mode 100644 index 0000000000000000000000000000000000000000..362c78d8a2f5cddf16459a5b45fc0cde5a801b21 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/aliengo_const.h @@ -0,0 +1,19 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_ALIENGO_H_ +#define _UNITREE_LEGGED_ALIENGO_H_ + +namespace UNITREE_LEGGED_SDK +{ + constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) + constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) + constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) + constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) + constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) + constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) +} + +#endif \ No newline at end of file diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/comm.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/comm.h new file mode 100644 index 0000000000000000000000000000000000000000..66577310aa46f03ae6652389929b7d0a67841714 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/comm.h @@ -0,0 +1,168 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_COMM_H_ +#define _UNITREE_LEGGED_COMM_H_ + +#include +#include + +namespace UNITREE_LEGGED_SDK +{ + + constexpr int HIGHLEVEL = 0x00; + constexpr int LOWLEVEL = 0xff; + constexpr double PosStopF = (2.146E+9f); + constexpr double VelStopF = (16000.0f); + +#pragma pack(1) + + typedef struct + { + float x; + float y; + float z; + } Cartesian; + + typedef struct + { + std::array quaternion; // quaternion, normalized, (w,x,y,z) + std::array gyroscope; // angular velocity （unit: rad/s) + std::array accelerometer; // m/(s2) + std::array rpy; // euler angle（unit: rad) + int8_t temperature; + } IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift. + + typedef struct + { + uint8_t r; + uint8_t g; + uint8_t b; + } LED; // foot led brightness: 0~255 + + typedef struct + { + uint8_t mode; // motor working mode + float q; // current angle (unit: radian) + float dq; // current velocity (unit: radian/second) + float ddq; // current acc (unit: radian/second*second) + float tauEst; // current estimated output torque (unit: N.m) + float q_raw; // current angle (unit: radian) + float dq_raw; // current velocity (unit: radian/second) + float ddq_raw; + int8_t temperature; // current temperature (temperature conduction is slow that leads to lag) + std::array reserve; + } MotorState; // motor feedback + + typedef struct + { + uint8_t mode; // desired working mode + float q; // desired angle (unit: radian) + float dq; // desired velocity (unit: radian/second) + float tau; // desired output torque (unit: N.m) + float Kp; // desired position stiffness (unit: N.m/rad ) + float Kd; // desired velocity stiffness (unit: N.m/(rad/s) ) + std::array reserve; + } MotorCmd; // motor control + + typedef struct + { + uint8_t levelFlag; // flag to distinguish high level or low level + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + IMU imu; + std::array motorState; + std::array footForce; // force sensors + std::array footForceEst; // force sensors + uint32_t tick; // reference real-time from motion controller (unit: us) + std::array wirelessRemote; // wireless commands + uint32_t reserve; + uint32_t crc; + } LowState; // low level feedback + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + std::array motorCmd; + std::array led; + std::array wirelessRemote; + uint32_t reserve; + uint32_t crc; + } LowCmd; // low level control + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + uint8_t mode; + IMU imu; + float forwardSpeed; + float sideSpeed; + float rotateSpeed; + float bodyHeight; + float updownSpeed; // speed of stand up or squat down + float forwardPosition; // front or rear displacement, an integrated number form kinematics function, usually drift + float sidePosition; // left or right displacement, an integrated number form kinematics function, usually drift + std::array footPosition2Body; // foot position relative to body + std::array footSpeed2Body; // foot speed relative to body + std::array footForce; + std::array footForceEst; + uint32_t tick; // reference real-time from motion controller (unit: us) + std::array wirelessRemote; + uint32_t reserve; + uint32_t crc; + } HighState; // high level feedback + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + uint8_t mode; // 0:idle, default stand 1:forced stand 2:walk continuously + float forwardSpeed; // speed of move forward or backward, scale: -1~1 + float sideSpeed; // speed of move left or right, scale: -1~1 + float rotateSpeed; // speed of spin left or right, scale: -1~1 + float bodyHeight; // body height, scale: -1~1 + float footRaiseHeight; // foot up height while walking (unavailable now) + float yaw; // unit: radian, scale: -1~1 + float pitch; // unit: radian, scale: -1~1 + float roll; // unit: radian, scale: -1~1 + std::array led; + std::array wirelessRemote; + std::array AppRemote; + uint32_t reserve; + uint32_t crc; + } HighCmd; // high level control + +#pragma pack() + + typedef struct + { + unsigned long long TotalCount; // total loop count + unsigned long long SendCount; // total send count + unsigned long long RecvCount; // total receive count + unsigned long long SendError; // total send error + unsigned long long FlagError; // total flag error + unsigned long long RecvCRCError; // total reveive CRC error + unsigned long long RecvLoseError; // total lose package count + } UDPState; // UDP communication state + + constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd)); + constexpr int HIGH_STATE_LENGTH = (sizeof(HighState)); + +} + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/lcm.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/lcm.h new file mode 100644 index 0000000000000000000000000000000000000000..957b7e66a57c468921e981daa407e2d765e89742 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/lcm.h @@ -0,0 +1,82 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_LCM_H_ +#define _UNITREE_LEGGED_LCM_H_ + +#include "comm.h" +#include +#include + +namespace UNITREE_LEGGED_SDK +{ + + constexpr char highCmdChannel[] = "LCM_High_Cmd"; + constexpr char highStateChannel[] = "LCM_High_State"; + constexpr char lowCmdChannel[] = "LCM_Low_Cmd"; + constexpr char lowStateChannel[] = "LCM_Low_State"; + + template + class LCMHandler + { + public: + LCMHandler(){ + pthread_mutex_init(&countMut, NULL); + pthread_mutex_init(&recvMut, NULL); + } + + void onMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel){ + isrunning = true; + + pthread_mutex_lock(&countMut); + counter = 0; + pthread_mutex_unlock(&countMut); + + T *msg = NULL; + msg = (T *)(rbuf->data); + pthread_mutex_lock(&recvMut); + // sourceBuf = *msg; + memcpy(&sourceBuf, msg, sizeof(T)); + pthread_mutex_unlock(&recvMut); + } + + bool isrunning = false; + T sourceBuf = {0}; + pthread_mutex_t countMut; + pthread_mutex_t recvMut; + int counter = 0; + }; + + class LCM { + public: + LCM(uint8_t level); + ~LCM(); + void SubscribeCmd(); + void SubscribeState(); // remember to call this when change control level + int Send(HighCmd&); // lcm send cmd + int Send(LowCmd&); // lcm send cmd + int Send(HighState&); // lcm send state + int Send(LowState&); // lcm send state + int Recv(); // directly save in buffer + void Get(HighCmd&); + void Get(LowCmd&); + void Get(HighState&); + void Get(LowState&); + + LCMHandler highStateLCMHandler; + LCMHandler lowStateLCMHandler; + LCMHandler highCmdLCMHandler; + LCMHandler lowCmdLCMHandler; + private: + uint8_t levelFlag = HIGHLEVEL; // default: high level + lcm::LCM lcm; + lcm::Subscription* subLcm; + int lcmFd; + int LCM_PERIOD = 2000; //default 2ms + }; + +} + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/lcm_server.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/lcm_server.h new file mode 100644 index 0000000000000000000000000000000000000000..d9bd66277ad6f627ee6cde5d7326cf7283a35203 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/lcm_server.h @@ -0,0 +1,106 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_LCM_SERVER_ +#define _UNITREE_LEGGED_LCM_SERVER_ + +#include "comm.h" +#include "unitree_legged_sdk/unitree_legged_sdk.h" + +namespace UNITREE_LEGGED_SDK +{ +// Low command Lcm Server +class Lcm_Server_Low +{ +public: + Lcm_Server_Low(LeggedType rname) : udp(LOWLEVEL), mylcm(LOWLEVEL){ + udp.InitCmdData(cmd); + } + void UDPRecv(){ + udp.Recv(); + } + void UDPSend(){ + udp.Send(); + } + void LCMRecv(); + void RobotControl(); + + UDP udp; + LCM mylcm; + LowCmd cmd = {0}; + LowState state = {0}; +}; +void Lcm_Server_Low::LCMRecv() +{ + if(mylcm.lowCmdLCMHandler.isrunning){ + pthread_mutex_lock(&mylcm.lowCmdLCMHandler.countMut); + mylcm.lowCmdLCMHandler.counter++; + if(mylcm.lowCmdLCMHandler.counter > 10){ + printf("Error! LCM Time out.\n"); + exit(-1); // can be commented out + } + pthread_mutex_unlock(&mylcm.lowCmdLCMHandler.countMut); + } + mylcm.Recv(); +} +void Lcm_Server_Low::RobotControl() +{ + udp.GetRecv(state); + mylcm.Send(state); + mylcm.Get(cmd); + udp.SetSend(cmd); +} + + + +// High command Lcm Server +class Lcm_Server_High +{ +public: + Lcm_Server_High(LeggedType rname): udp(HIGHLEVEL), mylcm(HIGHLEVEL){ + udp.InitCmdData(cmd); + } + void UDPRecv(){ + udp.Recv(); + } + void UDPSend(){ + udp.Send(); + } + void LCMRecv(); + void RobotControl(); + + UDP udp; + LCM mylcm; + HighCmd cmd = {0}; + HighState state = {0}; +}; + +void Lcm_Server_High::LCMRecv() +{ + if(mylcm.highCmdLCMHandler.isrunning){ + pthread_mutex_lock(&mylcm.highCmdLCMHandler.countMut); + mylcm.highCmdLCMHandler.counter++; + if(mylcm.highCmdLCMHandler.counter > 10){ + printf("Error! LCM Time out.\n"); + exit(-1); // can be commented out + } + pthread_mutex_unlock(&mylcm.highCmdLCMHandler.countMut); + } + mylcm.Recv(); +} + +void Lcm_Server_High::RobotControl() +{ + udp.GetRecv(state); + mylcm.Send(state); + mylcm.Get(cmd); + udp.SetSend(cmd); +} + + + + +} +#endif \ No newline at end of file diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/loop.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/loop.h new file mode 100644 index 0000000000000000000000000000000000000000..c58d67b0dc378439bdeaa18a7c88f5803d5d9cbf --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/loop.h @@ -0,0 +1,58 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_LOOP_H_ +#define _UNITREE_LEGGED_LOOP_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace UNITREE_LEGGED_SDK +{ + +constexpr int THREAD_PRIORITY = 99; // real-time priority + +typedef boost::function Callback; + +class Loop { +public: + Loop(std::string name, float period, int bindCPU = -1):_name(name), _period(period), _bindCPU(bindCPU) {} + ~Loop(); + void start(); + void shutdown(); + virtual void functionCB() = 0; + +private: + void entryFunc(); + + std::string _name; + float _period; + int _bindCPU; + bool _bind_cpu_flag = false; + bool _isrunning = false; + std::thread _thread; +}; + +class LoopFunc : public Loop { +public: + LoopFunc(std::string name, float period, const Callback& _cb) + : Loop(name, period), _fp(_cb){} + LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) + : Loop(name, period, bindCPU), _fp(_cb){} + void functionCB() { (_fp)(); } +private: + boost::function _fp; +}; + + +} + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/quadruped.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/quadruped.h new file mode 100644 index 0000000000000000000000000000000000000000..0c23f6215d99efbc613d51283ea8150a6db3256e --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/quadruped.h @@ -0,0 +1,48 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_QUADRUPED_H_ +#define _UNITREE_LEGGED_QUADRUPED_H_ + +namespace UNITREE_LEGGED_SDK +{ + +enum class LeggedType { + Aliengo, + A1 +}; + +enum class HighLevelType { + Basic, + Sport +}; + +void InitEnvironment(); // memory lock + +// definition of each leg and joint +constexpr int FR_ = 0; // leg index +constexpr int FL_ = 1; +constexpr int RR_ = 2; +constexpr int RL_ = 3; + +constexpr int FR_0 = 0; // joint index +constexpr int FR_1 = 1; +constexpr int FR_2 = 2; + +constexpr int FL_0 = 3; +constexpr int FL_1 = 4; +constexpr int FL_2 = 5; + +constexpr int RR_0 = 6; +constexpr int RR_1 = 7; +constexpr int RR_2 = 8; + +constexpr int RL_0 = 9; +constexpr int RL_1 = 10; +constexpr int RL_2 = 11; + +} + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/safety.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/safety.h new file mode 100644 index 0000000000000000000000000000000000000000..0013f61e458c2fe584082cdc8dc4bd11225b0102 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/safety.h @@ -0,0 +1,32 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_SAFETY_H_ +#define _UNITREE_LEGGED_SAFETY_H_ + +#include "comm.h" +#include "quadruped.h" + +namespace UNITREE_LEGGED_SDK +{ + +class Safety{ +public: + Safety(LeggedType type); + ~Safety(); + void PositionLimit(LowCmd&); // only effect under Low Level control in Position mode + void PowerProtect(LowCmd&, LowState&, int); /* only effect under Low Level control, input factor: 1~10, + means 10%~100% power limit. If you are new, then use 1; if you are familiar, + then can try bigger number or even comment this function. */ + void PositionProtect(LowCmd&, LowState&, double limit = 0.087); // default limit is 5 degree +private: + int WattLimit, Wcount; // Watt. When limit to 100, you can triger it with 4 hands shaking. + double Hip_max, Hip_min, Thigh_max, Thigh_min, Calf_max, Calf_min; +}; + + +} + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/udp.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/udp.h new file mode 100644 index 0000000000000000000000000000000000000000..35d318cf648d9f2a2bf9f137b4320f9a7d1b3424 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/udp.h @@ -0,0 +1,64 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_UDP_H_ +#define _UNITREE_LEGGED_UDP_H_ + +#include "comm.h" +#include "unitree_legged_sdk/quadruped.h" +#include + +namespace UNITREE_LEGGED_SDK +{ + +constexpr int UDP_CLIENT_PORT = 8080; // local port +constexpr int UDP_SERVER_PORT = 8007; // target port +constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10"; // target IP address +constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161"; // target IP address + +// Notice: User defined data(like struct) should add crc(4Byte) at the end. +class UDP { +public: + UDP(uint8_t level, HighLevelType highControl = HighLevelType::Basic); // unitree dafault IP and Port + UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort, int sendLength, int recvLength); + UDP(uint16_t localPort, uint16_t targetPort, int sendLength, int recvLength); // as server, client IP can change + ~UDP(); + void InitCmdData(HighCmd& cmd); + void InitCmdData(LowCmd& cmd); + void switchLevel(int level); + + int SetSend(HighCmd&); + int SetSend(LowCmd&); + int SetSend(char* cmd); + void GetRecv(HighState&); + void GetRecv(LowState&); + void GetRecv(char*); + int Send(); + int Recv(); // directly save in buffer + + UDPState udpState; + char* targetIP; + uint16_t targetPort; + char* localIP; + uint16_t localPort; +private: + void init(uint16_t localPort, const char* targetIP, uint16_t targetPort); + + uint8_t levelFlag = HIGHLEVEL; // default: high level + int sockFd; + bool connected; // udp only works when connected + int sendLength; + int recvLength; + char* recvTemp; + char* recvBuf; + char* sendBuf; + int lose_recv; + pthread_mutex_t sendMut; + pthread_mutex_t recvMut; +}; + +} + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/unitree_legged_sdk.h b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/unitree_legged_sdk.h new file mode 100644 index 0000000000000000000000000000000000000000..a93a6c106b1fcf3ec8debc6da404eb668458930e --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/include/unitree_legged_sdk/unitree_legged_sdk.h @@ -0,0 +1,19 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#ifndef _UNITREE_LEGGED_SDK_H_ +#define _UNITREE_LEGGED_SDK_H_ + +#include "comm.h" +#include "safety.h" +#include "udp.h" +#include "loop.h" +#include "lcm.h" +#include "quadruped.h" +#include + +#define UT UNITREE_LEGGED_SDK //short name + +#endif diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_amd64.so b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_amd64.so new file mode 100644 index 0000000000000000000000000000000000000000..eb0ada03af1bbace17035d933c91d0384c4b4ed7 Binary files /dev/null and b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_amd64.so differ diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_arm32.so b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_arm32.so new file mode 100644 index 0000000000000000000000000000000000000000..40c5b0d626f7af92e23cb2df1f3fc051f198552a Binary files /dev/null and b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_arm32.so differ diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_arm64.so b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_arm64.so new file mode 100644 index 0000000000000000000000000000000000000000..e734e82efcec879700b9d348a8a2ef0f3134fe76 Binary files /dev/null and b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/lib/libunitree_legged_sdk_arm64.so differ diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/pybind11 b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/pybind11 new file mode 120000 index 0000000000000000000000000000000000000000..b122d771b985bdb6c4be42ea293ab6759462cc8d --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/pybind11 @@ -0,0 +1 @@ +../../../../utils/pybind11 \ No newline at end of file diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/python_interface.cpp b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/python_interface.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6ad82bf6c811bb97bc056cb78e601bfa3e174dd0 --- /dev/null +++ b/rl-control/rl-robotics/legged_gym/legged_gym/envs/gazebo/a1_real/unitree_legged_sdk/python_interface.cpp @@ -0,0 +1,379 @@ +/************************************************************************ +Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "unitree_legged_msgs/MotorCmd.h" +#include "unitree_legged_msgs/MotorState.h" +#include "unitree_legged_msgs/LowCmd.h" +#include "unitree_legged_msgs/LowState.h" +#include "unitree_legged_msgs/IMU.h" + +using namespace std; + +namespace msg +{ + typedef struct + { + std::array quaternion; // quaternion, normalized, (w,x,y,z) + std::array gyroscope; // angular velocity （unit: rad/s) + std::array accelerometer; // m/(s2) + std::array rpy; // euler angle（unit: rad) + int8_t temperature; + } IMU_data; + + typedef struct + { + uint8_t mode; // motor working mode + float q; // current angle (unit: radian) + float dq; // current velocity (unit: radian/second) + float ddq; // current acc (unit: radian/second*second) + float tauEst; // current estimated output torque (unit: N.m) + float q_raw; // current angle (unit: radian) + float dq_raw; // current velocity (unit: radian/second) + float ddq_raw; + int8_t temperature; // current temperature (temperature conduction is slow that leads to lag) + std::array reserve; + } motion_data; + +} + +using namespace msg; + +typedef struct +{ +public: + uint8_t levelFlag; // flag to distinguish high level or low level + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + IMU_data imu; + std::array motorState; + std::array footForce; + std::array footForceEst; + uint32_t tick; // reference real-time from motion controller (unit: us) + std::array wirelessRemote; // wireless commands + uint32_t reserve; + uint32_t crc; +} MyState; + +class RobotInterface +{ +public: + RobotInterface() + { + int argc = 1; + char *argv[1] = {"robot"}; + ros::init(argc, argv, "qr_gazebo"); + _nm = std::make_unique(); + InitSend(); + InitRecv(); + } + MyState ReceiveObservation(); + void SendCommand(std::array motorcmd); + void Initialize(); + void InitSend(); + void InitRecv(); + float ReTime(); + void imuCallback(const sensor_msgs::Imu &msg); + void FRhipCallback(const unitree_legged_msgs::MotorState &msg); + void FRthighCallback(const unitree_legged_msgs::MotorState &msg); + void FRcalfCallback(const unitree_legged_msgs::MotorState &msg); + void FLhipCallback(const unitree_legged_msgs::MotorState &msg); + void FLthighCallback(const unitree_legged_msgs::MotorState &msg); + void FLcalfCallback(const unitree_legged_msgs::MotorState &msg); + void RRhipCallback(const unitree_legged_msgs::MotorState &msg); + void RRthighCallback(const unitree_legged_msgs::MotorState &msg); + void RRcalfCallback(const unitree_legged_msgs::MotorState &msg); + void RLhipCallback(const unitree_legged_msgs::MotorState &msg); + void RLthighCallback(const unitree_legged_msgs::MotorState &msg); + void RLcalfCallback(const unitree_legged_msgs::MotorState &msg); + + std::unique_ptr _nm; + ros::Subscriber _servo_sub[12], _imu_sub, _tick_sub; + ros::Publisher _servo_pub[12]; + std::string _robot_name{"a1"}; + MyState state = {0}; + unitree_legged_msgs::MotorCmd unitreeMsg[12]; +}; + +void RobotInterface::InitSend() +{ + _servo_pub[0] = _nm->advertise("/" + _robot_name + "_gazebo/FR_hip_controller/command", 1); + _servo_pub[1] = _nm->advertise("/" + _robot_name + "_gazebo/FR_thigh_controller/command", 1); + _servo_pub[2] = _nm->advertise("/" + _robot_name + "_gazebo/FR_calf_controller/command", 1); + _servo_pub[3] = _nm->advertise("/" + _robot_name + "_gazebo/FL_hip_controller/command", 1); + _servo_pub[4] = _nm->advertise("/" + _robot_name + "_gazebo/FL_thigh_controller/command", 1); + _servo_pub[5] = _nm->advertise("/" + _robot_name + "_gazebo/FL_calf_controller/command", 1); + _servo_pub[6] = _nm->advertise("/" + _robot_name + "_gazebo/RR_hip_controller/command", 1); + _servo_pub[7] = _nm->advertise("/" + _robot_name + "_gazebo/RR_thigh_controller/command", 1); + _servo_pub[8] = _nm->advertise("/" + _robot_name + "_gazebo/RR_calf_controller/command", 1); + _servo_pub[9] = _nm->advertise("/" + _robot_name + "_gazebo/RL_hip_controller/command", 1); + _servo_pub[10] = _nm->advertise("/" + _robot_name + "_gazebo/RL_thigh_controller/command", 1); + _servo_pub[11] = _nm->advertise("/" + _robot_name + "_gazebo/RL_calf_controller/command", 1); +} + +void RobotInterface::InitRecv() +{ + + _imu_sub = _nm->subscribe("/trunk_imu", 1, &RobotInterface::imuCallback, this); + + _servo_sub[0] = _nm->subscribe("/" + _robot_name + "_gazebo/FR_hip_controller/state", 1, &RobotInterface::FRhipCallback, this); + _servo_sub[1] = _nm->subscribe("/" + _robot_name + "_gazebo/FR_thigh_controller/state", 1, &RobotInterface::FRthighCallback, this); + _servo_sub[2] = _nm->subscribe("/" + _robot_name + "_gazebo/FR_calf_controller/state", 1, &RobotInterface::FRcalfCallback, this); + + _servo_sub[3] = _nm->subscribe("/" + _robot_name + "_gazebo/FL_hip_controller/state", 1, &RobotInterface::FLhipCallback, this); + _servo_sub[4] = _nm->subscribe("/" + _robot_name + "_gazebo/FL_thigh_controller/state", 1, &RobotInterface::FLthighCallback, this); + _servo_sub[5] = _nm->subscribe("/" + _robot_name + "_gazebo/FL_calf_controller/state", 1, &RobotInterface::FLcalfCallback, this); + + _servo_sub[6] = _nm->subscribe("/" + _robot_name + "_gazebo/RR_hip_controller/state", 1, &RobotInterface::RRhipCallback, this); + _servo_sub[7] = _nm->subscribe("/" + _robot_name + "_gazebo/RR_thigh_controller/state", 1, &RobotInterface::RRthighCallback, this); + _servo_sub[8] = _nm->subscribe("/" + _robot_name + "_gazebo/RR_calf_controller/state", 1, &RobotInterface::RRcalfCallback, this); + + _servo_sub[9] = _nm->subscribe("/" + _robot_name + "_gazebo/RL_hip_controller/state", 1, &RobotInterface::RLhipCallback, this); + _servo_sub[10] = _nm->subscribe("/" + _robot_name + "_gazebo/RL_thigh_controller/state", 1, &RobotInterface::RLthighCallback, this); + _servo_sub[11] = _nm->subscribe("/" + _robot_name + "_gazebo/RL_calf_controller/state", 1, &RobotInterface::RLcalfCallback, this); +} + +MyState RobotInterface::ReceiveObservation() +{ + ros::spinOnce(); + return state; +} + +void RobotInterface::SendCommand(std::array motorcmd) +{ + for (int motor_id = 0; motor_id < 12; motor_id++) + { + + // unitreeMsg[motor_id].mode = 10; + // unitreeMsg[motor_id].q = motorcmd[motor_id * 5]; + // unitreeMsg[motor_id].Kp = motorcmd[motor_id * 5 + 1]; + // unitreeMsg[motor_id].dq = motorcmd[motor_id * 5 + 2]; + // unitreeMsg[motor_id].Kd = motorcmd[motor_id * 5 + 3]; + // unitreeMsg[motor_id].tau = 0; + // _servo_pub[motor_id].publish(unitreeMsg[motor_id]); + + unitreeMsg[motor_id].mode = 10; + unitreeMsg[motor_id].q = motorcmd[motor_id * 5]; + unitreeMsg[motor_id].Kp = motorcmd[motor_id * 5 + 1]; + unitreeMsg[motor_id].dq = motorcmd[motor_id * 5 + 2]; + unitreeMsg[motor_id].Kd = motorcmd[motor_id * 5 + 3]; + unitreeMsg[motor_id].tau = motorcmd[motor_id * 5 + 4]; + _servo_pub[motor_id].publish(unitreeMsg[motor_id]); + } +} + +void RobotInterface::imuCallback(const sensor_msgs::Imu &Imu_msg) +{ + + state.imu.quaternion[0] = Imu_msg.orientation.w; + state.imu.quaternion[1] = Imu_msg.orientation.x; + state.imu.quaternion[2] = Imu_msg.orientation.y; + state.imu.quaternion[3] = Imu_msg.orientation.z; + + state.imu.gyroscope[0] = Imu_msg.angular_velocity.x; + state.imu.gyroscope[1] = Imu_msg.angular_velocity.y; + state.imu.gyroscope[2] = Imu_msg.angular_velocity.z; + + state.imu.accelerometer[0] = Imu_msg.linear_acceleration.x; + state.imu.accelerometer[1] = Imu_msg.linear_acceleration.y; + state.imu.accelerometer[2] = Imu_msg.linear_acceleration.z; +} + +void RobotInterface::FRhipCallback(const unitree_legged_msgs::MotorState &msg) +{ + + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[0] = data; +} + +void RobotInterface::FRthighCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[1] = data; +} + +void RobotInterface::FRcalfCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[2] = data; +} + +void RobotInterface::FLhipCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[3] = data; +} + +void RobotInterface::FLthighCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[4] = data; +} + +void RobotInterface::FLcalfCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[5] = data; +} +void RobotInterface::RRhipCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[6] = data; +} + +void RobotInterface::RRthighCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[7] = data; +} + +void RobotInterface::RRcalfCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[8] = data; +} + +void RobotInterface::RLhipCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[9] = data; +} + +void RobotInterface::RLthighCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[10] = data; +} + +void RobotInterface::RLcalfCallback(const unitree_legged_msgs::MotorState &msg) +{ + motion_data data; + data.mode = msg.mode; + data.q = msg.q; + data.dq = msg.dq; + data.tauEst = msg.tauEst; + state.motorState[11] = data; +} + +namespace py = pybind11; + +// TODO: Expose all of comm.h and the RobotInterface Class. + +PYBIND11_MODULE(robot_interface, m) +{ + m.doc() = R"pbdoc( + A1 Robot Interface Python Bindings + ----------------------- + .. currentmodule:: a1_robot_interface + .. autosummary:: + :toctree: _generate + )pbdoc"; + + py::class_(m, "imu_data") + + .def_readwrite("quaternion", &IMU_data::quaternion) + .def_readwrite("gyroscope", &IMU_data::gyroscope) + .def_readwrite("accelerometer", &IMU_data::accelerometer) + .def_readwrite("rpy", &IMU_data::rpy) + .def_readwrite("temperature", &IMU_data::temperature); + + py::class_(m, "motorState") + .def(py::init<>()) + .def_readwrite("mode", &motion_data::mode) + .def_readwrite("q", &motion_data::q) + .def_readwrite("dq", &motion_data::dq) + .def_readwrite("ddq", &motion_data::ddq) + .def_readwrite("tauEst", &motion_data::tauEst) + .def_readwrite("q_raw", &motion_data::q_raw) + .def_readwrite("dq_raw", &motion_data::dq_raw) + .def_readwrite("ddq_raw", &motion_data::ddq_raw) + .def_readwrite("temperature", &motion_data::temperature) + .def_readwrite("reserve", &motion_data::reserve); + + py::class_(m, "MyState") + .def(py::init<>()) + .def_readwrite("levelFlag", &MyState::levelFlag) + .def_readwrite("commVersion", &MyState::commVersion) + .def_readwrite("robotID", &MyState::robotID) + .def_readwrite("SN", &MyState::SN) + .def_readwrite("bandWidth", &MyState::bandWidth) + .def_readwrite("imu", &MyState::imu) + .def_readwrite("motorState", &MyState::motorState) + .def_readwrite("footForce", &MyState::footForce) + .def_readwrite("footForceEst", &MyState::footForceEst) + .def_readwrite("tick", &MyState::tick) + .def_readwrite("wirelessRemote", &MyState::wirelessRemote) + .def_readwrite("reserve", &MyState::reserve) + .def_readwrite("crc", &MyState::crc); + + py::class_(m, "RobotInterface") + .def(py::init<>()) + .def("receive_observation", &RobotInterface::ReceiveObservation) + .def("send_command", &RobotInterface::SendCommand) + .def("InitSend", &RobotInterface::InitSend) + .def("InitRecv", &RobotInterface::InitRecv); + +#ifdef VERSION_INFO + m.attr("__version__") = VERSION_INFO; +#else + m.attr("__version__") = "dev"; +#endif + + m.attr("TEST") = py::int_(int(42)); +} diff --git a/rl-control/rl-robotics/legged_gym/legged_gym/utils/helpers.py b/rl-control/rl-robotics/legged_gym/legged_gym/utils/helpers.py index b2dbe21afbfa2f2383ced1202e136a80ba67fc25..cad4dcab1e8f3d7741b807cbe7b9977a5fefb511 100644 --- a/rl-control/rl-robotics/legged_gym/legged_gym/utils/helpers.py +++ b/rl-control/rl-robotics/legged_gym/legged_gym/utils/helpers.py @@ -94,7 +94,7 @@ def get_load_path(root, load_run='-1', checkpoint=-1): try: model = models[-1] except: - raise ValueError("No model found in {}".format(load_run)) + raise ValueError("No model found in current load_run!") else: model = str(checkpoint) @@ -211,5 +211,10 @@ def register(task_name, task_registry): from legged_gym.envs.lite3.lite3_config import Lite3RoughCfg, Lite3RoughCfgPPO task_registry.register("lite3", LeggedRobot, Lite3RoughCfg(), Lite3RoughCfgPPO()) + elif task_name == 'wl_real': + # 先注册 + from legged_gym.envs.gazebo.a1_config import A1RoughCfg, A1RoughCfgPPO + from legged_gym.envs.gazebo.a1_legged_robot import A1LeggedRobot + task_registry.register("wl_real", A1LeggedRobot, A1RoughCfg(), A1RoughCfgPPO()) else: raise Exception("no such task_name")