├── LICENSE ├── README.md ├── actor_critic.py ├── agent.py ├── converter.py ├── cover.png ├── deploy.py ├── joint_angles.py ├── unitree_legged_sdk ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── example │ ├── example_joystick.cpp │ ├── example_position.cpp │ ├── example_torque.cpp │ ├── example_velocity.cpp │ └── example_walk.cpp ├── example_py │ ├── example_position.py │ ├── example_torque.py │ ├── example_velocity.py │ └── example_walk.py ├── include │ └── unitree_legged_sdk │ │ ├── a1_const.h │ │ ├── aliengo_const.h │ │ ├── comm.h │ │ ├── go1_const.h │ │ ├── joystick.h │ │ ├── loop.h │ │ ├── quadruped.h │ │ ├── safety.h │ │ ├── udp.h │ │ └── unitree_legged_sdk.h └── lib │ ├── cpp │ ├── amd64 │ │ └── libunitree_legged_sdk.a │ └── arm64 │ │ └── libunitree_legged_sdk.a │ └── python │ ├── amd64 │ └── robot_interface.cpython-38-x86_64-linux-gnu.so │ └── arm64 │ ├── robot_interface.cpython-37m-aarch64-linux-gnu.so │ └── robot_interface.cpython-38-aarch64-linux-gnu.so └── weights └── asym_full_model_10000.pt /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2023, Bandi Jai Krishna 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | 1. Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | 2. Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # go1-rl-kit 2 | ![Image1](/cover.png) 3 | Deployment kit for Unitree Go1 Edu 4 | 5 | This repo can be deployed in the following configurations: 6 | 1. External PC/NUC with LAN (Prefered) (Tested) 7 | 2. External PC/NUC with WiFi 8 | 3. Internal Computer of the Go1 9 | 10 | # Dependencies 11 | - python3.8 12 | - torch 13 | - matplotlib 14 | - numpy<1.24 15 | - scipy 16 | - other dependencies for unitree_legged_sdk 17 | 18 | # Installation 19 | On the target machine run the following commands 20 | 1. `git clone https://github.com/TextZip/go1-rl-kit` 21 | 2. `cd go1-rl-kit` 22 | 3. Build the unitree_legged_sdk using instructions from https://github.com/unitreerobotics/unitree_legged_sdk/tree/v3.8.0 23 | 4. Make sure to run one of the python examples from the `unitree_legged_sdk/example_py/` to make sure the build is working. 24 | 5. Manually assign the following network config 25 | - IP Address: 192.168.123.162 26 | - Subnet Mask: 255.255.255.0 27 | - Default Gateway: 192.168.123.1 28 | 6. `ping 192.168.123.10` to make sure you are able to reach the motor controller 29 | 30 | # Deployment 31 | 1. Turn on the robot and the unitree joystick and wait for it to automatically stand up. 32 | 2. Enter low level mode using the following commands: 33 | - L2 + A 34 | - L2 + A 35 | - L2 + B 36 | - L1 + L2 + Start 37 | 3. Connect the LAN cable to the robot and make sure you can ping the motor controller `ping 192.168.123.10` 38 | 4. Suspend the robot in the air 39 | 5. Run `python deploy.py` while the robot is suspended in the air 40 | 2. While suspended, please wait for the robot to reach a standing pose (2-3 Seconds) and then slowly put the robot back on the ground 41 | 3. Do not operate the joystick or the robot until the counter on the screen reaches 2300, after the counter reaches 2300 you can move the robot around. 42 | 4. Left joystick can be used for giving linear velocity commands, the right joystick can be used to give yaw commands. 43 | 44 | 45 | -------------------------------------------------------------------------------- /actor_critic.py: -------------------------------------------------------------------------------- 1 | # SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved. 2 | # SPDX-License-Identifier: BSD-3-Clause 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions are met: 6 | # 7 | # 1. Redistributions of source code must retain the above copyright notice, this 8 | # list of conditions and the following disclaimer. 9 | # 10 | # 2. Redistributions in binary form must reproduce the above copyright notice, 11 | # this list of conditions and the following disclaimer in the documentation 12 | # and/or other materials provided with the distribution. 13 | # 14 | # 3. Neither the name of the copyright holder nor the names of its 15 | # contributors may be used to endorse or promote products derived from 16 | # this software without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | # 29 | # Copyright (c) 2021 ETH Zurich, Nikita Rudin 30 | 31 | import numpy as np 32 | 33 | import torch 34 | import torch.nn as nn 35 | from torch.distributions import Normal 36 | from torch.nn.modules import rnn 37 | 38 | class ActorCritic(nn.Module): 39 | is_recurrent = False 40 | def __init__(self, num_actor_obs, 41 | num_critic_obs, 42 | num_actions, 43 | actor_hidden_dims=[256, 256, 256], 44 | critic_hidden_dims=[256, 256, 256], 45 | activation='elu', 46 | init_noise_std=1.0, 47 | **kwargs): 48 | if kwargs: 49 | print("ActorCritic.__init__ got unexpected arguments, which will be ignored: " + str([key for key in kwargs.keys()])) 50 | super(ActorCritic, self).__init__() 51 | 52 | activation = get_activation(activation) 53 | 54 | mlp_input_dim_a = num_actor_obs 55 | mlp_input_dim_c = num_critic_obs 56 | 57 | # Policy 58 | actor_layers = [] 59 | actor_layers.append(nn.Linear(mlp_input_dim_a, actor_hidden_dims[0])) 60 | actor_layers.append(activation) 61 | for l in range(len(actor_hidden_dims)): 62 | if l == len(actor_hidden_dims) - 1: 63 | actor_layers.append(nn.Linear(actor_hidden_dims[l], num_actions)) 64 | else: 65 | actor_layers.append(nn.Linear(actor_hidden_dims[l], actor_hidden_dims[l + 1])) 66 | actor_layers.append(activation) 67 | self.actor = nn.Sequential(*actor_layers) 68 | 69 | # Value function 70 | critic_layers = [] 71 | critic_layers.append(nn.Linear(mlp_input_dim_c, critic_hidden_dims[0])) 72 | critic_layers.append(activation) 73 | for l in range(len(critic_hidden_dims)): 74 | if l == len(critic_hidden_dims) - 1: 75 | critic_layers.append(nn.Linear(critic_hidden_dims[l], 1)) 76 | else: 77 | critic_layers.append(nn.Linear(critic_hidden_dims[l], critic_hidden_dims[l + 1])) 78 | critic_layers.append(activation) 79 | self.critic = nn.Sequential(*critic_layers) 80 | 81 | print(f"Actor MLP: {self.actor}") 82 | print(f"Critic MLP: {self.critic}") 83 | 84 | # Action noise 85 | self.std = nn.Parameter(init_noise_std * torch.ones(num_actions)) 86 | self.distribution = None 87 | # disable args validation for speedup 88 | Normal.set_default_validate_args = False 89 | 90 | # seems that we get better performance without init 91 | # self.init_memory_weights(self.memory_a, 0.001, 0.) 92 | # self.init_memory_weights(self.memory_c, 0.001, 0.) 93 | 94 | @staticmethod 95 | # not used at the moment 96 | def init_weights(sequential, scales): 97 | [torch.nn.init.orthogonal_(module.weight, gain=scales[idx]) for idx, module in 98 | enumerate(mod for mod in sequential if isinstance(mod, nn.Linear))] 99 | 100 | 101 | def reset(self, dones=None): 102 | pass 103 | 104 | def forward(self): 105 | raise NotImplementedError 106 | 107 | @property 108 | def action_mean(self): 109 | return self.distribution.mean 110 | 111 | @property 112 | def action_std(self): 113 | return self.distribution.stddev 114 | 115 | @property 116 | def entropy(self): 117 | return self.distribution.entropy().sum(dim=-1) 118 | 119 | def update_distribution(self, observations): 120 | mean = self.actor(observations) 121 | self.distribution = Normal(mean, mean*0. + self.std) 122 | 123 | def act(self, observations, **kwargs): 124 | self.update_distribution(observations) 125 | return self.distribution.sample() 126 | 127 | def get_actions_log_prob(self, actions): 128 | return self.distribution.log_prob(actions).sum(dim=-1) 129 | 130 | def act_inference(self, observations): 131 | actions_mean = self.actor(observations) 132 | return actions_mean 133 | 134 | def evaluate(self, critic_observations, **kwargs): 135 | value = self.critic(critic_observations) 136 | return value 137 | 138 | def get_activation(act_name): 139 | if act_name == "elu": 140 | return nn.ELU() 141 | elif act_name == "selu": 142 | return nn.SELU() 143 | elif act_name == "relu": 144 | return nn.ReLU() 145 | elif act_name == "crelu": 146 | return nn.ReLU() 147 | elif act_name == "lrelu": 148 | return nn.LeakyReLU() 149 | elif act_name == "tanh": 150 | return nn.Tanh() 151 | elif act_name == "sigmoid": 152 | return nn.Sigmoid() 153 | else: 154 | print("invalid activation function!") 155 | return None -------------------------------------------------------------------------------- /agent.py: -------------------------------------------------------------------------------- 1 | # BSD 2-Clause License 2 | 3 | # Copyright (c) 2023, Bandi Jai Krishna 4 | 5 | # Redistribution and use in source and binary forms, with or without 6 | # modification, are permitted provided that the following conditions are met: 7 | 8 | # 1. Redistributions of source code must retain the above copyright notice, this 9 | # list of conditions and the following disclaimer. 10 | 11 | # 2. Redistributions in binary form must reproduce the above copyright notice, 12 | # this list of conditions and the following disclaimer in the documentation 13 | # and/or other materials provided with the distribution. 14 | 15 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 19 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 21 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 22 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 23 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 24 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | 26 | import sys 27 | import time 28 | import math 29 | import numpy as np 30 | import struct 31 | import torch 32 | import scipy 33 | 34 | from actor_critic import ActorCritic 35 | sys.path.append('unitree_legged_sdk/lib/python/amd64') 36 | import robot_interface as sdk 37 | 38 | class Agent(): 39 | def __init__(self,path): 40 | self.dt = 0.02 41 | self.num_actions = 12 42 | self.num_obs = 44*5 43 | self.unit_obs = 44 44 | self.num_privl_obs = 421 45 | self.device = 'cpu' 46 | self.path = path#'bp4/model_1750.pt' 47 | self.d = {'FR_0':0, 'FR_1':1, 'FR_2':2, 48 | 'FL_0':3, 'FL_1':4, 'FL_2':5, 49 | 'RR_0':6, 'RR_1':7, 'RR_2':8, 50 | 'RL_0':9, 'RL_1':10, 'RL_2':11 } 51 | PosStopF = math.pow(10,9) 52 | VelStopF = 16000.0 53 | HIGHLEVEL = 0xee 54 | LOWLEVEL = 0xff 55 | self.init = True 56 | self.motiontime = 0 57 | self.timestep = 0 58 | self.time = 0 59 | 60 | ##################################################################### 61 | self.euler = np.zeros(3) 62 | self.buf_idx = 0 63 | 64 | self.smoothing_length = 12 65 | self.deuler_history = np.zeros((self.smoothing_length, 3)) 66 | self.dt_history = np.zeros((self.smoothing_length, 1)) 67 | self.euler_prev = np.zeros(3) 68 | self.timuprev = time.time() 69 | 70 | self.body_ang_vel = np.zeros(3) 71 | self.smoothing_ratio = 0.2 72 | ##################################################################### 73 | 74 | self.default_angles = [0.1,0.8,-1.5,-0.1,0.8,-1.5,0.1,1,-1.5,-0.1,1,-1.5] 75 | self.default_angles_tensor = torch.tensor([0.1,0.8,-1.5,-0.1,0.8,-1.5,0.1,1,-1.5,-0.1,1,-1.5],device=self.device,dtype=torch.float,requires_grad=False) 76 | 77 | self.actions = torch.zeros(self.num_actions,device=self.device,dtype=torch.float,requires_grad=False) 78 | self.obs = torch.zeros(self.num_obs,device=self.device,dtype=torch.float,requires_grad=False) 79 | self.obs_storage = torch.zeros(self.unit_obs*4,device=self.device,dtype=torch.float) 80 | 81 | actor_critic = ActorCritic(num_actor_obs=self.num_obs,num_critic_obs=self.num_privl_obs,num_actions=12,actor_hidden_dims = [512, 256, 128],critic_hidden_dims = [512, 256, 128],activation = 'elu',init_noise_std = 1.0) 82 | loaded_dict = torch.load(self.path) 83 | actor_critic.load_state_dict(loaded_dict['model_state_dict']) 84 | actor_critic.eval() 85 | self.policy = actor_critic.act_inference 86 | 87 | self.udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) 88 | self.safe = sdk.Safety(sdk.LeggedType.Go1) 89 | 90 | self.cmd = sdk.LowCmd() 91 | self.state = sdk.LowState() 92 | self.udp.InitCmdData(self.cmd) 93 | 94 | def get_body_angular_vel(self): 95 | # self.body_ang_vel = self.smoothing_ratio * np.mean(self.deuler_history / self.dt_history, axis=0) + ( 96 | # 1 - self.smoothing_ratio) * self.body_ang_vel 97 | 98 | self.body_ang_vel = self.smoothing_ratio * np.array(self.state.imu.gyroscope) + (1 - self.smoothing_ratio) * self.body_ang_vel 99 | 100 | return self.body_ang_vel 101 | 102 | def get_observations(self): 103 | self.euler = np.array(self.state.imu.rpy) 104 | self.deuler_history[self.buf_idx % self.smoothing_length, :] = self.euler - self.euler_prev 105 | self.dt_history[self.buf_idx % self.smoothing_length] = time.time() - self.timuprev 106 | self.timuprev = time.time() 107 | self.buf_idx += 1 108 | self.euler_prev = self.euler 109 | 110 | lx = struct.unpack('f', struct.pack('4B', *self.state.wirelessRemote[4:8])) 111 | ly = struct.unpack('f', struct.pack('4B', *self.state.wirelessRemote[20:24])) 112 | rx = struct.unpack('f', struct.pack('4B', *self.state.wirelessRemote[8:12])) 113 | # ry = struct.unpack('f', struct.pack('4B', *self.state.wirelessRemote[12:16])) 114 | forward = ly[0]*0.6 115 | if abs(forward) <0.30: 116 | forward = 0 117 | side = -lx[0]*0.5 118 | if abs(side) <0.2: 119 | side = 0 120 | rotate = -rx[0]*0.8 121 | if abs(rotate) <0.4: 122 | rotate = 0 123 | 124 | self.pitch = torch.tensor([self.state.imu.rpy[1]],device=self.device,dtype=torch.float,requires_grad=False) 125 | self.roll = torch.tensor([self.state.imu.rpy[0]],device=self.device,dtype=torch.float,requires_grad=False) 126 | angles = self.getJointPos() 127 | vel = self.getJointVelocity() 128 | 129 | self.dof_pos = torch.tensor([m - n for m,n in zip(angles,self.default_angles)],device=self.device,dtype=torch.float,requires_grad=False) 130 | body_ang_vel = self.get_body_angular_vel() #self.state.imu.gyroscope #[state.imu.gyroscope] 131 | # print(vel[1]) 132 | if self.timestep > 1600: 133 | self.base_ang_vel = torch.tensor([body_ang_vel],device=self.device,dtype=torch.float,requires_grad=False) 134 | self.dof_vel = torch.tensor([vel],device=self.device,dtype=torch.float,requires_grad=False) 135 | else: 136 | self.base_ang_vel = 0*torch.tensor([body_ang_vel],device=self.device,dtype=torch.float,requires_grad=False) 137 | self.dof_vel = 0*torch.tensor([vel],device=self.device,dtype=torch.float,requires_grad=False) 138 | 139 | if self.timestep > 2000: 140 | # self.commands = torch.tensor([0.5,0,0],device=self.device,dtype=torch.float,requires_grad=False) 141 | self.commands = torch.tensor([forward,side,rotate],device=self.device,dtype=torch.float,requires_grad=False) 142 | # print(f"{vel[1]} | {self.base_ang_vel}") 143 | else: 144 | self.commands = torch.tensor([0,0,0],device=self.device,dtype=torch.float,requires_grad=False) 145 | 146 | self.obs = torch.cat(( 147 | self.base_ang_vel.squeeze(), 148 | self.pitch, 149 | self.roll, 150 | self.commands, 151 | self.dof_pos, 152 | self.dof_vel.squeeze(), 153 | self.actions, 154 | ),dim=-1) 155 | current_obs = self.obs 156 | 157 | self.obs = torch.cat((self.obs,self.obs_storage),dim=-1) 158 | 159 | self.obs_storage[:-self.unit_obs] = self.obs_storage[self.unit_obs:].clone() 160 | self.obs_storage[-self.unit_obs:] = current_obs 161 | 162 | def init_pose(self): 163 | while self.init: 164 | self.pre_step() 165 | self.get_observations() 166 | self.motiontime = self.motiontime+1 167 | if self.motiontime <100: 168 | self.setJointValues(self.default_angles,kp=5,kd=1) 169 | else: 170 | self.setJointValues(self.default_angles,kp=50,kd=5) 171 | # self.setJointValues(self.default_angles,kp=20,kd=0.5) 172 | if self.motiontime > 1100: 173 | self.init = False 174 | self.post_step() 175 | print("Starting") 176 | while True: 177 | self.step() 178 | 179 | def pre_step(self): 180 | self.udp.Recv() 181 | self.udp.GetRecv(self.state) 182 | 183 | def step(self): 184 | ''' 185 | Has to be called after init_pose 186 | calls pre_step for getting udp packets 187 | calls policy with obs, clips and scales actions and adds default pose before sending them to robot 188 | calls post_step 189 | ''' 190 | self.pre_step() 191 | self.get_observations() 192 | self.actions = self.policy(self.obs) 193 | actions = torch.clip(self.actions, -100, 100).to('cpu').detach() 194 | scaled_actions = actions * 0.25 195 | final_angles = scaled_actions+self.default_angles_tensor 196 | 197 | # print("actions:" + ",".join(map(str, actions.numpy().tolist()))) 198 | # print("observations:" + str(time.process_time()) + ",".join(map(str, self.obs.detach().numpy().tolist()))) 199 | 200 | self.setJointValues(angles=final_angles,kp=20,kd=0.5) 201 | self.post_step() 202 | 203 | def post_step(self): 204 | ''' 205 | Offers power protection, sends udp packets, maintains timing 206 | ''' 207 | self.safe.PowerProtect(self.cmd, self.state, 9) 208 | self.udp.SetSend(self.cmd) 209 | self.udp.Send() 210 | time.sleep(max(self.dt - (time.time() - self.time), 0)) 211 | if self.timestep % 100 == 0: 212 | print(f"{self.timestep}| frq: {1 / (time.time() - self.time)} Hz") 213 | self.time = time.time() 214 | self.timestep = self.timestep + 1 215 | 216 | def getJointVelocity(self): 217 | velocity = [self.state.motorState[self.d['FL_0']].dq,self.state.motorState[self.d['FL_1']].dq,self.state.motorState[self.d['FL_2']].dq, 218 | self.state.motorState[self.d['FR_0']].dq,self.state.motorState[self.d['FR_1']].dq,self.state.motorState[self.d['FR_2']].dq, 219 | self.state.motorState[self.d['RL_0']].dq,self.state.motorState[self.d['RL_1']].dq,self.state.motorState[self.d['RL_2']].dq, 220 | self.state.motorState[self.d['RR_0']].dq,self.state.motorState[self.d['RR_1']].dq,self.state.motorState[self.d['RR_2']].dq] 221 | return velocity 222 | 223 | def getJointPos(self): 224 | current_angles = [ 225 | self.state.motorState[self.d['FL_0']].q,self.state.motorState[self.d['FL_1']].q,self.state.motorState[self.d['FL_2']].q, 226 | self.state.motorState[self.d['FR_0']].q,self.state.motorState[self.d['FR_1']].q,self.state.motorState[self.d['FR_2']].q, 227 | self.state.motorState[self.d['RL_0']].q,self.state.motorState[self.d['RL_1']].q,self.state.motorState[self.d['RL_2']].q, 228 | self.state.motorState[self.d['RR_0']].q,self.state.motorState[self.d['RR_1']].q,self.state.motorState[self.d['RR_2']].q] 229 | return current_angles 230 | 231 | def setJointValues(self,angles,kp,kd): 232 | self.cmd.motorCmd[self.d['FR_0']].q = angles[3] 233 | self.cmd.motorCmd[self.d['FR_0']].dq = 0 234 | self.cmd.motorCmd[self.d['FR_0']].Kp = kp 235 | self.cmd.motorCmd[self.d['FR_0']].Kd = kd 236 | self.cmd.motorCmd[self.d['FR_0']].tau = 0.0 237 | 238 | self.cmd.motorCmd[self.d['FR_1']].q = angles[4] 239 | self.cmd.motorCmd[self.d['FR_1']].dq = 0 240 | self.cmd.motorCmd[self.d['FR_1']].Kp = kp 241 | self.cmd.motorCmd[self.d['FR_1']].Kd = kd 242 | self.cmd.motorCmd[self.d['FR_1']].tau = 0.0 243 | 244 | self.cmd.motorCmd[self.d['FR_2']].q = angles[5] 245 | self.cmd.motorCmd[self.d['FR_2']].dq = 0 246 | self.cmd.motorCmd[self.d['FR_2']].Kp = kp 247 | self.cmd.motorCmd[self.d['FR_2']].Kd = kd 248 | self.cmd.motorCmd[self.d['FR_2']].tau = 0.0 249 | 250 | self.cmd.motorCmd[self.d['FL_0']].q = angles[0] 251 | self.cmd.motorCmd[self.d['FL_0']].dq = 0 252 | self.cmd.motorCmd[self.d['FL_0']].Kp = kp 253 | self.cmd.motorCmd[self.d['FL_0']].Kd = kd 254 | self.cmd.motorCmd[self.d['FL_0']].tau = 0.0 255 | 256 | self.cmd.motorCmd[self.d['FL_1']].q = angles[1] 257 | self.cmd.motorCmd[self.d['FL_1']].dq = 0 258 | self.cmd.motorCmd[self.d['FL_1']].Kp = kp 259 | self.cmd.motorCmd[self.d['FL_1']].Kd = kd 260 | self.cmd.motorCmd[self.d['FL_1']].tau = 0.0 261 | 262 | self.cmd.motorCmd[self.d['FL_2']].q = angles[2] 263 | self.cmd.motorCmd[self.d['FL_2']].dq = 0 264 | self.cmd.motorCmd[self.d['FL_2']].Kp = kp 265 | self.cmd.motorCmd[self.d['FL_2']].Kd = kd 266 | self.cmd.motorCmd[self.d['FL_2']].tau = 0.0 267 | 268 | self.cmd.motorCmd[self.d['RR_0']].q = angles[9] 269 | self.cmd.motorCmd[self.d['RR_0']].dq = 0 270 | self.cmd.motorCmd[self.d['RR_0']].Kp = kp 271 | self.cmd.motorCmd[self.d['RR_0']].Kd = kd 272 | self.cmd.motorCmd[self.d['RR_0']].tau = 0.0 273 | 274 | self.cmd.motorCmd[self.d['RR_1']].q = angles[10] 275 | self.cmd.motorCmd[self.d['RR_1']].dq = 0 276 | self.cmd.motorCmd[self.d['RR_1']].Kp = kp 277 | self.cmd.motorCmd[self.d['RR_1']].Kd = kd 278 | self.cmd.motorCmd[self.d['RR_1']].tau = 0.0 279 | 280 | self.cmd.motorCmd[self.d['RR_2']].q = angles[11] 281 | self.cmd.motorCmd[self.d['RR_2']].dq = 0 282 | self.cmd.motorCmd[self.d['RR_2']].Kp = kp 283 | self.cmd.motorCmd[self.d['RR_2']].Kd = kd 284 | self.cmd.motorCmd[self.d['RR_2']].tau = 0.0 285 | 286 | self.cmd.motorCmd[self.d['RL_0']].q = angles[6] 287 | self.cmd.motorCmd[self.d['RL_0']].dq = 0 288 | self.cmd.motorCmd[self.d['RL_0']].Kp = kp 289 | self.cmd.motorCmd[self.d['RL_0']].Kd = kd 290 | self.cmd.motorCmd[self.d['RL_0']].tau = 0.0 291 | 292 | self.cmd.motorCmd[self.d['RL_1']].q = angles[7] 293 | self.cmd.motorCmd[self.d['RL_1']].dq = 0 294 | self.cmd.motorCmd[self.d['RL_1']].Kp = kp 295 | self.cmd.motorCmd[self.d['RL_1']].Kd = kd 296 | self.cmd.motorCmd[self.d['RL_1']].tau = 0.0 297 | 298 | self.cmd.motorCmd[self.d['RL_2']].q = angles[8] 299 | self.cmd.motorCmd[self.d['RL_2']].dq = 0 300 | self.cmd.motorCmd[self.d['RL_2']].Kp = kp 301 | self.cmd.motorCmd[self.d['RL_2']].Kd = kd 302 | self.cmd.motorCmd[self.d['RL_2']].tau = 0.0 303 | -------------------------------------------------------------------------------- /converter.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from actor_critic import ActorCritic 3 | 4 | num_obs = 44 5 | path = 'weights/day5/walk8_sm45_model_2050.pt' 6 | save = 'OLDwalk8_sm45_model_2050.pth.tar' 7 | actor_critic = ActorCritic(num_actor_obs=num_obs,num_critic_obs=num_obs,num_actions=12,actor_hidden_dims = [128, 128],critic_hidden_dims = [128, 128],activation = 'elu',init_noise_std = 1.0) 8 | loaded_dict = torch.load(path) 9 | actor_critic.load_state_dict(loaded_dict['model_state_dict']) 10 | torch.save(actor_critic.state_dict(),save,_use_new_zipfile_serialization=False) 11 | # actor_critic.eval() 12 | # self.policy = actor_critic.act_inference -------------------------------------------------------------------------------- /cover.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TextZip/go1-rl-kit/aaa2f34a362065935697d960f2b8ea880880e090/cover.png -------------------------------------------------------------------------------- /deploy.py: -------------------------------------------------------------------------------- 1 | from agent import Agent 2 | 3 | Go1 = Agent(path='weights/asym_full_model_10000.pt') 4 | Go1.init_pose() 5 | -------------------------------------------------------------------------------- /joint_angles.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | import numpy as np 7 | import struct 8 | 9 | sys.path.append('unitree_legged_sdk/lib/python/amd64') 10 | import robot_interface as sdk 11 | 12 | # def jointLinearInterpolation(initPos, targetPos, rate): 13 | 14 | # rate = np.fmin(np.fmax(rate, 0.0), 1.0) 15 | # p = initPos*(1-rate) + targetPos*rate 16 | # return p 17 | 18 | 19 | def setJointValues(angles,kp,kd): 20 | cmd.motorCmd[d['FR_0']].q = angles[3] 21 | cmd.motorCmd[d['FR_0']].dq = 0 22 | cmd.motorCmd[d['FR_0']].Kp = kp 23 | cmd.motorCmd[d['FR_0']].Kd = kd 24 | cmd.motorCmd[d['FR_0']].tau = 0.0 25 | 26 | cmd.motorCmd[d['FR_1']].q = angles[4] 27 | cmd.motorCmd[d['FR_1']].dq = 0 28 | cmd.motorCmd[d['FR_1']].Kp = kp 29 | cmd.motorCmd[d['FR_1']].Kd = kd 30 | cmd.motorCmd[d['FR_1']].tau = 0.0 31 | 32 | cmd.motorCmd[d['FR_2']].q = angles[5] 33 | cmd.motorCmd[d['FR_2']].dq = 0 34 | cmd.motorCmd[d['FR_2']].Kp = kp 35 | cmd.motorCmd[d['FR_2']].Kd = kd 36 | cmd.motorCmd[d['FR_2']].tau = 0.0 37 | 38 | cmd.motorCmd[d['FL_0']].q = angles[0] 39 | cmd.motorCmd[d['FL_0']].dq = 0 40 | cmd.motorCmd[d['FL_0']].Kp = kp 41 | cmd.motorCmd[d['FL_0']].Kd = kd 42 | cmd.motorCmd[d['FL_0']].tau = 0.0 43 | 44 | cmd.motorCmd[d['FL_1']].q = angles[1] 45 | cmd.motorCmd[d['FL_1']].dq = 0 46 | cmd.motorCmd[d['FL_1']].Kp = kp 47 | cmd.motorCmd[d['FL_1']].Kd = kd 48 | cmd.motorCmd[d['FL_1']].tau = 0.0 49 | 50 | cmd.motorCmd[d['FL_2']].q = angles[2] 51 | cmd.motorCmd[d['FL_2']].dq = 0 52 | cmd.motorCmd[d['FL_2']].Kp = kp 53 | cmd.motorCmd[d['FL_2']].Kd = kd 54 | cmd.motorCmd[d['FL_2']].tau = 0.0 55 | 56 | cmd.motorCmd[d['RR_0']].q = angles[9] 57 | cmd.motorCmd[d['RR_0']].dq = 0 58 | cmd.motorCmd[d['RR_0']].Kp = kp 59 | cmd.motorCmd[d['RR_0']].Kd = kd 60 | cmd.motorCmd[d['RR_0']].tau = 0.0 61 | 62 | cmd.motorCmd[d['RR_1']].q = angles[10] 63 | cmd.motorCmd[d['RR_1']].dq = 0 64 | cmd.motorCmd[d['RR_1']].Kp = kp 65 | cmd.motorCmd[d['RR_1']].Kd = kd 66 | cmd.motorCmd[d['RR_1']].tau = 0.0 67 | 68 | cmd.motorCmd[d['RR_2']].q = angles[11] 69 | cmd.motorCmd[d['RR_2']].dq = 0 70 | cmd.motorCmd[d['RR_2']].Kp = kp 71 | cmd.motorCmd[d['RR_2']].Kd = kd 72 | cmd.motorCmd[d['RR_2']].tau = 0.0 73 | 74 | cmd.motorCmd[d['RL_0']].q = angles[6] 75 | cmd.motorCmd[d['RL_0']].dq = 0 76 | cmd.motorCmd[d['RL_0']].Kp = kp 77 | cmd.motorCmd[d['RL_0']].Kd = kd 78 | cmd.motorCmd[d['RL_0']].tau = 0.0 79 | 80 | cmd.motorCmd[d['RL_1']].q = angles[7] 81 | cmd.motorCmd[d['RL_1']].dq = 0 82 | cmd.motorCmd[d['RL_1']].Kp = kp 83 | cmd.motorCmd[d['RL_1']].Kd = kd 84 | cmd.motorCmd[d['RL_1']].tau = 0.0 85 | 86 | cmd.motorCmd[d['RL_2']].q = angles[8] 87 | cmd.motorCmd[d['RL_2']].dq = 0 88 | cmd.motorCmd[d['RL_2']].tau = 0.0 89 | cmd.motorCmd[d['RL_2']].Kp = kp 90 | cmd.motorCmd[d['RL_2']].Kd = kd 91 | 92 | 93 | if __name__ == '__main__': 94 | 95 | d = {'FR_0':0, 'FR_1':1, 'FR_2':2, 96 | 'FL_0':3, 'FL_1':4, 'FL_2':5, 97 | 'RR_0':6, 'RR_1':7, 'RR_2':8, 98 | 'RL_0':9, 'RL_1':10, 'RL_2':11 } 99 | PosStopF = math.pow(10,9) 100 | VelStopF = 16000.0 101 | HIGHLEVEL = 0xee 102 | LOWLEVEL = 0xff 103 | # sin_mid_q = [0.0, 1.2, -2.0] 104 | dt = 0.002 105 | # qInit = [0, 0, 0] 106 | # qDes = [0, 0, 0] 107 | # sin_count = 0 108 | # rate_count = 0 109 | # Kp = [0, 0, 0] 110 | # Kd = [0, 0, 0] 111 | 112 | udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) 113 | safe = sdk.Safety(sdk.LeggedType.Go1) 114 | 115 | cmd = sdk.LowCmd() 116 | state = sdk.LowState() 117 | udp.InitCmdData(cmd) 118 | 119 | init = True 120 | # Tpi = 0 121 | motiontime = 0 122 | while True: 123 | time.sleep(0.02) 124 | motiontime += 1 125 | 126 | # print(motiontime) 127 | # print(state.imu.rpy[0]) 128 | 129 | default_angles = [0.1,0.8,-1.5,-0.1,0.8,-1.5,0.1,1,-1.5,-0.1,1,-1.5] 130 | udp.Recv() 131 | udp.GetRecv(state) 132 | 133 | # print(struct.unpack('f', struct.pack('4B', *state.wirelessRemote[20:24]))) 134 | print(f"{state.motorState[d['FR_0']].q} | {time.time()}") 135 | if init: 136 | motiontime = motiontime+1 137 | setJointValues(default_angles,kp=5,kd=1) 138 | if motiontime > 100: 139 | init = False 140 | 141 | if init == False: 142 | setJointValues(default_angles,kp=50,kd=5) 143 | 144 | 145 | 146 | 147 | # if( motiontime >= 0): 148 | 149 | # # first, get record initial position 150 | # if( motiontime >= 0 and motiontime < 10): 151 | # qInit[0] = state.motorState[d['FR_0']].q 152 | # qInit[1] = state.motorState[d['FR_1']].q 153 | # qInit[2] = state.motorState[d['FR_2']].q 154 | 155 | # # second, move to the origin point of a sine movement with Kp Kd 156 | # if( motiontime >= 10 and motiontime < 400): 157 | # rate_count += 1 158 | # rate = rate_count/200.0 # needs count to 200 159 | # Kp = [5, 5, 5] 160 | # Kd = [1, 1, 1] 161 | # # Kp = [20, 20, 20] 162 | # # Kd = [2, 2, 2] 163 | 164 | # qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate) 165 | # qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate) 166 | # qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate) 167 | 168 | # # last, do sine wave 169 | # freq_Hz = 1 170 | # # freq_Hz = 5 171 | # freq_rad = freq_Hz * 2* math.pi 172 | # t = dt*sin_count 173 | # if( motiontime >= 400): 174 | # sin_count += 1 175 | # # sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0) 176 | # # sin_joint2 = -0.9 * sin(3*M_PI*sin_count/1000.0) 177 | # sin_joint1 = 0.6 * math.sin(t*freq_rad) 178 | # sin_joint2 = -0.9 * math.sin(t*freq_rad) 179 | # qDes[0] = sin_mid_q[0] 180 | # qDes[1] = sin_mid_q[1] + sin_joint1 181 | # qDes[2] = sin_mid_q[2] + sin_joint2 182 | 183 | 184 | 185 | 186 | # cmd.motorCmd[d['FR_1']].q = qDes[1] 187 | # cmd.motorCmd[d['FR_1']].dq = 0 188 | # cmd.motorCmd[d['FR_1']].Kp = Kp[1] 189 | # cmd.motorCmd[d['FR_1']].Kd = Kd[1] 190 | # cmd.motorCmd[d['FR_1']].tau = 0.0 191 | 192 | # cmd.motorCmd[d['FR_2']].q = qDes[2] 193 | # cmd.motorCmd[d['FR_2']].dq = 0 194 | # cmd.motorCmd[d['FR_2']].Kp = Kp[2] 195 | # cmd.motorCmd[d['FR_2']].Kd = Kd[2] 196 | # cmd.motorCmd[d['FR_2']].tau = 0.0 197 | # # cmd.motorCmd[d['FR_2']].tau = 2 * sin(t*freq_rad) 198 | 199 | 200 | # if(motiontime > 10): 201 | safe.PowerProtect(cmd, state, 1) 202 | 203 | udp.SetSend(cmd) 204 | udp.Send() 205 | 206 | 207 | -------------------------------------------------------------------------------- /unitree_legged_sdk/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | 3 | *.o 4 | .vscode 5 | -------------------------------------------------------------------------------- /unitree_legged_sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unitree_legged_sdk) 3 | 4 | include_directories(include) 5 | 6 | add_compile_options(-std=c++14) 7 | 8 | # check arch and os 9 | message("-- CMAKE_SYSTEM_PROCESSOR: ${CMAKE_SYSTEM_PROCESSOR}") 10 | if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64.*") 11 | set(ARCH amd64) 12 | endif() 13 | if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "aarch64.*") 14 | set(ARCH arm64) 15 | endif() 16 | 17 | link_directories(lib/cpp/${ARCH}) 18 | set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) 19 | 20 | set(CMAKE_CXX_FLAGS "-O3 -fPIC") 21 | 22 | # one pc one process 23 | add_executable(example_position example/example_position.cpp) 24 | target_link_libraries(example_position ${EXTRA_LIBS}) 25 | 26 | add_executable(example_velocity example/example_velocity.cpp) 27 | target_link_libraries(example_velocity ${EXTRA_LIBS}) 28 | 29 | add_executable(example_torque example/example_torque.cpp) 30 | target_link_libraries(example_torque ${EXTRA_LIBS}) 31 | 32 | add_executable(example_walk example/example_walk.cpp) 33 | target_link_libraries(example_walk ${EXTRA_LIBS}) 34 | 35 | add_executable(example_joystick example/example_joystick.cpp) 36 | target_link_libraries(example_joystick ${EXTRA_LIBS}) 37 | 38 | 39 | # install 40 | install(TARGETS 41 | example_position example_velocity example_torque example_walk example_joystick 42 | DESTINATION bin/unitree) 43 | install(DIRECTORY lib/cpp/${ARCH}/ 44 | DESTINATION lib/unitree 45 | USE_SOURCE_PERMISSIONS) 46 | -------------------------------------------------------------------------------- /unitree_legged_sdk/LICENSE: -------------------------------------------------------------------------------- 1 | Mozilla Public License Version 2.0 2 | ================================== 3 | 4 | 1. Definitions 5 | -------------- 6 | 7 | 1.1. "Contributor" 8 | means each individual or legal entity that creates, contributes to 9 | the creation of, or owns Covered Software. 10 | 11 | 1.2. "Contributor Version" 12 | means the combination of the Contributions of others (if any) used 13 | by a Contributor and that particular Contributor's Contribution. 14 | 15 | 1.3. "Contribution" 16 | means Covered Software of a particular Contributor. 17 | 18 | 1.4. "Covered Software" 19 | means Source Code Form to which the initial Contributor has attached 20 | the notice in Exhibit A, the Executable Form of such Source Code 21 | Form, and Modifications of such Source Code Form, in each case 22 | including portions thereof. 23 | 24 | 1.5. "Incompatible With Secondary Licenses" 25 | means 26 | 27 | (a) that the initial Contributor has attached the notice described 28 | in Exhibit B to the Covered Software; or 29 | 30 | (b) that the Covered Software was made available under the terms of 31 | version 1.1 or earlier of the License, but not also under the 32 | terms of a Secondary License. 33 | 34 | 1.6. "Executable Form" 35 | means any form of the work other than Source Code Form. 36 | 37 | 1.7. "Larger Work" 38 | means a work that combines Covered Software with other material, in 39 | a separate file or files, that is not Covered Software. 40 | 41 | 1.8. "License" 42 | means this document. 43 | 44 | 1.9. "Licensable" 45 | means having the right to grant, to the maximum extent possible, 46 | whether at the time of the initial grant or subsequently, any and 47 | all of the rights conveyed by this License. 48 | 49 | 1.10. "Modifications" 50 | means any of the following: 51 | 52 | (a) any file in Source Code Form that results from an addition to, 53 | deletion from, or modification of the contents of Covered 54 | Software; or 55 | 56 | (b) any new file in Source Code Form that contains any Covered 57 | Software. 58 | 59 | 1.11. "Patent Claims" of a Contributor 60 | means any patent claim(s), including without limitation, method, 61 | process, and apparatus claims, in any patent Licensable by such 62 | Contributor that would be infringed, but for the grant of the 63 | License, by the making, using, selling, offering for sale, having 64 | made, import, or transfer of either its Contributions or its 65 | Contributor Version. 66 | 67 | 1.12. "Secondary License" 68 | means either the GNU General Public License, Version 2.0, the GNU 69 | Lesser General Public License, Version 2.1, the GNU Affero General 70 | Public License, Version 3.0, or any later versions of those 71 | licenses. 72 | 73 | 1.13. "Source Code Form" 74 | means the form of the work preferred for making modifications. 75 | 76 | 1.14. "You" (or "Your") 77 | means an individual or a legal entity exercising rights under this 78 | License. For legal entities, "You" includes any entity that 79 | controls, is controlled by, or is under common control with You. For 80 | purposes of this definition, "control" means (a) the power, direct 81 | or indirect, to cause the direction or management of such entity, 82 | whether by contract or otherwise, or (b) ownership of more than 83 | fifty percent (50%) of the outstanding shares or beneficial 84 | ownership of such entity. 85 | 86 | 2. License Grants and Conditions 87 | -------------------------------- 88 | 89 | 2.1. Grants 90 | 91 | Each Contributor hereby grants You a world-wide, royalty-free, 92 | non-exclusive license: 93 | 94 | (a) under intellectual property rights (other than patent or trademark) 95 | Licensable by such Contributor to use, reproduce, make available, 96 | modify, display, perform, distribute, and otherwise exploit its 97 | Contributions, either on an unmodified basis, with Modifications, or 98 | as part of a Larger Work; and 99 | 100 | (b) under Patent Claims of such Contributor to make, use, sell, offer 101 | for sale, have made, import, and otherwise transfer either its 102 | Contributions or its Contributor Version. 103 | 104 | 2.2. Effective Date 105 | 106 | The licenses granted in Section 2.1 with respect to any Contribution 107 | become effective for each Contribution on the date the Contributor first 108 | distributes such Contribution. 109 | 110 | 2.3. Limitations on Grant Scope 111 | 112 | The licenses granted in this Section 2 are the only rights granted under 113 | this License. No additional rights or licenses will be implied from the 114 | distribution or licensing of Covered Software under this License. 115 | Notwithstanding Section 2.1(b) above, no patent license is granted by a 116 | Contributor: 117 | 118 | (a) for any code that a Contributor has removed from Covered Software; 119 | or 120 | 121 | (b) for infringements caused by: (i) Your and any other third party's 122 | modifications of Covered Software, or (ii) the combination of its 123 | Contributions with other software (except as part of its Contributor 124 | Version); or 125 | 126 | (c) under Patent Claims infringed by Covered Software in the absence of 127 | its Contributions. 128 | 129 | This License does not grant any rights in the trademarks, service marks, 130 | or logos of any Contributor (except as may be necessary to comply with 131 | the notice requirements in Section 3.4). 132 | 133 | 2.4. Subsequent Licenses 134 | 135 | No Contributor makes additional grants as a result of Your choice to 136 | distribute the Covered Software under a subsequent version of this 137 | License (see Section 10.2) or under the terms of a Secondary License (if 138 | permitted under the terms of Section 3.3). 139 | 140 | 2.5. Representation 141 | 142 | Each Contributor represents that the Contributor believes its 143 | Contributions are its original creation(s) or it has sufficient rights 144 | to grant the rights to its Contributions conveyed by this License. 145 | 146 | 2.6. Fair Use 147 | 148 | This License is not intended to limit any rights You have under 149 | applicable copyright doctrines of fair use, fair dealing, or other 150 | equivalents. 151 | 152 | 2.7. Conditions 153 | 154 | Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted 155 | in Section 2.1. 156 | 157 | 3. Responsibilities 158 | ------------------- 159 | 160 | 3.1. Distribution of Source Form 161 | 162 | All distribution of Covered Software in Source Code Form, including any 163 | Modifications that You create or to which You contribute, must be under 164 | the terms of this License. You must inform recipients that the Source 165 | Code Form of the Covered Software is governed by the terms of this 166 | License, and how they can obtain a copy of this License. You may not 167 | attempt to alter or restrict the recipients' rights in the Source Code 168 | Form. 169 | 170 | 3.2. Distribution of Executable Form 171 | 172 | If You distribute Covered Software in Executable Form then: 173 | 174 | (a) such Covered Software must also be made available in Source Code 175 | Form, as described in Section 3.1, and You must inform recipients of 176 | the Executable Form how they can obtain a copy of such Source Code 177 | Form by reasonable means in a timely manner, at a charge no more 178 | than the cost of distribution to the recipient; and 179 | 180 | (b) You may distribute such Executable Form under the terms of this 181 | License, or sublicense it under different terms, provided that the 182 | license for the Executable Form does not attempt to limit or alter 183 | the recipients' rights in the Source Code Form under this License. 184 | 185 | 3.3. Distribution of a Larger Work 186 | 187 | You may create and distribute a Larger Work under terms of Your choice, 188 | provided that You also comply with the requirements of this License for 189 | the Covered Software. If the Larger Work is a combination of Covered 190 | Software with a work governed by one or more Secondary Licenses, and the 191 | Covered Software is not Incompatible With Secondary Licenses, this 192 | License permits You to additionally distribute such Covered Software 193 | under the terms of such Secondary License(s), so that the recipient of 194 | the Larger Work may, at their option, further distribute the Covered 195 | Software under the terms of either this License or such Secondary 196 | License(s). 197 | 198 | 3.4. Notices 199 | 200 | You may not remove or alter the substance of any license notices 201 | (including copyright notices, patent notices, disclaimers of warranty, 202 | or limitations of liability) contained within the Source Code Form of 203 | the Covered Software, except that You may alter any license notices to 204 | the extent required to remedy known factual inaccuracies. 205 | 206 | 3.5. Application of Additional Terms 207 | 208 | You may choose to offer, and to charge a fee for, warranty, support, 209 | indemnity or liability obligations to one or more recipients of Covered 210 | Software. However, You may do so only on Your own behalf, and not on 211 | behalf of any Contributor. You must make it absolutely clear that any 212 | such warranty, support, indemnity, or liability obligation is offered by 213 | You alone, and You hereby agree to indemnify every Contributor for any 214 | liability incurred by such Contributor as a result of warranty, support, 215 | indemnity or liability terms You offer. You may include additional 216 | disclaimers of warranty and limitations of liability specific to any 217 | jurisdiction. 218 | 219 | 4. Inability to Comply Due to Statute or Regulation 220 | --------------------------------------------------- 221 | 222 | If it is impossible for You to comply with any of the terms of this 223 | License with respect to some or all of the Covered Software due to 224 | statute, judicial order, or regulation then You must: (a) comply with 225 | the terms of this License to the maximum extent possible; and (b) 226 | describe the limitations and the code they affect. Such description must 227 | be placed in a text file included with all distributions of the Covered 228 | Software under this License. Except to the extent prohibited by statute 229 | or regulation, such description must be sufficiently detailed for a 230 | recipient of ordinary skill to be able to understand it. 231 | 232 | 5. Termination 233 | -------------- 234 | 235 | 5.1. The rights granted under this License will terminate automatically 236 | if You fail to comply with any of its terms. However, if You become 237 | compliant, then the rights granted under this License from a particular 238 | Contributor are reinstated (a) provisionally, unless and until such 239 | Contributor explicitly and finally terminates Your grants, and (b) on an 240 | ongoing basis, if such Contributor fails to notify You of the 241 | non-compliance by some reasonable means prior to 60 days after You have 242 | come back into compliance. Moreover, Your grants from a particular 243 | Contributor are reinstated on an ongoing basis if such Contributor 244 | notifies You of the non-compliance by some reasonable means, this is the 245 | first time You have received notice of non-compliance with this License 246 | from such Contributor, and You become compliant prior to 30 days after 247 | Your receipt of the notice. 248 | 249 | 5.2. If You initiate litigation against any entity by asserting a patent 250 | infringement claim (excluding declaratory judgment actions, 251 | counter-claims, and cross-claims) alleging that a Contributor Version 252 | directly or indirectly infringes any patent, then the rights granted to 253 | You by any and all Contributors for the Covered Software under Section 254 | 2.1 of this License shall terminate. 255 | 256 | 5.3. In the event of termination under Sections 5.1 or 5.2 above, all 257 | end user license agreements (excluding distributors and resellers) which 258 | have been validly granted by You or Your distributors under this License 259 | prior to termination shall survive termination. 260 | 261 | ************************************************************************ 262 | * * 263 | * 6. Disclaimer of Warranty * 264 | * ------------------------- * 265 | * * 266 | * Covered Software is provided under this License on an "as is" * 267 | * basis, without warranty of any kind, either expressed, implied, or * 268 | * statutory, including, without limitation, warranties that the * 269 | * Covered Software is free of defects, merchantable, fit for a * 270 | * particular purpose or non-infringing. The entire risk as to the * 271 | * quality and performance of the Covered Software is with You. * 272 | * Should any Covered Software prove defective in any respect, You * 273 | * (not any Contributor) assume the cost of any necessary servicing, * 274 | * repair, or correction. This disclaimer of warranty constitutes an * 275 | * essential part of this License. No use of any Covered Software is * 276 | * authorized under this License except under this disclaimer. * 277 | * * 278 | ************************************************************************ 279 | 280 | ************************************************************************ 281 | * * 282 | * 7. Limitation of Liability * 283 | * -------------------------- * 284 | * * 285 | * Under no circumstances and under no legal theory, whether tort * 286 | * (including negligence), contract, or otherwise, shall any * 287 | * Contributor, or anyone who distributes Covered Software as * 288 | * permitted above, be liable to You for any direct, indirect, * 289 | * special, incidental, or consequential damages of any character * 290 | * including, without limitation, damages for lost profits, loss of * 291 | * goodwill, work stoppage, computer failure or malfunction, or any * 292 | * and all other commercial damages or losses, even if such party * 293 | * shall have been informed of the possibility of such damages. This * 294 | * limitation of liability shall not apply to liability for death or * 295 | * personal injury resulting from such party's negligence to the * 296 | * extent applicable law prohibits such limitation. Some * 297 | * jurisdictions do not allow the exclusion or limitation of * 298 | * incidental or consequential damages, so this exclusion and * 299 | * limitation may not apply to You. * 300 | * * 301 | ************************************************************************ 302 | 303 | 8. Litigation 304 | ------------- 305 | 306 | Any litigation relating to this License may be brought only in the 307 | courts of a jurisdiction where the defendant maintains its principal 308 | place of business and such litigation shall be governed by laws of that 309 | jurisdiction, without reference to its conflict-of-law provisions. 310 | Nothing in this Section shall prevent a party's ability to bring 311 | cross-claims or counter-claims. 312 | 313 | 9. Miscellaneous 314 | ---------------- 315 | 316 | This License represents the complete agreement concerning the subject 317 | matter hereof. If any provision of this License is held to be 318 | unenforceable, such provision shall be reformed only to the extent 319 | necessary to make it enforceable. Any law or regulation which provides 320 | that the language of a contract shall be construed against the drafter 321 | shall not be used to construe this License against a Contributor. 322 | 323 | 10. Versions of the License 324 | --------------------------- 325 | 326 | 10.1. New Versions 327 | 328 | Mozilla Foundation is the license steward. Except as provided in Section 329 | 10.3, no one other than the license steward has the right to modify or 330 | publish new versions of this License. Each version will be given a 331 | distinguishing version number. 332 | 333 | 10.2. Effect of New Versions 334 | 335 | You may distribute the Covered Software under the terms of the version 336 | of the License under which You originally received the Covered Software, 337 | or under the terms of any subsequent version published by the license 338 | steward. 339 | 340 | 10.3. Modified Versions 341 | 342 | If you create software not governed by this License, and you want to 343 | create a new license for such software, you may create and use a 344 | modified version of this License if you rename the license and remove 345 | any references to the name of the license steward (except to note that 346 | such modified license differs from this License). 347 | 348 | 10.4. Distributing Source Code Form that is Incompatible With Secondary 349 | Licenses 350 | 351 | If You choose to distribute Source Code Form that is Incompatible With 352 | Secondary Licenses under the terms of this version of the License, the 353 | notice described in Exhibit B of this License must be attached. 354 | 355 | Exhibit A - Source Code Form License Notice 356 | ------------------------------------------- 357 | 358 | This Source Code Form is subject to the terms of the Mozilla Public 359 | License, v. 2.0. If a copy of the MPL was not distributed with this 360 | file, You can obtain one at http://mozilla.org/MPL/2.0/. 361 | 362 | If it is not possible or desirable to put the notice in a particular 363 | file, then You may include the notice in a location (such as a LICENSE 364 | file in a relevant directory) where a recipient would be likely to look 365 | for such a notice. 366 | 367 | You may add additional accurate notices of copyright ownership. 368 | 369 | Exhibit B - "Incompatible With Secondary Licenses" Notice 370 | --------------------------------------------------------- 371 | 372 | This Source Code Form is "Incompatible With Secondary Licenses", as 373 | defined by the Mozilla Public License, v. 2.0. 374 | -------------------------------------------------------------------------------- /unitree_legged_sdk/README.md: -------------------------------------------------------------------------------- 1 | # v3.8.0 2 | The unitree_legged_sdk is mainly used for communication between PC and Controller board. 3 | It also can be used in other PCs with UDP. 4 | 5 | ### Notice 6 | support robot: Go1 7 | 8 | not support robot: Laikago, Aliengo, A1. (Check release [v3.3.1](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/v3.3.1) for support) 9 | 10 | ### Sport Mode 11 | ```bash 12 | Legged_sport >= v1.36.0 13 | firmware H0.1.7 >= v0.1.35 14 | H0.1.9 >= v0.1.35 15 | ``` 16 | 17 | ### Dependencies 18 | * [Boost](http://www.boost.org) (version 1.5.4 or higher) 19 | * [CMake](http://www.cmake.org) (version 2.8.3 or higher) 20 | * [g++](https://gcc.gnu.org/) (version 8.3.0 or higher) 21 | 22 | 23 | ### Build 24 | ```bash 25 | mkdir build 26 | cd build 27 | cmake ../ 28 | make 29 | ``` 30 | 31 | ### Run 32 | 33 | #### Cpp 34 | Run examples with 'sudo' for memory locking. 35 | 36 | #### Python 37 | ##### arm 38 | change `sys.path.append('../lib/python/amd64')` to `sys.path.append('../lib/python/arm64')` 39 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example/example_joystick.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 7 | #include "unitree_legged_sdk/joystick.h" 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace UNITREE_LEGGED_SDK; 13 | 14 | class Custom 15 | { 16 | public: 17 | Custom(uint8_t level): 18 | safe(LeggedType::Go1), 19 | udp(level, 8090, "192.168.123.10", 8007){ 20 | udp.InitCmdData(cmd); 21 | } 22 | void UDPSend(); 23 | void UDPRecv(); 24 | void RobotControl(); 25 | 26 | Safety safe; 27 | UDP udp; 28 | LowCmd cmd = {0}; 29 | LowState state = {0}; 30 | xRockerBtnDataStruct _keyData; 31 | int motiontime = 0; 32 | float dt = 0.002; // 0.001~0.01 33 | }; 34 | 35 | void Custom::UDPRecv() 36 | { 37 | udp.Recv(); 38 | } 39 | 40 | void Custom::UDPSend() 41 | { 42 | udp.Send(); 43 | } 44 | 45 | void Custom::RobotControl() 46 | { 47 | motiontime++; 48 | udp.GetRecv(state); 49 | 50 | memcpy(&_keyData, &state.wirelessRemote[0], 40); 51 | 52 | if((int)_keyData.btn.components.A == 1){ 53 | std::cout << "The key A is pressed, and the value of lx is " << _keyData.lx << std::endl; 54 | } 55 | 56 | safe.PowerProtect(cmd, state, 1); 57 | udp.SetSend(cmd); 58 | } 59 | 60 | int main(void) 61 | { 62 | std::cout << "Communication level is set to LOW-level." << std::endl 63 | << "WARNING: Make sure the robot is hung up." << std::endl 64 | << "Press Enter to continue..." << std::endl; 65 | std::cin.ignore(); 66 | 67 | Custom custom(LOWLEVEL); 68 | // InitEnvironment(); 69 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 70 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 71 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 72 | 73 | loop_udpSend.start(); 74 | loop_udpRecv.start(); 75 | loop_control.start(); 76 | 77 | while(1){ 78 | sleep(10); 79 | }; 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example/example_position.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace std; 12 | using namespace UNITREE_LEGGED_SDK; 13 | 14 | class Custom 15 | { 16 | public: 17 | Custom(uint8_t level): 18 | safe(LeggedType::Go1), 19 | udp(level, 8090, "192.168.123.10", 8007) { 20 | udp.InitCmdData(cmd); 21 | } 22 | void UDPRecv(); 23 | void UDPSend(); 24 | void RobotControl(); 25 | 26 | Safety safe; 27 | UDP udp; 28 | LowCmd cmd = {0}; 29 | LowState state = {0}; 30 | float qInit[3]={0}; 31 | float qDes[3]={0}; 32 | float sin_mid_q[3] = {0.0, 1.2, -2.0}; 33 | float Kp[3] = {0}; 34 | float Kd[3] = {0}; 35 | double time_consume = 0; 36 | int rate_count = 0; 37 | int sin_count = 0; 38 | int motiontime = 0; 39 | float dt = 0.002; // 0.001~0.01 40 | }; 41 | 42 | void Custom::UDPRecv() 43 | { 44 | udp.Recv(); 45 | } 46 | 47 | void Custom::UDPSend() 48 | { 49 | udp.Send(); 50 | } 51 | 52 | double jointLinearInterpolation(double initPos, double targetPos, double rate) 53 | { 54 | double p; 55 | rate = std::min(std::max(rate, 0.0), 1.0); 56 | p = initPos*(1-rate) + targetPos*rate; 57 | return p; 58 | } 59 | 60 | void Custom::RobotControl() 61 | { 62 | motiontime++; 63 | udp.GetRecv(state); 64 | // printf("%d %f\n", motiontime, state.motorState[FR_2].q); 65 | printf("%d %f %f\n", motiontime, state.motorState[FR_1].q, state.motorState[FR_1].dq); 66 | 67 | // gravity compensation 68 | cmd.motorCmd[FR_0].tau = -0.65f; 69 | cmd.motorCmd[FL_0].tau = +0.65f; 70 | cmd.motorCmd[RR_0].tau = -0.65f; 71 | cmd.motorCmd[RL_0].tau = +0.65f; 72 | 73 | // if( motiontime >= 100){ 74 | if( motiontime >= 0){ 75 | // first, get record initial position 76 | // if( motiontime >= 100 && motiontime < 500){ 77 | if( motiontime >= 0 && motiontime < 10){ 78 | qInit[0] = state.motorState[FR_0].q; 79 | qInit[1] = state.motorState[FR_1].q; 80 | qInit[2] = state.motorState[FR_2].q; 81 | } 82 | // second, move to the origin point of a sine movement with Kp Kd 83 | // if( motiontime >= 500 && motiontime < 1500){ 84 | if( motiontime >= 10 && motiontime < 400){ 85 | rate_count++; 86 | double rate = rate_count/200.0; // needs count to 200 87 | Kp[0] = 5.0; Kp[1] = 5.0; Kp[2] = 5.0; 88 | Kd[0] = 1.0; Kd[1] = 1.0; Kd[2] = 1.0; 89 | // Kp[0] = 20.0; Kp[1] = 20.0; Kp[2] = 20.0; 90 | // Kd[0] = 2.0; Kd[1] = 2.0; Kd[2] = 2.0; 91 | 92 | qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate); 93 | qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate); 94 | qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate); 95 | } 96 | double sin_joint1, sin_joint2; 97 | // last, do sine wave 98 | if( motiontime >= 400){ 99 | sin_count++; 100 | sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0); 101 | sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0); 102 | qDes[0] = sin_mid_q[0]; 103 | qDes[1] = sin_mid_q[1]; 104 | qDes[2] = sin_mid_q[2] + sin_joint2; 105 | // qDes[2] = sin_mid_q[2]; 106 | } 107 | 108 | cmd.motorCmd[FR_0].q = qDes[0]; 109 | cmd.motorCmd[FR_0].dq = 0; 110 | cmd.motorCmd[FR_0].Kp = Kp[0]; 111 | cmd.motorCmd[FR_0].Kd = Kd[0]; 112 | cmd.motorCmd[FR_0].tau = -0.65f; 113 | 114 | cmd.motorCmd[FR_1].q = qDes[1]; 115 | cmd.motorCmd[FR_1].dq = 0; 116 | cmd.motorCmd[FR_1].Kp = Kp[1]; 117 | cmd.motorCmd[FR_1].Kd = Kd[1]; 118 | cmd.motorCmd[FR_1].tau = 0.0f; 119 | 120 | cmd.motorCmd[FR_2].q = qDes[2]; 121 | cmd.motorCmd[FR_2].dq = 0; 122 | cmd.motorCmd[FR_2].Kp = Kp[2]; 123 | cmd.motorCmd[FR_2].Kd = Kd[2]; 124 | cmd.motorCmd[FR_2].tau = 0.0f; 125 | 126 | } 127 | 128 | if(motiontime > 10){ 129 | safe.PositionLimit(cmd); 130 | int res1 = safe.PowerProtect(cmd, state, 1); 131 | // You can uncomment it for position protection 132 | // int res2 = safe.PositionProtect(cmd, state, 10); 133 | if(res1 < 0) exit(-1); 134 | } 135 | 136 | udp.SetSend(cmd); 137 | 138 | } 139 | 140 | 141 | int main(void) 142 | { 143 | std::cout << "Communication level is set to LOW-level." << std::endl 144 | << "WARNING: Make sure the robot is hung up." << std::endl 145 | << "Press Enter to continue..." << std::endl; 146 | std::cin.ignore(); 147 | 148 | Custom custom(LOWLEVEL); 149 | // InitEnvironment(); 150 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 151 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 152 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 153 | 154 | loop_udpSend.start(); 155 | loop_udpRecv.start(); 156 | loop_control.start(); 157 | 158 | while(1){ 159 | sleep(10); 160 | }; 161 | 162 | return 0; 163 | } 164 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example/example_torque.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace UNITREE_LEGGED_SDK; 11 | 12 | class Custom 13 | { 14 | public: 15 | Custom(uint8_t level): 16 | safe(LeggedType::Go1), 17 | udp(level, 8090, "192.168.123.10", 8007){ 18 | udp.InitCmdData(cmd); 19 | } 20 | void UDPSend(); 21 | void UDPRecv(); 22 | void RobotControl(); 23 | 24 | Safety safe; 25 | UDP udp; 26 | LowCmd cmd = {0}; 27 | LowState state = {0}; 28 | int motiontime = 0; 29 | float dt = 0.002; // 0.001~0.01 30 | }; 31 | 32 | void Custom::UDPRecv() 33 | { 34 | udp.Recv(); 35 | } 36 | 37 | void Custom::UDPSend() 38 | { 39 | udp.Send(); 40 | } 41 | 42 | void Custom::RobotControl() 43 | { 44 | motiontime++; 45 | udp.GetRecv(state); 46 | // printf("%d\n", motiontime); 47 | // gravity compensation 48 | cmd.motorCmd[FR_0].tau = -0.65f; 49 | cmd.motorCmd[FL_0].tau = +0.65f; 50 | cmd.motorCmd[RR_0].tau = -0.65f; 51 | cmd.motorCmd[RL_0].tau = +0.65f; 52 | 53 | if( motiontime >= 500){ 54 | float torque = (0 - state.motorState[FR_1].q)*10.0f + (0 - state.motorState[FR_1].dq)*1.0f; 55 | if(torque > 5.0f) torque = 5.0f; 56 | if(torque < -5.0f) torque = -5.0f; 57 | // if(torque > 15.0f) torque = 15.0f; 58 | // if(torque < -15.0f) torque = -15.0f; 59 | 60 | cmd.motorCmd[FR_1].q = PosStopF; 61 | cmd.motorCmd[FR_1].dq = VelStopF; 62 | cmd.motorCmd[FR_1].Kp = 0; 63 | cmd.motorCmd[FR_1].Kd = 0; 64 | cmd.motorCmd[FR_1].tau = torque; 65 | } 66 | int res = safe.PowerProtect(cmd, state, 1); 67 | if(res < 0) exit(-1); 68 | 69 | udp.SetSend(cmd); 70 | } 71 | 72 | int main(void) 73 | { 74 | std::cout << "Communication level is set to LOW-level." << std::endl 75 | << "WARNING: Make sure the robot is hung up." << std::endl 76 | << "Press Enter to continue..." << std::endl; 77 | std::cin.ignore(); 78 | 79 | Custom custom(LOWLEVEL); 80 | // InitEnvironment(); 81 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 82 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 83 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 84 | 85 | loop_udpSend.start(); 86 | loop_udpRecv.start(); 87 | loop_control.start(); 88 | 89 | while(1){ 90 | sleep(10); 91 | }; 92 | 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example/example_velocity.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | 9 | using namespace UNITREE_LEGGED_SDK; 10 | 11 | class Custom 12 | { 13 | public: 14 | Custom(uint8_t level): 15 | safe(LeggedType::Go1), 16 | udp(level, 8090, "192.168.123.10", 8007){ 17 | udp.InitCmdData(cmd); 18 | } 19 | void UDPRecv(); 20 | void UDPSend(); 21 | void RobotControl(); 22 | 23 | Safety safe; 24 | UDP udp; 25 | LowCmd cmd = {0}; 26 | LowState state = {0}; 27 | int Tpi = 0; 28 | int motiontime = 0; 29 | float dt = 0.002; // 0.001~0.01 30 | }; 31 | 32 | void Custom::UDPRecv() 33 | { 34 | udp.Recv(); 35 | } 36 | 37 | void Custom::UDPSend() 38 | { 39 | udp.Send(); 40 | } 41 | 42 | void Custom::RobotControl() 43 | { 44 | motiontime++; 45 | udp.GetRecv(state); 46 | 47 | // gravity compensation 48 | cmd.motorCmd[FR_0].tau = -0.65f; 49 | cmd.motorCmd[FL_0].tau = +0.65f; 50 | cmd.motorCmd[RR_0].tau = -0.65f; 51 | cmd.motorCmd[RL_0].tau = +0.65f; 52 | 53 | if( motiontime >= 500){ 54 | float speed = 2 * sin(3*M_PI*Tpi/1500.0); 55 | 56 | cmd.motorCmd[FR_1].q = PosStopF; 57 | cmd.motorCmd[FR_1].dq = speed; 58 | cmd.motorCmd[FR_1].Kp = 0; 59 | cmd.motorCmd[FR_1].Kd = 4; 60 | // cmd.motorCmd[FR_1].Kd = 6; 61 | cmd.motorCmd[FR_1].tau = 0.0f; 62 | Tpi++; 63 | } 64 | 65 | if(motiontime > 10){ 66 | int res1 = safe.PowerProtect(cmd, state, 1); 67 | // You can uncomment it for position protection 68 | // int res2 = safe.PositionProtect(cmd, state, 10); 69 | if(res1 < 0) exit(-1); 70 | } 71 | 72 | udp.SetSend(cmd); 73 | } 74 | 75 | int main(void) 76 | { 77 | std::cout << "Communication level is set to LOW-level." << std::endl 78 | << "WARNING: Make sure the robot is hung up." << std::endl 79 | << "Press Enter to continue..." << std::endl; 80 | std::cin.ignore(); 81 | 82 | Custom custom(LOWLEVEL); 83 | // InitEnvironment(); 84 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 85 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 86 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 87 | 88 | loop_udpSend.start(); 89 | loop_udpRecv.start(); 90 | loop_control.start(); 91 | 92 | while(1){ 93 | sleep(10); 94 | }; 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example/example_walk.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace UNITREE_LEGGED_SDK; 12 | 13 | class Custom 14 | { 15 | public: 16 | Custom(uint8_t level): 17 | safe(LeggedType::Go1), 18 | udp(level, 8090, "192.168.123.161", 8082){ 19 | udp.InitCmdData(cmd); 20 | } 21 | void UDPRecv(); 22 | void UDPSend(); 23 | void RobotControl(); 24 | 25 | Safety safe; 26 | UDP udp; 27 | HighCmd cmd = {0}; 28 | HighState state = {0}; 29 | int motiontime = 0; 30 | float dt = 0.002; // 0.001~0.01 31 | }; 32 | 33 | 34 | void Custom::UDPRecv() 35 | { 36 | udp.Recv(); 37 | } 38 | 39 | void Custom::UDPSend() 40 | { 41 | udp.Send(); 42 | } 43 | 44 | void Custom::RobotControl() 45 | { 46 | motiontime += 2; 47 | udp.GetRecv(state); 48 | printf("%d %f\n", motiontime, state.imu.quaternion[2]); 49 | 50 | cmd.mode = 0; // 0:idle, default stand 1:forced stand 2:walk continuously 51 | cmd.gaitType = 0; 52 | cmd.speedLevel = 0; 53 | cmd.footRaiseHeight = 0; 54 | cmd.bodyHeight = 0; 55 | cmd.euler[0] = 0; 56 | cmd.euler[1] = 0; 57 | cmd.euler[2] = 0; 58 | cmd.velocity[0] = 0.0f; 59 | cmd.velocity[1] = 0.0f; 60 | cmd.yawSpeed = 0.0f; 61 | cmd.reserve = 0; 62 | 63 | if(motiontime > 0 && motiontime < 1000){ 64 | cmd.mode = 1; 65 | cmd.euler[0] = -0.3; 66 | } 67 | if(motiontime > 1000 && motiontime < 2000){ 68 | cmd.mode = 1; 69 | cmd.euler[0] = 0.3; 70 | } 71 | if(motiontime > 2000 && motiontime < 3000){ 72 | cmd.mode = 1; 73 | cmd.euler[1] = -0.2; 74 | } 75 | if(motiontime > 3000 && motiontime < 4000){ 76 | cmd.mode = 1; 77 | cmd.euler[1] = 0.2; 78 | } 79 | if(motiontime > 4000 && motiontime < 5000){ 80 | cmd.mode = 1; 81 | cmd.euler[2] = -0.2; 82 | } 83 | if(motiontime > 5000 && motiontime < 6000){ 84 | cmd.mode = 1; 85 | cmd.euler[2] = 0.2; 86 | } 87 | if(motiontime > 6000 && motiontime < 7000){ 88 | cmd.mode = 1; 89 | cmd.bodyHeight = -0.2; 90 | } 91 | if(motiontime > 7000 && motiontime < 8000){ 92 | cmd.mode = 1; 93 | cmd.bodyHeight = 0.1; 94 | } 95 | if(motiontime > 8000 && motiontime < 9000){ 96 | cmd.mode = 1; 97 | cmd.bodyHeight = 0.0; 98 | } 99 | if(motiontime > 9000 && motiontime < 11000){ 100 | cmd.mode = 5; 101 | } 102 | if(motiontime > 11000 && motiontime < 13000){ 103 | cmd.mode = 6; 104 | } 105 | if(motiontime > 13000 && motiontime < 14000){ 106 | cmd.mode = 0; 107 | } 108 | if(motiontime > 14000 && motiontime < 18000){ 109 | cmd.mode = 2; 110 | cmd.gaitType = 2; 111 | cmd.velocity[0] = 0.4f; // -1 ~ +1 112 | cmd.yawSpeed = 2; 113 | cmd.footRaiseHeight = 0.1; 114 | // printf("walk\n"); 115 | } 116 | if(motiontime > 18000 && motiontime < 20000){ 117 | cmd.mode = 0; 118 | cmd.velocity[0] = 0; 119 | } 120 | if(motiontime > 20000 && motiontime < 24000){ 121 | cmd.mode = 2; 122 | cmd.gaitType = 1; 123 | cmd.velocity[0] = 0.2f; // -1 ~ +1 124 | cmd.bodyHeight = 0.1; 125 | // printf("walk\n"); 126 | } 127 | if(motiontime>24000 ){ 128 | cmd.mode = 1; 129 | } 130 | 131 | udp.SetSend(cmd); 132 | } 133 | 134 | int main(void) 135 | { 136 | std::cout << "Communication level is set to HIGH-level." << std::endl 137 | << "WARNING: Make sure the robot is standing on the ground." << std::endl 138 | << "Press Enter to continue..." << std::endl; 139 | std::cin.ignore(); 140 | 141 | Custom custom(HIGHLEVEL); 142 | // InitEnvironment(); 143 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 144 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 145 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 146 | 147 | loop_udpSend.start(); 148 | loop_udpRecv.start(); 149 | loop_control.start(); 150 | 151 | while(1){ 152 | sleep(10); 153 | }; 154 | 155 | return 0; 156 | } 157 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example_py/example_position.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | import numpy as np 7 | 8 | sys.path.append('../lib/python/amd64') 9 | import robot_interface as sdk 10 | 11 | def jointLinearInterpolation(initPos, targetPos, rate): 12 | 13 | rate = np.fmin(np.fmax(rate, 0.0), 1.0) 14 | p = initPos*(1-rate) + targetPos*rate 15 | return p 16 | 17 | 18 | if __name__ == '__main__': 19 | 20 | d = {'FR_0':0, 'FR_1':1, 'FR_2':2, 21 | 'FL_0':3, 'FL_1':4, 'FL_2':5, 22 | 'RR_0':6, 'RR_1':7, 'RR_2':8, 23 | 'RL_0':9, 'RL_1':10, 'RL_2':11 } 24 | PosStopF = math.pow(10,9) 25 | VelStopF = 16000.0 26 | HIGHLEVEL = 0xee 27 | LOWLEVEL = 0xff 28 | sin_mid_q = [0.0, 1.2, -2.0] 29 | dt = 0.002 30 | qInit = [0, 0, 0] 31 | qDes = [0, 0, 0] 32 | sin_count = 0 33 | rate_count = 0 34 | Kp = [0, 0, 0] 35 | Kd = [0, 0, 0] 36 | 37 | udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) 38 | safe = sdk.Safety(sdk.LeggedType.Go1) 39 | 40 | cmd = sdk.LowCmd() 41 | state = sdk.LowState() 42 | udp.InitCmdData(cmd) 43 | 44 | Tpi = 0 45 | motiontime = 0 46 | while True: 47 | time.sleep(0.002) 48 | motiontime += 1 49 | 50 | # print(motiontime) 51 | # print() 52 | 53 | 54 | udp.Recv() 55 | udp.GetRecv(state) 56 | 57 | if( motiontime >= 0): 58 | 59 | # first, get record initial position 60 | if( motiontime >= 0 and motiontime < 10): 61 | qInit[0] = state.motorState[d['FR_0']].q 62 | qInit[1] = state.motorState[d['FR_1']].q 63 | qInit[2] = state.motorState[d['FR_2']].q 64 | 65 | # second, move to the origin point of a sine movement with Kp Kd 66 | if( motiontime >= 10 and motiontime < 400): 67 | rate_count += 1 68 | rate = rate_count/200.0 # needs count to 200 69 | Kp = [5, 5, 5] 70 | Kd = [1, 1, 1] 71 | # Kp = [20, 20, 20] 72 | # Kd = [0.5, 0.5, 0.5] 73 | 74 | qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate) 75 | qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate) 76 | qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate) 77 | 78 | # last, do sine wave 79 | freq_Hz = 1 80 | # freq_Hz = 5 81 | freq_rad = freq_Hz * 2* math.pi 82 | t = dt*sin_count 83 | if( motiontime >= 400): 84 | sin_count += 1 85 | # sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0) 86 | # sin_joint2 = -0.9 * sin(3*M_PI*sin_count/1000.0) 87 | sin_joint1 = 0.6 * math.sin(t*freq_rad) 88 | sin_joint2 = -0.9 * math.sin(t*freq_rad) 89 | qDes[0] = sin_mid_q[0] 90 | qDes[1] = sin_mid_q[1] + sin_joint1 91 | qDes[2] = sin_mid_q[2] + sin_joint2 92 | 93 | 94 | if motiontime >= 400: 95 | print(f"{qDes[1]},{state.motorState[d['FR_1']].dq},{state.motorState[d['FR_1']].dq_raw},{time.time()}") 96 | cmd.motorCmd[d['FR_0']].q = qDes[0] 97 | cmd.motorCmd[d['FR_0']].dq = 0 98 | cmd.motorCmd[d['FR_0']].Kp = Kp[0] 99 | cmd.motorCmd[d['FR_0']].Kd = Kd[0] 100 | cmd.motorCmd[d['FR_0']].tau = -0.65 101 | 102 | cmd.motorCmd[d['FR_1']].q = qDes[1] 103 | cmd.motorCmd[d['FR_1']].dq = 0 104 | cmd.motorCmd[d['FR_1']].Kp = Kp[1] 105 | cmd.motorCmd[d['FR_1']].Kd = Kd[1] 106 | cmd.motorCmd[d['FR_1']].tau = 0.0 107 | 108 | cmd.motorCmd[d['FR_2']].q = qDes[2] 109 | cmd.motorCmd[d['FR_2']].dq = 0 110 | cmd.motorCmd[d['FR_2']].Kp = Kp[2] 111 | cmd.motorCmd[d['FR_2']].Kd = Kd[2] 112 | cmd.motorCmd[d['FR_2']].tau = 0.0 113 | # cmd.motorCmd[d['FR_2']].tau = 2 * sin(t*freq_rad) 114 | 115 | 116 | if(motiontime > 10): 117 | safe.PowerProtect(cmd, state, 1) 118 | 119 | udp.SetSend(cmd) 120 | udp.Send() 121 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example_py/example_torque.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | import numpy as np 7 | 8 | sys.path.append('../lib/python/amd64') 9 | import robot_interface as sdk 10 | 11 | 12 | if __name__ == '__main__': 13 | 14 | d = {'FR_0':0, 'FR_1':1, 'FR_2':2, 15 | 'FL_0':3, 'FL_1':4, 'FL_2':5, 16 | 'RR_0':6, 'RR_1':7, 'RR_2':8, 17 | 'RL_0':9, 'RL_1':10, 'RL_2':11 } 18 | PosStopF = math.pow(10,9) 19 | VelStopF = 16000.0 20 | HIGHLEVEL = 0xee 21 | LOWLEVEL = 0xff 22 | dt = 0.002 23 | sin_count = 0 24 | 25 | udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) 26 | safe = sdk.Safety(sdk.LeggedType.Go1) 27 | 28 | cmd = sdk.LowCmd() 29 | state = sdk.LowState() 30 | udp.InitCmdData(cmd) 31 | 32 | Tpi = 0 33 | motiontime = 0 34 | while True: 35 | time.sleep(0.002) 36 | motiontime += 1 37 | 38 | # freq_Hz = 1 39 | freq_Hz = 2 40 | # freq_Hz = 5; 41 | freq_rad = freq_Hz * 2* math.pi 42 | # t = dt*sin_count 43 | 44 | # print(motiontime) 45 | # print(state.imu.rpy[0]) 46 | 47 | 48 | udp.Recv() 49 | udp.GetRecv(state) 50 | 51 | if( motiontime >= 500): 52 | sin_count += 1 53 | torque = (0 - state.motorState[d['FR_1']].q)*10.0 + (0 - state.motorState[d['FR_1']].dq)*1.0 54 | # torque = (0 - state.motorState[d['FR_1']].q)*20.0 + (0 - state.motorState[d['FR_1']].dq)*2.0 55 | # torque = 2 * sin(t*freq_rad) 56 | torque = np.fmin(np.fmax(torque, -5.0), 5.0) 57 | # torque = np.fmin(np.fmax(torque, -15.0), 15.0) 58 | 59 | 60 | cmd.motorCmd[d['FR_1']].q = PosStopF 61 | cmd.motorCmd[d['FR_1']].dq = VelStopF 62 | cmd.motorCmd[d['FR_1']].Kp = 0 63 | cmd.motorCmd[d['FR_1']].Kd = 0 64 | cmd.motorCmd[d['FR_1']].tau = torque 65 | 66 | if(motiontime > 10): 67 | safe.PowerProtect(cmd, state, 1) 68 | 69 | udp.SetSend(cmd) 70 | udp.Send() 71 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example_py/example_velocity.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | 7 | sys.path.append('../lib/python/amd64') 8 | import robot_interface as sdk 9 | 10 | 11 | if __name__ == '__main__': 12 | 13 | d = {'FR_0':0, 'FR_1':1, 'FR_2':2, 14 | 'FL_0':3, 'FL_1':4, 'FL_2':5, 15 | 'RR_0':6, 'RR_1':7, 'RR_2':8, 16 | 'RL_0':9, 'RL_1':10, 'RL_2':11 } 17 | PosStopF = math.pow(10,9) 18 | VelStopF = 16000.0 19 | HIGHLEVEL = 0xee 20 | LOWLEVEL = 0xff 21 | 22 | udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) 23 | safe = sdk.Safety(sdk.LeggedType.Go1) 24 | 25 | cmd = sdk.LowCmd() 26 | state = sdk.LowState() 27 | udp.InitCmdData(cmd) 28 | 29 | Tpi = 0 30 | motiontime = 0 31 | while True: 32 | time.sleep(0.002) 33 | motiontime += 1 34 | 35 | # print(motiontime) 36 | # print(state.imu.rpy[0]) 37 | 38 | 39 | udp.Recv() 40 | udp.GetRecv(state) 41 | 42 | if( motiontime >= 500): 43 | speed = 2 * math.sin(3*math.pi*Tpi/2000.0) 44 | 45 | # cmd.motorCmd[d['FL_2']].q = PosStopF 46 | cmd.motorCmd[d['FL_2']].q = 0 47 | cmd.motorCmd[d['FL_2']].dq = speed 48 | cmd.motorCmd[d['FL_2']].Kp = 0 49 | cmd.motorCmd[d['FL_2']].Kd = 4 50 | cmd.motorCmd[d['FL_2']].tau = 0.0 51 | 52 | Tpi += 1 53 | 54 | if(motiontime > 10): 55 | safe.PowerProtect(cmd, state, 1) 56 | 57 | udp.SetSend(cmd) 58 | udp.Send() 59 | -------------------------------------------------------------------------------- /unitree_legged_sdk/example_py/example_walk.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | 7 | sys.path.append('../lib/python/amd64') 8 | import robot_interface as sdk 9 | 10 | 11 | if __name__ == '__main__': 12 | 13 | HIGHLEVEL = 0xee 14 | LOWLEVEL = 0xff 15 | 16 | udp = sdk.UDP(HIGHLEVEL, 8080, "192.168.123.161", 8082) 17 | 18 | cmd = sdk.HighCmd() 19 | state = sdk.HighState() 20 | udp.InitCmdData(cmd) 21 | 22 | motiontime = 0 23 | while True: 24 | time.sleep(0.002) 25 | motiontime = motiontime + 1 26 | 27 | udp.Recv() 28 | udp.GetRecv(state) 29 | 30 | # print(motiontime) 31 | # print(state.imu.rpy[0]) 32 | # print(motiontime, state.motorState[0].q, state.motorState[1].q, state.motorState[2].q) 33 | # print(state.imu.rpy[0]) 34 | 35 | cmd.mode = 0 # 0:idle, default stand 1:forced stand 2:walk continuously 36 | cmd.gaitType = 0 37 | cmd.speedLevel = 0 38 | cmd.footRaiseHeight = 0 39 | cmd.bodyHeight = 0 40 | cmd.euler = [0, 0, 0] 41 | cmd.velocity = [0, 0] 42 | cmd.yawSpeed = 0.0 43 | cmd.reserve = 0 44 | 45 | # cmd.mode = 2 46 | # cmd.gaitType = 1 47 | # # cmd.position = [1, 0] 48 | # # cmd.position[0] = 2 49 | # cmd.velocity = [-0.2, 0] # -1 ~ +1 50 | # cmd.yawSpeed = 0 51 | # cmd.bodyHeight = 0.1 52 | 53 | if(motiontime > 0 and motiontime < 1000): 54 | cmd.mode = 1 55 | cmd.euler = [-0.3, 0, 0] 56 | 57 | if(motiontime > 1000 and motiontime < 2000): 58 | cmd.mode = 1 59 | cmd.euler = [0.3, 0, 0] 60 | 61 | if(motiontime > 2000 and motiontime < 3000): 62 | cmd.mode = 1 63 | cmd.euler = [0, -0.2, 0] 64 | 65 | if(motiontime > 3000 and motiontime < 4000): 66 | cmd.mode = 1 67 | cmd.euler = [0, 0.2, 0] 68 | 69 | if(motiontime > 4000 and motiontime < 5000): 70 | cmd.mode = 1 71 | cmd.euler = [0, 0, -0.2] 72 | 73 | if(motiontime > 5000 and motiontime < 6000): 74 | cmd.mode = 1 75 | cmd.euler = [0.2, 0, 0] 76 | 77 | if(motiontime > 6000 and motiontime < 7000): 78 | cmd.mode = 1 79 | cmd.bodyHeight = -0.2 80 | 81 | if(motiontime > 7000 and motiontime < 8000): 82 | cmd.mode = 1 83 | cmd.bodyHeight = 0.1 84 | 85 | if(motiontime > 8000 and motiontime < 9000): 86 | cmd.mode = 1 87 | cmd.bodyHeight = 0.0 88 | 89 | if(motiontime > 9000 and motiontime < 11000): 90 | cmd.mode = 5 91 | 92 | if(motiontime > 11000 and motiontime < 13000): 93 | cmd.mode = 6 94 | 95 | if(motiontime > 13000 and motiontime < 14000): 96 | cmd.mode = 0 97 | 98 | if(motiontime > 14000 and motiontime < 18000): 99 | cmd.mode = 2 100 | cmd.gaitType = 2 101 | cmd.velocity = [0.4, 0] # -1 ~ +1 102 | cmd.yawSpeed = 2 103 | cmd.footRaiseHeight = 0.1 104 | # printf("walk\n") 105 | 106 | if(motiontime > 18000 and motiontime < 20000): 107 | cmd.mode = 0 108 | cmd.velocity = [0, 0] 109 | 110 | if(motiontime > 20000 and motiontime < 24000): 111 | cmd.mode = 2 112 | cmd.gaitType = 1 113 | cmd.velocity = [0.2, 0] # -1 ~ +1 114 | cmd.bodyHeight = 0.1 115 | # printf("walk\n") 116 | 117 | 118 | udp.SetSend(cmd) 119 | udp.Send() 120 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/a1_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_A1_H_ 6 | #define _UNITREE_LEGGED_A1_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double a1_Hip_max = 0.802; // unit:radian ( = 46 degree) 11 | constexpr double a1_Hip_min = -0.802; // unit:radian ( = -46 degree) 12 | constexpr double a1_Thigh_max = 4.19; // unit:radian ( = 240 degree) 13 | constexpr double a1_Thigh_min = -1.05; // unit:radian ( = -60 degree) 14 | constexpr double a1_Calf_max = -0.916; // unit:radian ( = -52.5 degree) 15 | constexpr double a1_Calf_min = -2.7; // unit:radian ( = -154.5 degree) 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/aliengo_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_ALIENGO_H_ 6 | #define _UNITREE_LEGGED_ALIENGO_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) 11 | constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) 12 | constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) 13 | constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) 14 | constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) 15 | constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/comm.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_COMM_H_ 6 | #define _UNITREE_LEGGED_COMM_H_ 7 | 8 | #include 9 | #include 10 | 11 | namespace UNITREE_LEGGED_SDK 12 | { 13 | 14 | constexpr int HIGHLEVEL = 0xee; 15 | constexpr int LOWLEVEL = 0xff; 16 | constexpr int TRIGERLEVEL = 0xf0; 17 | constexpr double PosStopF = (2.146E+9f); 18 | constexpr double VelStopF = (16000.0f); 19 | extern const int HIGH_CMD_LENGTH; // sizeof(HighCmd) 20 | extern const int HIGH_STATE_LENGTH; // sizeof(HighState) 21 | extern const int LOW_CMD_LENGTH; // shorter than sizeof(LowCmd), bytes compressed LowCmd length 22 | extern const int LOW_STATE_LENGTH; // shorter than sizeof(LowState), bytes compressed LowState length 23 | 24 | #pragma pack(1) 25 | 26 | typedef struct 27 | { 28 | uint8_t off; // off 0xA5 29 | std::array reserve; 30 | } BmsCmd; 31 | 32 | typedef struct 33 | { 34 | uint8_t version_h; 35 | uint8_t version_l; 36 | uint8_t bms_status; 37 | uint8_t SOC; // SOC 0-100% 38 | int32_t current; // mA 39 | uint16_t cycle; 40 | std::array BQ_NTC; // x1 degrees centigrade 41 | std::array MCU_NTC; // x1 degrees centigrade 42 | std::array cell_vol; // cell voltage mV 43 | } BmsState; 44 | 45 | typedef struct 46 | { 47 | float x; 48 | float y; 49 | float z; 50 | } Cartesian; 51 | 52 | typedef struct 53 | { 54 | std::array quaternion; // quaternion, normalized, (w,x,y,z) 55 | std::array gyroscope; // angular velocity （unit: rad/s) (raw data) 56 | std::array accelerometer; // m/(s2) (raw data) 57 | std::array rpy; // euler angle（unit: rad) 58 | int8_t temperature; 59 | } IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift. 60 | 61 | typedef struct 62 | { 63 | uint8_t r; 64 | uint8_t g; 65 | uint8_t b; 66 | } LED; // foot led brightness: 0~255 67 | 68 | typedef struct 69 | { 70 | uint8_t mode; // motor working mode 71 | float q; // current angle (unit: radian) 72 | float dq; // current velocity (unit: radian/second) 73 | float ddq; // current acc (unit: radian/second*second) 74 | float tauEst; // current estimated output torque (unit: N.m) 75 | float q_raw; // current angle (unit: radian) 76 | float dq_raw; // current velocity (unit: radian/second) 77 | float ddq_raw; 78 | int8_t temperature; // current temperature (temperature conduction is slow that leads to lag) 79 | std::array reserve; 80 | } MotorState; // motor feedback 81 | 82 | typedef struct 83 | { 84 | uint8_t mode; // desired working mode 85 | float q; // desired angle (unit: radian) 86 | float dq; // desired velocity (unit: radian/second) 87 | float tau; // desired output torque (unit: N.m) 88 | float Kp; // desired position stiffness (unit: N.m/rad ) 89 | float Kd; // desired velocity stiffness (unit: N.m/(rad/s) ) 90 | std::array reserve; 91 | } MotorCmd; // motor control 92 | 93 | typedef struct 94 | { 95 | std::array head; 96 | uint8_t levelFlag; 97 | uint8_t frameReserve; 98 | 99 | std::array SN; 100 | std::array version; 101 | uint16_t bandWidth; 102 | IMU imu; 103 | std::array motorState; 104 | BmsState bms; 105 | std::array footForce; // force sensors 106 | std::array footForceEst; // force sensors 107 | uint32_t tick; // reference real-time from motion controller (unit: us) 108 | std::array wirelessRemote; // wireless commands 109 | uint32_t reserve; 110 | 111 | uint32_t crc; 112 | } LowState; // low level feedback 113 | 114 | typedef struct 115 | { 116 | std::array head; 117 | uint8_t levelFlag; 118 | uint8_t frameReserve; 119 | 120 | std::array SN; 121 | std::array version; 122 | uint16_t bandWidth; 123 | std::array motorCmd; 124 | BmsCmd bms; 125 | std::array wirelessRemote; 126 | uint32_t reserve; 127 | 128 | uint32_t crc; 129 | } LowCmd; // low level control 130 | 131 | typedef struct 132 | { 133 | std::array head; 134 | uint8_t levelFlag; 135 | uint8_t frameReserve; 136 | 137 | std::array SN; 138 | std::array version; 139 | uint16_t bandWidth; 140 | IMU imu; 141 | std::array motorState; 142 | BmsState bms; 143 | std::array footForce; 144 | std::array footForceEst; 145 | uint8_t mode; 146 | float progress; 147 | uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle 148 | float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking 149 | std::array position; // (unit: m), from own odometry in inertial frame, usually drift 150 | float bodyHeight; // (unit: m, default: 0.28m), 151 | std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame 152 | float yawSpeed; // (unit: rad/s), rotateSpeed in body frame 153 | std::array rangeObstacle; 154 | std::array footPosition2Body; // foot position relative to body 155 | std::array footSpeed2Body; // foot speed relative to body 156 | std::array wirelessRemote; 157 | uint32_t reserve; 158 | 159 | uint32_t crc; 160 | } HighState; // high level feedback 161 | 162 | typedef struct 163 | { 164 | std::array head; 165 | uint8_t levelFlag; 166 | uint8_t frameReserve; 167 | 168 | std::array SN; 169 | std::array version; 170 | uint16_t bandWidth; 171 | uint8_t mode; // 0. idle, default stand 1. force stand (controlled by dBodyHeight + ypr) 172 | // 2. target velocity walking (controlled by velocity + yawSpeed) 173 | // 3. target position walking (controlled by position + ypr[0]) 174 | // 4. path mode walking (reserve for future release) 175 | // 5. position stand down. 176 | // 6. position stand up 177 | // 7. damping mode 178 | // 8. recovery stand 179 | // 9. backflip 180 | // 10. jumpYaw 181 | // 11. straightHand 182 | // 12. dance1 183 | // 13. dance2 184 | 185 | uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle 186 | uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3 187 | float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking, delta value 188 | float bodyHeight; // (unit: m, default: 0.28m), delta value 189 | std::array position; // (unit: m), desired position in inertial frame 190 | std::array euler; // (unit: rad), roll pitch yaw in stand mode 191 | std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed in body frame 192 | float yawSpeed; // (unit: rad/s), rotateSpeed in body frame 193 | BmsCmd bms; 194 | std::array led; 195 | std::array wirelessRemote; 196 | uint32_t reserve; 197 | 198 | uint32_t crc; 199 | } HighCmd; // high level control 200 | 201 | #pragma pack() 202 | 203 | typedef struct 204 | { 205 | unsigned long long TotalCount; // total loop count 206 | unsigned long long SendCount; // total send count 207 | unsigned long long RecvCount; // total receive count 208 | unsigned long long SendError; // total send error 209 | unsigned long long FlagError; // total flag error 210 | unsigned long long RecvCRCError; // total reveive CRC error 211 | unsigned long long RecvLoseError; // total lose package count 212 | } UDPState; // UDP communication state 213 | 214 | } 215 | 216 | #endif 217 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/go1_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_GO1_H_ 6 | #define _UNITREE_LEGGED_GO1_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double go1_Hip_max = 1.047; // unit:radian ( = 60 degree) 11 | constexpr double go1_Hip_min = -1.047; // unit:radian ( = -60 degree) 12 | constexpr double go1_Thigh_max = 2.966; // unit:radian ( = 170 degree) 13 | constexpr double go1_Thigh_min = -0.663; // unit:radian ( = -38 degree) 14 | constexpr double go1_Calf_max = -0.837; // unit:radian ( = -48 degree) 15 | constexpr double go1_Calf_min = -2.721; // unit:radian ( = -156 degree) 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/joystick.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | *****************************************************************/ 4 | #ifndef _UNITREE_LEGGED_JOYSTICK_H_ 5 | #define _UNITREE_LEGGED_JOYSTICK_H_ 6 | 7 | #include 8 | // 16b 9 | typedef union { 10 | struct { 11 | uint8_t R1 :1; 12 | uint8_t L1 :1; 13 | uint8_t start :1; 14 | uint8_t select :1; 15 | uint8_t R2 :1; 16 | uint8_t L2 :1; 17 | uint8_t F1 :1; 18 | uint8_t F2 :1; 19 | uint8_t A :1; 20 | uint8_t B :1; 21 | uint8_t X :1; 22 | uint8_t Y :1; 23 | uint8_t up :1; 24 | uint8_t right :1; 25 | uint8_t down :1; 26 | uint8_t left :1; 27 | } components; 28 | uint16_t value; 29 | } xKeySwitchUnion; 30 | 31 | // 40 Byte (now used 24B) 32 | typedef struct { 33 | uint8_t head[2]; 34 | xKeySwitchUnion btn; 35 | float lx; 36 | float rx; 37 | float ry; 38 | float L2; 39 | float ly; 40 | 41 | uint8_t idle[16]; 42 | } xRockerBtnDataStruct; 43 | 44 | #endif // _UNITREE_LEGGED_JOYSTICK_H_ -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/loop.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_LOOP_H_ 6 | #define _UNITREE_LEGGED_LOOP_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace UNITREE_LEGGED_SDK 18 | { 19 | 20 | constexpr int THREAD_PRIORITY = 95; // real-time priority 21 | 22 | typedef boost::function Callback; 23 | 24 | class Loop { 25 | public: 26 | Loop(std::string name, float period, int bindCPU = -1):_name(name), _period(period), _bindCPU(bindCPU) {} 27 | ~Loop(); 28 | void start(); 29 | void shutdown(); 30 | virtual void functionCB() = 0; 31 | 32 | private: 33 | void entryFunc(); 34 | 35 | std::string _name; 36 | float _period; 37 | int _bindCPU; 38 | bool _bind_cpu_flag = false; 39 | bool _isrunning = false; 40 | std::thread _thread; 41 | }; 42 | 43 | /* 44 | period unit:second 45 | bindCPU change the CPU affinity of this thread 46 | */ 47 | class LoopFunc : public Loop { 48 | public: 49 | LoopFunc(std::string name, float period, const Callback& _cb) 50 | : Loop(name, period), _fp(_cb){} 51 | LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) 52 | : Loop(name, period, bindCPU), _fp(_cb){} 53 | void functionCB() { (_fp)(); } 54 | private: 55 | boost::function _fp; 56 | }; 57 | 58 | 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/quadruped.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_QUADRUPED_H_ 6 | #define _UNITREE_LEGGED_QUADRUPED_H_ 7 | 8 | #include 9 | 10 | using namespace std; 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | 15 | enum class LeggedType { 16 | Aliengo, 17 | A1, 18 | Go1, 19 | B1 20 | }; 21 | 22 | 23 | string VersionSDK(); 24 | int InitEnvironment(); // memory lock 25 | 26 | // definition of each leg and joint 27 | constexpr int FR_ = 0; // leg index 28 | constexpr int FL_ = 1; 29 | constexpr int RR_ = 2; 30 | constexpr int RL_ = 3; 31 | 32 | constexpr int FR_0 = 0; // joint index 33 | constexpr int FR_1 = 1; 34 | constexpr int FR_2 = 2; 35 | 36 | constexpr int FL_0 = 3; 37 | constexpr int FL_1 = 4; 38 | constexpr int FL_2 = 5; 39 | 40 | constexpr int RR_0 = 6; 41 | constexpr int RR_1 = 7; 42 | constexpr int RR_2 = 8; 43 | 44 | constexpr int RL_0 = 9; 45 | constexpr int RL_1 = 10; 46 | constexpr int RL_2 = 11; 47 | 48 | } 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/safety.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_SAFETY_H_ 6 | #define _UNITREE_LEGGED_SAFETY_H_ 7 | 8 | #include "comm.h" 9 | #include "quadruped.h" 10 | 11 | namespace UNITREE_LEGGED_SDK 12 | { 13 | 14 | class Safety{ 15 | public: 16 | Safety(LeggedType type); 17 | ~Safety(); 18 | void PositionLimit(LowCmd&); // only effect under Low Level control in Position mode 19 | int PowerProtect(LowCmd&, LowState&, int); /* only effect under Low Level control, input factor: 1~10, 20 | means 10%~100% power limit. If you are new, then use 1; if you are familiar, 21 | then can try bigger number or even comment this function. */ 22 | int PositionProtect(LowCmd&, LowState&, double limit = 0.087); // default limit is 5 degree 23 | private: 24 | int WattLimit, Wcount; // Watt. When limit to 100, you can triger it with 4 hands shaking. 25 | double Hip_max, Hip_min, Thigh_max, Thigh_min, Calf_max, Calf_min; 26 | }; 27 | 28 | 29 | } 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/udp.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_UDP_H_ 6 | #define _UNITREE_LEGGED_UDP_H_ 7 | 8 | #include "comm.h" 9 | #include "unitree_legged_sdk/quadruped.h" 10 | #include 11 | #include 12 | 13 | /* 14 | UDP critical configuration: 15 | 16 | 1. initiativeDisconnect: if need disconnection after connected, another ip/port can access after disconnection 17 | 18 | /--- block will block till data come 19 | 2. recvType ---- block + timeout will block till data come or timeout 20 | \--- non block if no data will return immediately 21 | 22 | /--- Y ip/port will be set later 23 | 3. setIpPort: 24 | \--- N ip/port not specified, as a server wait for connect 25 | */ 26 | 27 | 28 | namespace UNITREE_LEGGED_SDK 29 | { 30 | 31 | constexpr int UDP_CLIENT_PORT = 8080; // local port 32 | constexpr int UDP_SERVER_PORT = 8007; // target port 33 | constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10"; // target IP address 34 | constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161"; // target IP address 35 | 36 | typedef enum { 37 | nonBlock = 0x00, 38 | block = 0x01, 39 | blockTimeout = 0x02, 40 | } RecvEnum; 41 | 42 | // Notice: User defined data(like struct) should add crc(4Byte) at the end. 43 | class UDP { 44 | public: 45 | UDP(uint8_t level, uint16_t localPort, const char* targetIP, uint16_t targetPort); // udp use dafault length according to level 46 | UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort, 47 | int sendLength, int recvLength, bool initiativeDisconnect = false, RecvEnum recvType = RecvEnum::nonBlock); 48 | UDP(uint16_t localPort, 49 | int sendLength, int recvLength, bool initiativeDisconnect = false, RecvEnum recvType = RecvEnum::nonBlock, bool setIpPort = false); 50 | ~UDP(); 51 | 52 | void SetIpPort(const char* targetIP, uint16_t targetPort); // if not indicated at constructor function 53 | void SetRecvTimeout(int time); // use in RecvEnum::blockTimeout (unit: ms) 54 | 55 | void SetDisconnectTime(float callback_dt, float disconnectTime); // initiativeDisconnect = true, disconnect for another IP to connect 56 | void SetAccessibleTime(float callback_dt, float accessibleTime); // check if can access data 57 | 58 | int Send(); 59 | int Recv(); // directly save in buffer 60 | 61 | void InitCmdData(HighCmd& cmd); 62 | void InitCmdData(LowCmd& cmd); 63 | int SetSend(char*); 64 | int SetSend(HighCmd&); 65 | int SetSend(LowCmd&); 66 | void GetRecv(char*); 67 | void GetRecv(HighState&); 68 | void GetRecv(LowState&); 69 | 70 | UDPState udpState; 71 | char* targetIP; 72 | uint16_t targetPort; 73 | char* localIP; 74 | uint16_t localPort; 75 | bool accessible = false; // can access or not 76 | 77 | private: 78 | void init(uint16_t localPort, const char* targetIP = NULL, uint16_t targetPort = 0); 79 | 80 | int sockFd; 81 | bool connected; // udp works with connect() function, rather than server mode 82 | int sendLength; 83 | int recvLength; 84 | int lose_recv; 85 | 86 | char* recvBuf; 87 | char* recvAvaliable; 88 | char* sendBuf; 89 | pthread_mutex_t sendMutex; 90 | pthread_mutex_t recvMutex; 91 | pthread_mutex_t udpMutex; 92 | 93 | bool nonblock = true; 94 | int blockTimeout = -1; // use time out method or not, (unit: ms) 95 | bool initiativeDisconnect = false; // 96 | 97 | }; 98 | 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /unitree_legged_sdk/include/unitree_legged_sdk/unitree_legged_sdk.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_SDK_H_ 6 | #define _UNITREE_LEGGED_SDK_H_ 7 | 8 | #include "comm.h" 9 | #include "safety.h" 10 | #include "udp.h" 11 | #include "loop.h" 12 | #include "quadruped.h" 13 | #include "joystick.h" 14 | #include 15 | 16 | #define UT UNITREE_LEGGED_SDK //short name 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_legged_sdk/lib/cpp/amd64/libunitree_legged_sdk.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TextZip/go1-rl-kit/aaa2f34a362065935697d960f2b8ea880880e090/unitree_legged_sdk/lib/cpp/amd64/libunitree_legged_sdk.a -------------------------------------------------------------------------------- /unitree_legged_sdk/lib/cpp/arm64/libunitree_legged_sdk.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TextZip/go1-rl-kit/aaa2f34a362065935697d960f2b8ea880880e090/unitree_legged_sdk/lib/cpp/arm64/libunitree_legged_sdk.a -------------------------------------------------------------------------------- /unitree_legged_sdk/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TextZip/go1-rl-kit/aaa2f34a362065935697d960f2b8ea880880e090/unitree_legged_sdk/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /unitree_legged_sdk/lib/python/arm64/robot_interface.cpython-37m-aarch64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TextZip/go1-rl-kit/aaa2f34a362065935697d960f2b8ea880880e090/unitree_legged_sdk/lib/python/arm64/robot_interface.cpython-37m-aarch64-linux-gnu.so -------------------------------------------------------------------------------- /unitree_legged_sdk/lib/python/arm64/robot_interface.cpython-38-aarch64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TextZip/go1-rl-kit/aaa2f34a362065935697d960f2b8ea880880e090/unitree_legged_sdk/lib/python/arm64/robot_interface.cpython-38-aarch64-linux-gnu.so -------------------------------------------------------------------------------- /weights/asym_full_model_10000.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TextZip/go1-rl-kit/aaa2f34a362065935697d960f2b8ea880880e090/weights/asym_full_model_10000.pt --------------------------------------------------------------------------------