当前位置：首页 > news >正文

“Hopf Oscillator-Based Gait Transition for A Quadruped Robot“代码复现

news 来源：原创 2024/5/20 10:41:23

paper链接：https://ieeexplore.ieee.org/abstract/document/7090642/

import math
import numpy as np
import matplotlib.pyplot as plt# 设置中文显示
plt.rcParams['font.sans-serif'] = ['SimHei']  # 设置中文字体为黑体
plt.rcParams['axes.unicode_minus'] = False  # 解决负号'-'显示为方块的问题class CPG(object):def __init__(self, gait=0):self.gait_num = 3self.gait_names = ["Walk","Trot", "Pace", "Bound"]self.labels = ['FL', 'FR', 'HL', 'HR']# CPG构建基本参数self._alpha = 100self.leg_num = 4self.gait = gaitself.mu_ = -1self._a = 50self.u1 = 0self.u2 = 0# 负载系数对相关参数影响self.BEAT_ = [0.75, 0.5, 0.5, 0.5]self.period_ = [0.6, 0.5, 0.5, 0.4]self._beta = self.BEAT_[0]self._t = self.period_[0]# 时间self.t_step = 0.001self.point_x = np.zeros(self.leg_num)self.point_y = np.zeros(self.leg_num)self.PHASE = [[0 * 2 * np.pi, 0.5 * 2 * np.pi, 0.25 * 2 * np.pi, 0.75 * 2 * np.pi],[0 * 2 * np.pi, 0.5 * 2 * np.pi, 0.5 * 2 * np.pi, 0 * 2 * np.pi],[0 * 2 * np.pi, 0.5 * 2 * np.pi, 0 * 2 * np.pi, 0.5 * 2 * np.pi],[0 * 2 * np.pi, 0 * 2 * np.pi, 0.5 * 2 * np.pi, 0.5 * 2 * np.pi],]# 相对相位矩阵self.R_cell = np.zeros(shape=(self.leg_num, self.leg_num, 2, 2))self.Phi = None# 初始值，非0即可self.leg_x = Noneself.leg_y = Noneself.reset()# 计数，什么时候完成相位同步self.count_ = 0# 计数，什么时候结束步态转换self.t_count_ = 0self.init_phase()def init_phase(self):for _ in range(700):self.step()def reset(self):self._beta = self.BEAT_[self.gait]self._t = self.period_[self.gait]self.Phi = self.PHASE[self.gait]self.leg_x = np.ones(self.leg_num) * 0.0001self.leg_y = np.ones(self.leg_num) * 0.0001self.update_rotation_matrix()def next_gait(self):self.gait = (self.gait + 1) % self.gait_numself.reset()def update_rotation_matrix(self):        for i in np.arange(0, self.leg_num):for j in np.arange(0, self.leg_num):self.R_cell[j, i] = np.array([[np.cos(self.Phi[i] - self.Phi[j]), - np.sin(self.Phi[i] - self.Phi[j])],[np.sin(self.Phi[i] - self.Phi[j]), np.cos(self.Phi[i] - self.Phi[j])]])def transit_gait(self, tau=0):self.t_count_ = self.t_count_ + 1# 步态在一个周期内完成转换if tau == 0:# walk to trot# beta/φ2从0.75变换到0.5# walk步态周期为0.6，假设在1s内完成转换，共200步，每步变换1/800=0.00125transit_step = 0.00125self.Phi[0] = 0# self.Phi[1] = 0.5phi2_ = 0.75 - transit_step * self.t_count_self.Phi[2] = phi2_ * 2 * np.piself.Phi[3] = (phi2_ - 0.5) * 2 * np.pi# 更新占空比参数和周期参数self._beta = phi2_            else:# trot to gallop# φ1从0.5到0，0.5/200=1/400=0.0025transit_step = 0.0025phi1_ = 0.5 - transit_step * self.t_count_self.Phi[1] = phi1_ * 2 * np.piself.Phi[3] = (0.5 - phi1_) * 2 * np.piif self.t_count_ >= 200:self.t_count_ = 0if tau == 0:self._t = 0.5else:self._t = 0.4self.update_rotation_matrix()def step(self):if self.count_ > 400:self.mu_ = 1self.count_  = self.count_ + 1if self.count_ > 1200 and self.count_ <= 1400:self.transit_gait(tau=0)if self.count_ > 1800 and self.count_ <= 2000:self.transit_gait(tau=1)for _ in range(5):for i in np.arange(0, self.leg_num):r_pow = (self.leg_x[i] - self.u1) ** 2 + (self.leg_y[i] - self.u2) ** 2W = math.pi / (self._beta*self._t*(math.exp(-self._a*self.leg_y[i]) + 1)) + math.pi / ((1-self._beta)*self._t*(math.exp(self._a*self.leg_y[i]) + 1))V = np.matmul(np.array([[self._alpha * (self.mu_ - r_pow), - W], [W, self._alpha * (self.mu_ - r_pow)]]),np.array([[self.leg_x[i] - self.u1], [self.leg_y[i] - self.u2]])) + \np.matmul(self.R_cell[0, i], np.array([[self.leg_x[0] - self.u1], [self.leg_y[0] - self.u1]])) + \np.matmul(self.R_cell[1, i], np.array([[self.leg_x[1] - self.u2], [self.leg_y[1] - self.u2]])) + \np.matmul(self.R_cell[2, i], np.array([[self.leg_x[2] - self.u1], [self.leg_y[2] - self.u2]])) + \np.matmul(self.R_cell[3, i], np.array([[self.leg_x[3] - self.u1], [self.leg_y[3] - self.u2]]))self.leg_x[i] = self.leg_x[i] + V[0, 0] * self.t_stepself.leg_y[i] = self.leg_y[i] + V[1, 0] * self.t_stepfor i in range(0, self.leg_num):self.point_x[i] = self.leg_x[i]self.point_y[i] = self.leg_y[i]if self.leg_y[i] > 0:self.point_y[i] = 0else:self.point_y[i] = -self.leg_y[i]return np.concatenate([[h, k] for h, k in zip(self.point_x, self.point_y)])if __name__ == '__main__':cpg = CPG(0)step_num = 1800plt.rcParams.update({'font.size': 20})fig1 = plt.figure(figsize=(9, 6))plt.subplots_adjust(left=None, bottom=None, right=None, top=None, wspace=None, hspace=0.5)phases = np.stack(np.array([cpg.step() for _ in range(step_num)]), axis=1)ax = plt.subplot(4, 1, 1)leg_labels = ['FL', 'FR', 'HL', 'HR']for i in range(4):ax.plot(np.arange(0, step_num) * 0.005, phases[2 * i, :], linewidth=2, label=leg_labels[i])ax.set_xlim(0,step_num * 0.005)ax.legend(prop={'size': 14}, loc= 'lower right')plt.show()

代码实现了从静止到行走（walk）到小跑（trot）再到飞奔（gallop）步态的转换。具体实现细节和解释有时间再写👌。