MATの実装

.gitignore

@@ -44,3 +44,4 @@ build/
 .idea/
 __pycache__/
 venv/
+/controllers/mat_train/results/

controllers/GankenKun_soccer/GankenKun/foot_step_planner.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+# 
+# foot step planner
+
+import math
+
+class foot_step_planner():
+  def __init__(self, max_stride_x, max_stride_y, max_stride_th, period, width):
+    self.max_stride_x = max_stride_x
+    self.max_stride_y = max_stride_y
+    self.max_stride_th = max_stride_th
+    self.period = period
+    self.width = width
+
+  def calculate(self, goal_x, goal_y, goal_th, current_x, current_y, current_th, next_support_leg, status):
+    # calculate the number of foot step
+    time = 0.0
+    goal_distance = math.sqrt((goal_x-current_x)**2 + (goal_y-current_y)**2)
+    step_x  = abs(goal_x  - current_x )/self.max_stride_x
+    step_y  = abs(goal_y  - current_y )/self.max_stride_y
+    step_th = abs(goal_th - current_th)/self.max_stride_th
+    max_step = max(step_x, step_y, step_th, 1)
+    stride_x  = (goal_x  - current_x )/max_step
+    stride_y  = (goal_y  - current_y )/max_step
+    stride_th = (goal_th - current_th)/max_step
+
+    # first step
+    foot_step = []
+    if status == 'start':
+      foot_step += [[0.0, current_x, current_y, current_th, 'both', 0]]
+      time += self.period
+    if next_support_leg == 'right':
+      foot_step += [[time, current_x, -self.width+current_y, current_th, next_support_leg, -self.width]]
+      next_support_leg = 'left'
+    elif next_support_leg == 'left':
+      foot_step += [[time, current_x,  self.width+current_y, current_th, next_support_leg, self.width]]
+      next_support_leg = 'right'
+
+    # walking
+    counter = 0
+    while True:
+      counter += 1
+      diff_x  = abs(goal_x  - current_x )
+      diff_y  = abs(goal_y  - current_y )
+      diff_th = abs(goal_th - current_th)
+#      if diff_x <= self.max_stride_x and diff_y <= self.max_stride_y and diff_th <= self.max_stride_th:
+      if counter == 2:
+        break
+      time += self.period
+      next_x  = current_x  + stride_x
+      next_y  = current_y  + stride_y
+      next_th = current_th + stride_th
+      if next_support_leg == 'right':
+        foot_step += [[time, next_x, next_y-self.width, next_th, next_support_leg, -self.width]]
+        next_support_leg = 'left'
+      elif next_support_leg == 'left':
+        foot_step += [[time, next_x, next_y+self.width, next_th, next_support_leg, self.width]]
+        next_support_leg = 'right'
+      current_x, current_y, current_th = next_x, next_y, next_th
+
+    # last step
+#    next_x, next_y, next_th = goal_x, goal_y, goal_th
+    if not status == 'stop':
+      time += self.period
+      if next_support_leg == 'right':
+        foot_step += [[time, next_x, next_y-self.width, next_th, next_support_leg, -self.width]]
+      elif next_support_leg == 'left':
+        foot_step += [[time, next_x, next_y+self.width, next_th, next_support_leg, self.width]]
+      time += self.period
+      next_support_leg = 'both'
+      foot_step += [[time, next_x, next_y, next_th, next_support_leg, 0]]
+      time += self.period
+      foot_step += [[time, next_x, next_y, next_th, next_support_leg, 0]]
+      time += 100
+      foot_step += [[time, next_x, next_y, next_th, next_support_leg, 0]]
+
+    return foot_step
+
+if __name__ == '__main__':
+  planner = foot_step_planner(0.06, 0.04, 0.1, 0.34, 0.044)
+  foot_step = planner.calculate(1.0, 0.0, 0.5, 0.5, 0.0, 0.1, 'right', 'start')
+  for i in foot_step:
+    print(i)

controllers/GankenKun_soccer/GankenKun/kinematics.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+# 
+# solving kinematics for the GankenKun
+
+import math
+
+class kinematics():
+  L1  = 0.118
+  L12 = 0.023
+  L2  = 0.118
+  L3  = 0.043
+  OFFSET_W   = 0.044
+  OFFSET_X   = -0.0275
+  def __init__(self, motorNames):
+    self.motorNames = motorNames
+
+  def solve_ik(self, left_foot, right_foot, current_angles):
+    joint_angles = current_angles.copy()
+    l_x, l_y, l_z, l_roll, l_pitch, l_yaw = left_foot
+    l_x -= self.OFFSET_X
+    l_y -= self.OFFSET_W
+    l_z = self.L1 + self.L12 + self.L2 + self.L3 - l_z
+    l_x2 =  l_x * math.cos(l_yaw) + l_y * math.sin(l_yaw)
+    l_y2 = -l_x * math.sin(l_yaw) + l_y * math.cos(l_yaw)
+    l_z2 =  l_z - self.L3
+    waist_roll = math.atan2(l_y2, l_z2)
+    l2 = l_y2**2 + l_z2**2
+    l_z3 = math.sqrt(max(l2 - l_x2**2, 0.0)) - self.L12
+    pitch = math.atan2(l_x2, l_z3)
+    l = math.sqrt(l_x2**2 + l_z3**2)
+    knee_disp = math.acos(min(max(l/(2.0*self.L1),-1.0),1.0))
+    waist_pitch = - pitch - knee_disp
+    knee_pitch  = - pitch + knee_disp
+    joint_angles[self.motorNames.index('left_waist_yaw_joint'       )] = -l_yaw
+    joint_angles[self.motorNames.index('left_waist_roll_joint [hip]')] = waist_roll
+    joint_angles[self.motorNames.index('left_waist_pitch_joint'     )] = -waist_pitch
+    joint_angles[self.motorNames.index('left_knee_pitch_joint'      )] = -knee_pitch
+    joint_angles[self.motorNames.index('left_ankle_pitch_joint'     )] = l_pitch
+    joint_angles[self.motorNames.index('left_ankle_roll_joint'      )] = l_roll - waist_roll
+
+    r_x, r_y, r_z, r_roll, r_pitch, r_yaw = right_foot
+    r_x -= self.OFFSET_X
+    r_y += self.OFFSET_W
+    r_z = self.L1 + self.L12 + self.L2 + self.L3 - r_z
+    r_x2 =  r_x * math.cos(r_yaw) + r_y * math.sin(r_yaw)
+    r_y2 = -r_x * math.sin(r_yaw) + r_y * math.cos(r_yaw)
+    r_z2 =  r_z - self.L3
+    waist_roll = math.atan2(r_y2, r_z2)
+    r2 = r_y2**2 + r_z2**2
+    r_z3 = math.sqrt(max(r2 - r_x2**2, 0.0)) - self.L12
+    pitch = math.atan2(r_x2, r_z3)
+    l = math.sqrt(r_x2**2 + r_z3**2)
+    knee_disp = math.acos(min(max(l/(2.0*self.L1),-1.0),1.0))
+    waist_pitch = - pitch - knee_disp
+    knee_pitch  = - pitch + knee_disp
+    joint_angles[self.motorNames.index('right_waist_yaw_joint'       )] = -r_yaw
+    joint_angles[self.motorNames.index('right_waist_roll_joint [hip]')] = waist_roll
+    joint_angles[self.motorNames.index('right_waist_pitch_joint'     )] = -waist_pitch
+    joint_angles[self.motorNames.index('right_knee_pitch_joint'      )] = -knee_pitch
+    joint_angles[self.motorNames.index('right_ankle_pitch_joint'     )] = r_pitch
+    joint_angles[self.motorNames.index('right_ankle_roll_joint'      )] = r_roll - waist_roll
+
+    return joint_angles
+
+if __name__ == '__main__':
+  TIME_STEP = 0.001
+  physicsClient = p.connect(p.GUI)
+  p.setGravity(0, 0, -9.8)
+  p.setTimeStep(TIME_STEP)
+
+  planeId = p.loadURDF("../URDF/plane.urdf", [0, 0, 0])
+  RobotId = p.loadURDF("../URDF/gankenkun.urdf", [0, 0, 0])
+  kine = kinematics(RobotId)
+
+  index = {p.getBodyInfo(RobotId)[0].decode('UTF-8'):-1,}
+  for id in range(p.getNumJoints(RobotId)):
+    index[p.getJointInfo(RobotId, id)[12].decode('UTF-8')] = id
+
+  left_foot_pos0,  left_foot_ori0  = p.getLinkState(RobotId, index['left_foot_link' ])[:2]
+  right_foot_pos0, right_foot_ori0 = p.getLinkState(RobotId, index['right_foot_link'])[:2]
+
+  index_dof = {p.getBodyInfo(RobotId)[0].decode('UTF-8'):-1,}
+  for id in range(p.getNumJoints(RobotId)):
+    index_dof[p.getJointInfo(RobotId, id)[12].decode('UTF-8')] = p.getJointInfo(RobotId, id)[3] - 7
+
+  joint_angles = []
+  for id in range(p.getNumJoints(RobotId)):
+    if p.getJointInfo(RobotId, id)[3] > -1:
+      joint_angles += [0,]
+
+  height = 0.0
+  velocity = 0.1
+  while p.isConnected():
+    height += velocity * TIME_STEP
+    body_pos, body_ori = p.getLinkState(RobotId, index['body_link'])[:2]
+    tar_left_foot_pos  = [left_foot_pos0[0] , left_foot_pos0[1] , height, 0.0, 0.0, 0.0]
+    tar_right_foot_pos = [right_foot_pos0[0], right_foot_pos0[1], height, 0.0, 0.0, 0.0]
+    joint_angles = kine.solve_ik(tar_left_foot_pos, tar_right_foot_pos, joint_angles)
+
+    for id in range(p.getNumJoints(RobotId)):
+      qIndex = p.getJointInfo(RobotId, id)[3]
+      if qIndex > -1:
+        p.setJointMotorControl2(RobotId, id, p.POSITION_CONTROL, joint_angles[qIndex-7])
+
+    if height <= 0.0 or height >= 0.1:
+      velocity *= -1.0
+
+    p.stepSimulation()
+#    sleep(TIME_STEP)
+

controllers/GankenKun_soccer/GankenKun/preview_control.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+# 
+# preview control
+
+import math
+import numpy as np
+import control
+import control.matlab
+import csv
+
+class preview_control():
+  def __init__(self, dt, period, z, Q = 1.0e+8, H = 1.0):
+    self.dt = dt
+    self.period = period
+    G = 9.8
+    A = np.matrix([
+      [0.0, 1.0, 0.0],
+      [0.0, 0.0, 1.0],
+      [0.0, 0.0, 0.0]])
+    B = np.matrix([[0.0], [0.0], [1.0]])
+    C = np.matrix([[1.0, 0.0, -z/G]])
+    D = 0
+    sys = control.matlab.ss(A, B, C, D)
+    sys_d = control.c2d(sys, dt)
+    self.A_d, self.B_d, self.C_d, D_d = control.matlab.ssdata(sys_d)
+    E_d = np.matrix([[dt], [1.0], [0.0]])
+    Zero = np.matrix([[0.0], [0.0], [0.0]])
+    Phai = np.block([[1.0, -self.C_d * self.A_d], [Zero, self.A_d]])
+    G = np.block([[-self.C_d*self.B_d], [self.B_d]])
+    GR = np.block([[1.0], [Zero]])
+    Gd = np.block([[-self.C_d*E_d], [E_d]])
+    Qm = np.zeros((4,4))
+    Qm[0][0] = Q
+    P = control.dare(Phai, G, Qm, H)[0]
+    self.F = -np.linalg.inv(H+G.transpose()*P*G)*G.transpose()*P*Phai
+    xi = (np.eye(4)-G*np.linalg.inv(H+G.transpose()*P*G)*G.transpose()*P)*Phai;
+    self.f = []
+    self.xp, self.yp = np.matrix([[0.0],[0.0],[0.0]]), np.matrix([[0.0],[0.0],[0.0]])
+    self.ux, self.uy = 0.0, 0.0
+    for i in range(0,round(period/dt)):
+      self.f += [-np.linalg.inv(H+G.transpose()*P*G)*G.transpose()*np.linalg.matrix_power(xi.transpose(),i-1)*P*GR]
+
+  def set_param(self, t, current_x, current_y, foot_plan, pre_reset = False):
+    x, y = current_x.copy(), current_y.copy()
+    if pre_reset == True:
+      self.xp, self.yp = x.copy(), y.copy()
+      self.ux, self.uy = 0.0, 0.0
+    COG_X = []
+    for i in range(round((foot_plan[1][0] - t)/self.dt)):
+      px, py = self.C_d * x, self.C_d * y
+      ex, ey = foot_plan[0][1] - px, foot_plan[0][2] - py
+      X, Y = np.block([[ex], [x - self.xp]]), np.block([[ey], [y - self.yp]])
+      self.xp, self.yp = x.copy(), y.copy()
+      dux, duy = self.F * X, self.F * Y
+      index = 1
+      for j in range(1,round(self.period/self.dt)-1):
+        if round((i+j)+t/self.dt) >= round(foot_plan[index][0]/self.dt):
+          dux += self.f[j] * (foot_plan[index][1]-foot_plan[index-1][1])
+          duy += self.f[j] * (foot_plan[index][2]-foot_plan[index-1][2])
+          index += 1
+      self.ux, self.uy = self.ux + dux, self.uy + duy
+      x, y = self.A_d * x + self.B_d * self.ux, self.A_d * y + self.B_d * self.uy
+      COG_X += [np.block([x[0][0], y[0][0], px[0][0], py[0][0]])]
+    return COG_X, x, y
+
+if __name__ == '__main__':
+  pc = preview_control(0.01, 1.0, 0.27)
+  foot_step = [[0, 0, 0], [0.34, 0, 0.06+0.00], [0.68, 0.05, -0.06+0.02], [1.02, 0.10, 0.06+0.04], [1.36, 0.15, -0.06+0.06], [1.7, 0.20, 0.06+0.08], [2.04, 0.25, 0.0+0.10], [2.72, 0.25, 0.0+0.10], [100, 0.25, 0.0+0.10]]
+  x, y = np.matrix([[0.0],[0.0],[0.0]]), np.matrix([[0.0],[0.0],[0.0]])
+  with open('result.csv', mode='w') as f:
+    f.write('')
+  t = 0
+  while True:
+    if len(foot_step) <= 2:
+      break
+    cog, x, y = pc.set_param(t, x, y, foot_step)
+    with open('result.csv', mode='a') as f:
+      writer = csv.writer(f)
+      for i in cog:
+        writer.writerow(i.tolist()[0])
+      f.write('\n')
+    del foot_step[0]
+    t = foot_step[0][0]
+#  print(pc.set_param([0,0], [0,0], [0,0], foot_step))