Python_Robot

#!/usr/bin/env python

# coding: utf-8

# In[1]:

import numpy as np

import math

import sys

import matplotlib.pyplot as plt

from mpl_toolkits.mplot3d import Axes3D

# In[2]:

#For Dynamic simulation dvo, w, vo　追加

class uLINK:

  def __init__(self, name, sister, child, mother, m,  b ,a ,q,R,p, dq, ddq, c, I, Ir, gr, u, w, vo, dw, dvo):

    self.name = name

    self.sister = sister

    self.child = child

    self.mother = mother

    self.m = m

    self.b = b

    self.a = a

    self.q = q

    self.R = R

    self.p = p

    self.dq =dq

    self.ddq = ddq

    self.c  = c

    self.I  = I

    self.u  = u

    self.w = w 

    self.vo = vo 

    self.dw = dw 

    self.dvo = dvo

# In[3]:

def FindMother(j):

    if j != 0:

        if j == 1:

            arr[j].mother = 0;

        if arr[j].child != 0:

            arr[arr[j].child].mother = j;

            FindMother(arr[j].child);

        if arr[j].sister != 0:

            arr[arr[j].sister].mother = arr[j].mother;

            FindMother(arr[j].sister);

# In[4]:

def TotalMass(x):

  if x == 0:

    m = 0

    return m

  else:

    m = arr[x].m + TotalMass(arr[x].sister) + TotalMass(arr[x].child)

    return m

# In[5]:

def Rodrigues(w,dt):

    norm_w = np.linalg.norm(w);

    if norm_w < sys.float_info.epsilon :

        R = np.eye(3);

    else :

        th = norm_w * dt;

        wn = w/norm_w;

        w_wedge = np.array([ [0, -wn[2], wn[1]],[wn[2], 0, -wn[0]],[-wn[1], wn[0], 0]]);

        R = np.eye(3) + w_wedge * math.sin(th) + np.matmul(w_wedge,w_wedge) * (1-math.cos(th));

    return R

# In[6]:

def ForwardKinematics(j):

    if j == 0: return

    if j != 1:

        mom = arr[j].mother;

        arr[j].p = np.matmul(arr[mom].R, arr[j].b) + arr[mom].p #　絶対座標系の位置に回転行列で戻してから位置を加えていくことで絶対座標系での関節位置を求める

        ROD = Rodrigues(arr[j].a, arr[j].q); #　回転行列をロドリゲスの関数から導く

        arr[j].R = np.matmul(arr[mom].R, ROD); #　一つ手前（親）のリンクの回転行列をかけて，絶対座標系でのリンクの姿勢行列を求める

    ForwardKinematics(arr[j].sister);

    ForwardKinematics(arr[j].child);

# In[7]:

def RPY2R(rpy): # 入力rpyはnumpyのarrayで1行3列のベクトル

    roll = rpy[0]; pitch = rpy[1]; yaw = rpy[2];

    Cphi = math.cos(roll);  Sphi = math.sin(roll);

    Cthe = math.cos(pitch); Sthe = math.sin(pitch);

    Cpsi = math.cos(yaw);   Spsi = math.sin(yaw);

    R = [[Cpsi*Cthe,  -Spsi*Cphi+Cpsi*Sthe*Sphi,  Spsi*Sphi+Cpsi*Sthe*Cphi],

       [Spsi*Cthe,  Cpsi*Cphi+Spsi*Sthe*Sphi,   -Cpsi*Sphi+Spsi*Sthe*Cphi],

       [-Sthe,      Cthe*Sphi,                  Cthe*Cphi]                 ]

    return R

# In[8]:

def FindRoute(to): # 入力toには経路を知りたいリンクのIDを与える

#return the list of joint number connecting ROOT to 'to'

    i = arr[to].mother;

#if i == 0

#    idx = [];    # search failed

#    fprintf('FindRoute: search failed\n');

#else

    if i == 1:

        idx = [to];

    else:

        idx = FindRoute(i) + [to];

    return idx

# In[9]:

def rot2omega(R):

    # T.Sugihara "Solvability-unconcerned Inverse Kinemacics based on 

    # Levenberg-Marquardt method with Robust Damping," Humanoids 2009

    #el = [R[3,2]-R[2,3], R[1,3]-R[3,1], R[2,1]-R[1,2]];

    el = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]]);

    norm_el = np.linalg.norm(el);

    if norm_el >  sys.float_info.epsilon :

        w = math.atan2(norm_el, np.trace(R)-1)/norm_el * el;

    elif R[0,0]>0 and R[1,1]>0 and R[2,2]>0 :

        w = [0, 0, 0];

    else :

        w = math.pi/2*[R(0,0)+1, R(1,1)+1, R(2,2)+1];

    return w # [roll, pitch, yaw]の単位はrad/sec

# In[10]:

def CalcVWerr(arr1, arr2):

    perr = arr1.p - arr2.p;

    Rerr = np.matmul(arr2.R.T, arr1.R);

    werr = np.matmul(arr2.R, rot2omega(Rerr));

    #print("rot2omega",rot2omega(Rerr))

    #err = [perr, werr];

    err = [0]*6

    for k in range(3):

        err[k] = perr[k];

        err[k+3] = werr[k];

    return err

# In[11]:

def CalcJacobian(idx) :

    # Jacobian matrix of current configration in World frame

    jsize = len(idx);

    target = arr[idx[jsize-1]].p;   # absolute target position

    J = np.zeros((6,jsize));

    #JT = np.zeros((jsize,6));

    for n in range(jsize) : #n=1:jsize

        j = idx[n];

        #print("j ", j)

        #print("arr.R ", arr[j].R)

        #print("arr.a ", arr[j].a)

        a = np.matmul(arr[j].R, arr[j].a);  # joint axis vector in world frame

        #print("cross ", np.cross(a, target - arr[j].p) )

        #print("a ", a)

        cross = np.cross(a, target - arr[j].p);

        #print("len(cross) ", len(cross))

        for k in range(len(cross)):

            J[k, n] = cross[k];

            J[k+len(cross),n] = a[k];

    return J

# In[12]:

def InverseKinematics(to, Target):

    lam = 0.9;

    idx = FindRoute(to);

    #print("idx ", idx)

    ForwardKinematics(1);

    err = CalcVWerr(Target, arr[to]);

   # print("err ",err)

    for n in range(10):

        if np.linalg.norm(err) < 1e-6 : break;

        J  = CalcJacobian(idx);

        #print("J ", J )

        dq = lam * np.matmul(np.linalg.inv(J), err);

        print("dq ",dq)

        #print("idx ", idx)

        MoveJoints(idx, dq);

        ForwardKinematics(1);

        err = CalcVWerr(Target, arr[to]);

        #print("err ", err)

    err_norm = np.linalg.norm(err);

    return err_norm

# In[13]:

def MoveJoints(idx, dq):

    for n in range(len(idx)):

        j = idx[n];

        arr0 = arr[j].q

        arr[j].q = arr[j].q + dq[n];

        #print("arr1, arr0, dq ",arr[n].q , arr0 , dq[n])

    #print("arr_q ", arr[0].q, arr[1].q, arr[2].q, arr[3].q, arr[4].q, arr[5].q)

# In[14]:

def Draw_R(arr):

    fig, ax = plt.subplots(figsize=(6, 6), subplot_kw={'projection' : '3d'})

    r_leg = np.array([arr[1].p,arr[2].p,arr[3].p,arr[4].p,arr[5].p,arr[6].p,arr[7].p])

    l_leg = np.array([arr[1].p,arr[8].p,arr[9].p,arr[10].p,arr[11].p,arr[12].p,arr[13].p])

    ax.plot(r_leg[:,0],r_leg[:,1],r_leg[:,2],"o-", c='b')

    ax.plot(l_leg[:,0],l_leg[:,1],l_leg[:,2],"o-", c='b')

    ax.set_xticks(np.linspace(-1.0, 1.0, 5))

    ax.set_yticks(np.linspace(-1.0, 1.0, 5))

    ax.set_zticks(np.linspace(-1.0, 1.0, 5))

    #inline

    plt.show()

# In[15]:

def SetJointAngles(idx, q):

    for n in range(len(idx)):

        j = idx[n];

        arr[j].q = q[n];

    ForwardKinematics(1);

# In[16]:

def InverseKinematics_LM(to, Target):

# Levenberg-Marquardt, Chan-Lawrence, Sugihara's modification

    idx = FindRoute(to);

    wn_pos = 1/0.3;

    wn_ang = 1/(2*math.pi);

    We = np.diag([wn_pos, wn_pos, wn_pos, wn_ang, wn_ang, wn_ang]);#何やってる？

    Wn = np.eye(len(idx));#何やってるの？

    ForwardKinematics(1);

    err = CalcVWerr(Target, arr[to]);

    Ek = np.matmul(np.matmul(err, We),err); #Ek = err'*We*err;

    for n in range(10):

        J  = CalcJacobian(idx);

        #Jh = J'*We*J + Wn*(Ek + 0.002);  Hk + wn

        Jh = np.matmul(np.matmul(J.T,We),J) + Wn*(Ek+0.002)

        #gerr = J'*We*err;    gk

        gerr = np.matmul(np.matmul(J.T, We), err);

        #dq   = Jh \ gerr;    new

        dq = np.matmul(np.linalg.inv(Jh), gerr);

        MoveJoints(idx, dq);

        ForwardKinematics(1);

        err = CalcVWerr(Target, arr[to]);

        Ek2 = np.matmul(np.matmul(err, We),err);

        #print("Ek2 ",Ek2)

        #Ek2 = err'*We*err;

        if Ek2 < 1e-12:

          break;

        elif Ek2 < Ek:

          Ek = Ek2;

        else:

          MoveJoints(idx, -dq);#   revert

          ForwardKinematics(1);

        break; 

        #end

    #end

    #if nargout == 1 

    err_norm = np.linalg.norm(err);

    #end

    return err_norm

# In[17]:

def MakeRigidBody(j, wdh, mass):

    arr[j].m = mass;

    arr[j].c = [0, 0, 0];

    arr[j].I = np.array([[1/12*(wdh[1]*wdh[1] + wdh[2]*wdh[2]), 0.0, 0.0],

            [0.0, 1/12*(wdh[0]*wdh[0] + wdh[2]*wdh[2]),  0.0],

            [0.0, 0.0, 1/12*(wdh[0]*wdh[0] + wdh[1]*wdh[1])]]) * mass;

    arr[j].vertex = np.array([

        [0.0,      0.0,      0.0],

        [0.0,      wdh[1], 0.0],

        [wdh[0],   wdh[1],  0.0],

        [wdh[0],   0.0,     0.0],

        [0.0,      0.0,     wdh[2]],

        [0.0,      wdh[1],  wdh[2]],

        [wdh[0],   wdh[1],  wdh[2]],

        [wdh[0],   0.0,     wdh[2]]

        ])

    for n in range(3):

        for i in range(8):

            arr[1].vertex[i,n] = arr[1].vertex[i,n] - wdh[n]/2;

    arr[j].face = [

        [1, 2, 3, 4], [2, 6, 7, 3], [4, 3, 7, 8],

        [1, 5, 8, 4], [1, 2, 6, 5], [5, 6, 7, 8]

        ];

# In[18]:

def EulerDynamics(j):

    I = np.matmul(np.matmul(arr[j].R, arr[j].I), arr[j].R.T); #     慣性テンソル

    L = np.matmul(I, arr[j].w);   # 角運動量

    arr[j].dw  = -np.matmul(np.linalg.inv(I), np.cross(arr[j].w, L)); #    Eulerの方程式

    return L

# In[19]:

def hat(c):

    c_hat = np.array([[0, -c[2], c[1]], [c[2], 0., -c[0]], [-c[1], c[0], 0.]]);

    return c_hat

# In[20]:

def SE3exp(j, dt):

# see Murray, Li, Sastry p.42

    norm_w = np.linalg.norm(arr[j].w);

    if norm_w < sys.float_info.epsilon :

        p2 = arr[j].p + arr[j].vo * dt;

        R2 = arr[j].R;

    else :

        th = norm_w*dt;

        wn = arr[j].w/norm_w;  # normarized vector

        vo = arr[j].vo/norm_w;

        #print("th",th)

        #print("sin(th)",math.sin(th))

        w_wedge = np.array([[0., -wn[2], wn[1]],[wn[2], 0., -wn[0]],[-wn[1], wn[0], 0.]]);

        #print("wedge",w_wedge)

        drot = w_wedge * math.sin(th) + np.matmul(w_wedge,w_wedge) * (1.0-math.cos(th));

        rot  = np.eye(3) + drot;

        #print("test2 ", wn.reshape([3,1])@wn.reshape([1,3])) # 「＠」は縦ベクトル(3x1)x横ベクトル(1x3)=３ｘ３行列の計算．あと，reshapeも必要．転置では対応できない

        #p2 = np.matmul(rot, arr[j].p) - np.matmul(drot, np.cross(wn,bb vo)) + np.dot(wn, wn.T)* vo * th;

        p2 = np.matmul(rot, arr[j].p) - np.matmul(drot, np.matmul(w_wedge, vo)) + np.matmul(wn.reshape([3,1])@wn.reshape([1,3]), vo) * th;

        R2 = np.matmul(rot, arr[j].R);

    return p2, R2

# In[21]:

def SE3dynamics(j,f,tau):

    w_c = np.matmul(arr[j].R, arr[j].c) + arr[j].p;   # center of mass

    w_I = np.matmul(np.matmul(arr[j].R, arr[j].I),  arr[j].R.T);  # inertia in world frame

    c_hat = hat(w_c);

    Iww = w_I + arr[j].m * np.matmul(c_hat,  c_hat.T); 

    Ivv = arr[j].m * np.eye(3);

    Iwv = arr[j].m * c_hat;

    #AB()で使うために保存 20220527 そもそもこの関数をどこでコールするべきなのか？

    arr[j].Iww = Iww;

    arr[j].Ivv = Ivv;

    arr[j].Iwv = Iwv;

    P = arr[j].m * (arr[j].vo + np.cross(arr[j].w, w_c));     #  linear  momentum

    L = arr[j].m * np.cross(w_c, arr[j].vo) + np.matmul(Iww, arr[j].w); #  angular momentum

    #pp = [];

    pp = np.hstack([np.cross(arr[j].w, P),

        np.cross(arr[j].vo,P) + np.cross(arr[j].w, L)]);

    pp = pp - np.hstack([f, tau]);   # 外力，外モーメントを加算

    Ia = np.vstack([np.hstack([Ivv, Iwv.T]), np.hstack([Iwv, Iww])]);

    #print("Iwv ", Iwv)

    #print("Ia ", Ia)

    a0 = -np.matmul(np.linalg.inv(Ia),  pp);

    #print("a0 ", a0)

    for k in range(3):

        arr[j].dvo[k] = a0[k];

        arr[j].dw[k] = a0[k+3];

    return P, L

# In[22]:

def SetupRigidBody(j, wdh, com): #ほぼMakeRigidBodyと同じ．引数の違いは第３引数の質量と重心位置の違い

    #arr[j].m = mass;

    arr[j].c = com;

    arr[j].I = np.array([[1/12*(wdh[1]*wdh[1] + wdh[2]*wdh[2]), 0.0, 0.0],

            [0.0, 1/12*(wdh[0]*wdh[0] + wdh[2]*wdh[2]),  0.0],

            [0.0, 0.0, 1/12*(wdh[0]*wdh[0] + wdh[1]*wdh[1])]]) * arr[j].m;

    arr[j].vertex = np.array([

        [0.0,      0.0,      0.0],

        [0.0,      wdh[1], 0.0],

        [wdh[0],   wdh[1],  0.0],

        [wdh[0],   0.0,     0.0],

        [0.0,      0.0,     wdh[2]],

        [0.0,      wdh[1],  wdh[2]],

        [wdh[0],   wdh[1],  wdh[2]],

        [wdh[0],   0.0,     wdh[2]]

        ])

    for n in range(3):

        for i in range(8):

            arr[j].vertex[i,n] = arr[j].vertex[i,n] - wdh[n]/2 + com[n];

    arr[j].face = [

        [1, 2, 3, 4], [2, 6, 7, 3], [4, 3, 7, 8],

        [1, 5, 8, 4], [1, 2, 6, 5], [5, 6, 7, 8]

        ];

# In[23]:

# ２足歩行ロボットのモデルを動力学対応のため再定義

def SetupRobot(arr): # arr = [0]*14（１３リンク＋ゼロ番目の空のリンク）を引数にしないとだめ．

    ToDeg = 180/math.pi;   #r adianからdegreeに変換するときの係数

    ToRad = math.pi/180;   #d egreeからradianに変換するときの係数

    UX = [1.0,0.0,0.0];    # X軸の単位ベクトルで関節軸を設定するためのシンボル設定

    UY = [0.0,1.0,0.0];    # Y軸の単位ベクトルで関節軸を設定するためのシンボル設定

    UZ = [0.0,0.0,1.0];    # Z軸の単位ベクトルで関節軸を設定するためのシンボル設定

    R0=np.eye(3);          # 単位行列のシンボル設定

    p0= [0,0,0];           # ゼロ位置のシンボル設定

    #arr = [0]*14          # リンクの数（この例では14リンク）の配列を初期化

    # def __init__(self, name, sister, child, m, mother, b ,a ,q,R,p, dq, ddq, c, I, Ir, gr, u, dw):

    arr[0]=uLINK("empty",0,0,0,0,0,0,0,np.eye(3),np.zeros(3),0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3)) # 空のゼロリンクを設定（ワールド座標系）

    #arr[0].vertex=np.array([]);

    arr[1]=uLINK("BODY",0, 2, 0, 10, [0.0, 0.0, 0.7],UZ,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    # 右足のリンクの初期化：第3関節（股関節のピッチ）と第4関節（膝関節）のピッチ角を与えている）

    arr[2]=uLINK("RLEG_J0", 8, 3, 0, 5, [0.0,-0.1, 0.0],UZ,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[3]=uLINK("RLEG_J1", 0, 4, 0, 1, [0.0, 0.0, 0.0],UX,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[4]=uLINK("RLEG_J2", 0, 5, 0, 5, [0.0, 0.0, 0.0],UY,-90*ToRad,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[5]=uLINK("RLEG_J3", 0, 6, 0, 1, [0.0, 0.0,-0.3],UY,90*ToRad,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[6]=uLINK("RLEG_J4", 0, 7, 0, 6, [0.0, 0.0,-0.3],UY,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[7]=uLINK("RLEG_J5", 0, 0, 0, 2, [0.0, 0.0, 0.0],UX,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    # 左足のリンクの初期化：第10関節（股関節のピッチ）と第11関節（膝関節）のピッチ角を与えている）

    arr[8] =uLINK("LLEG_J0", 0,  9, 0, 5, [0.0, 0.1, 0.0],UZ,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[9] =uLINK("LLEG_J1", 0, 10, 0, 1, [0.0, 0.0, 0.0],UX,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[10]=uLINK("LLEG_J2", 0, 11, 0, 5, [0.0, 0.0, 0.0],UY,-90*ToRad,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[11]=uLINK("LLEG_J3", 0, 12, 0, 1, [0.0, 0.0,-0.3],UY,90*ToRad,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[12]=uLINK("LLEG_J4", 0, 13, 0, 6, [0.0, 0.0,-0.3],UY,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    arr[13]=uLINK("LLEG_J5", 0,  0, 0, 2, [0.0, 0.0, 0.0],UX,0,R0,p0,0,0,0,0,0,0,0,np.zeros(3),np.zeros(3),np.zeros(3),np.zeros(3))

    FindMother(1)

    #フィールドの追加

    # for n=1:length(uLINK)

    #     uLINK(n).dq     = 0;             関節速度   [rad/s]

    #     uLINK(n).ddq    = 0;             関節加速度 [rad/s^2]

    #     uLINK(n).c      = [0 0 0]';      重心位置   [m]

    #     uLINK(n).I      = zeros(3,3);    重心回りの慣性テンソル [kg.m^2]

    #     uLINK(n).Ir     = 0.0;           モータの電気子慣性モーメント [kg.m^2]

    #     uLINK(n).gr     = 0.0;           モータの減速比 [-]

    #     uLINK(n).u      = 0.0;            関節トルク [Nm]

    # end

    for n in range(len(arr)):

        arr[n].c = np.zeros(3);

        arr[n].I = np.zeros((3,3));

        arr[n].Ir = 0.0;

        arr[n].gr = 0.0;

    #  プログラムを見やすくするため，リンクnameと同名の変数にID番号をセット

    for n in range(len(arr)):

        name_set = arr[n].name + "=" + str(n);

        exec(name_set);

    # 　胴体，および足部を剛体でモデル化

    shape = [0.1, 0.3, 0.5];     # 奥行き，幅，高さ [m]

    com   = [0, 0, 0.3];        # 重心位置

    SetupRigidBody(BODY, shape,com);

    shape = [0.2, 0.1, 0.02];    # 奥行き，幅，高さ [m]

    com   = [0.05,  0., -0.04];  # 重心位置

    SetupRigidBody(RLEG_J5, shape,com);

    shape = [0.2, 0.1, 0.02];     # 奥行き，幅，高さ [m]

    com   = [0.05,  0., -0.04];   # 重心位置

    SetupRigidBody(LLEG_J5, shape,com);

    # 特異姿勢にならないように足を曲げておく．各足のピッチ軸3つの初期角度を修正

    arr[RLEG_J2].q = -10.0*ToRad;

    arr[RLEG_J3].q = 20.0*ToRad;

    arr[RLEG_J4].q = -10.0*ToRad;

    arr[LLEG_J2].q = -10.0*ToRad;

    arr[LLEG_J3].q = 20.0*ToRad;

    arr[LLEG_J4].q = -10.0*ToRad;

    arr[BODY].p = [0.0, 0.0, 0.65]; # ボディの初期の高さを修正

    arr[BODY].R = np.eye(3);

    ForwardKinematics(1);

# In[24]:

#　重心位置計算のための再帰計算部分

def calcMC(j):

    if j == 0:

        mc = 0;

    else:

        mc = arr[j].m * (arr[j].p + np.matmul(arr[j].R, arr[j].c));

        mc = mc + calcMC(arr[j].sister) + calcMC(arr[j].child);

    return mc

# In[25]:

# 重心位置計算の関数

def calcCoM(): 

    M  = TotalMass(1); # 再帰関数を使って総重量を計算

    MC = calcMC(1); # 再帰関数を使って重心位置計算

    com = MC / M;

    return com

# In[26]:

# 順運動学計算 2022/6/27 修正

def ForwardAllKinematics(j):

    if j == 0 : return;

    if j != 1 :

        mom = arr[j].mother;

        # position and orientation

        arr[j].p = np.matmul(arr[mom].R, arr[j].b) + arr[mom].p;

        arr[j].R = np.matmul(arr[mom].R, Rodrigues(arr[j].a, arr[j].q));

        # spatial velocity

        hw = np.matmul(arr[mom].R, arr[j].a);  # axis vector in world frame

        hv = np.cross(arr[j].p, hw);      # p_i x axis

        arr[j].hw = hw;                # store h1 and h2 for future use

        arr[j].hv = hv;

        arr[j].w  = arr[mom].w  + hw * arr[j].dq;

        arr[j].vo = arr[mom].vo + hv * arr[j].dq; 

        # spatial acceleration

        dhv = np.cross(arr[mom].w, hv) + np.cross(arr[mom].vo, hw);

        dhw = np.cross(arr[mom].w, hw);

        arr[j].cv = arr[j].dq * dhv;

        arr[j].cw = arr[j].dq * dhw;

        arr[j].dw  = arr[mom].dw  + dhw * arr[j].dq + hw * arr[j].ddq;

        arr[j].dvo = arr[mom].dvo + dhv * arr[j].dq + hv * arr[j].ddq;  

    ForwardAllKinematics(arr[j].sister);

    ForwardAllKinematics(arr[j].child);

# In[27]:

# 床からの外力計算 2022/1/18

def ExtForce(j):

    #w_c = uLINK(j).R * uLINK(j).c + uLINK(j).p;   # 重心位置

    #f = np.array([0, 0, -arr[j].m * G]);    # 重力

    f   = np.array([0., 0., 0.]);        # 床反力

    t   = np.array([0., 0., 0.]);        # モーメント

    Kf  = (1.0E+4);        # 床面の剛性[N/m]

    Df  = (1.0E+3);        # 床面の粘性[N/(m/s)]

    w_p = np.array([0., 0., 0.]);  

    for n in range(4): # 足裏の４点の位置をチェック0-3

    #if(isempty(arr[j].vertex) != 1) :

        w_p = np.matmul(arr[j].R, arr[j].vertex[n,:]); # ワールド座標へ回転

        pos = w_p + arr[j].p; #ワールド座標での位置

        if pos[2] < 0.0 :  # 支点は床面と接触している

            v = arr[j].vo + np.cross(arr[j].w, pos);  # ワールド座標の速度

            fc = np.array([-Df * v[0],  -Df * v[1], -Kf * pos[2] - Df * v[2]]);

            f = f + fc;

            t = t +  np.cross(pos, fc);

    f = f/4;

    t = t/4;

    return f, t

# In[28]:

#　逆運動学計算アルゴリズム　2022/6/28　修正

def InverseDynamics(j, Dtime):

    if j == 0 : 

        f=np.array([0,0,0]);

        t=np.array([0,0,0]);

        return f, t

    c = np.matmul(arr[j].R, arr[j].c) + arr[j].p;   # center of mass : wcと同じ ForwardDynamicsABM.cpp line 307

    # print("check inv ", np.matmul(arr[j].R, arr[j].R.T)) # check transform

    I = np.matmul(arr[j].R, np.matmul(arr[j].I, arr[j].R.T));  # inertia in world frame　： Iwと同じ ForwardDynamicsABM.cpp line 310

    c_hat = hat(c);

    # print("c_hat", c_hat)

    # print("c_hat.T", c_hat.T)

    I = I + arr[j].m * np.matmul(c_hat, c_hat.T);  # Iwwの計算になっている．ForwardDynamicsABM.cpp line 314

    arr[j].Iww = I; # Iwwの保存 2022/5/29

    arr[j].Iwv = arr[j].m * c_hat; # Iwvの保存　2022/5/29

    arr[j].Ivv = np.eye(3)*arr[j].m; # Ivvの保存　2022/5/29

    P = arr[j].m * (arr[j].vo + np.cross(arr[j].w,c));   # linear  momentum

    L = arr[j].m * np.cross(c, arr[j].vo) + np.matmul(I, arr[j].w); # angular momentum

    f0 = arr[j].m * (arr[j].dvo + np.cross(arr[j].dw,c))   + np.cross(arr[j].w, P);

    t0 = arr[j].m * np.cross(c, arr[j].dvo) + np.matmul(I, arr[j].dw) + np.cross(arr[j].vo, P) + np.cross(arr[j].w, L);

    # from child link

    [f1,t1] = InverseDynamics(arr[j].child, Dtime);

    f = f0 + f1;

    t = t0 + t1;

    # ここから外力項を追加　f -= link->F_ext(); 重心位置に加わる外力

    # 床と足裏の接触

    if j == 7 or j == 13 or j == 1: #右足先と左足先のリンク j==1を入れるとボディのvetexも床面との接触チェックをする

      [f3, t3]=ExtForce(j); #接触力の計算（ただのバネダンパ）

      f = f - f3; #符号に注意

      t = t - t3;

    if j != 1 : # BODY(ID=1)以外の計算

        arr[j].u = np.dot(arr[j].hv, f) + np.dot(arr[j].hw, t);  # joint driving force #ここは内積

    # return force to mother, with sisters

    [f2,t2] = InverseDynamics(arr[j].sister,Dtime);

    f = f + f2;

    t = t + t2;

    w_c = np.matmul(arr[j].R, arr[j].c) + arr[j].p;   # 重心位置

    fg = np.array([0,0,-G]) * arr[j].m; #20220628　重力項追加

    tg = np.cross(w_c, fg); #20220628　重力項追加

    f = f - fg;

    t = t - tg;

    # 20220529 ForwardDynamicsABM.cpp line 333-334 のように各リンクにバイアス項保存

    arr[j].pf = f;

    arr[j].ptau = t;

    return f,t

# In[29]:

# 逆運動学計算の関数：単位ベクトル法 2022/2/21

def InvDyn(j):

    arr[1].dvo = np.array([0, 0, 0]);

    arr[1].dw  = np.array([0, 0, 0]);

    if j >= 1 and j <= 3 :

        arr[1].dvo[j-1] = 1; # BODYの加速度を1に設定

    elif j >= 4 and j <= 6:

        arr[1].dw[j-4] = 1;  # BODYの角加速度を1に設定

    #arr[1].dvo = arr[1].dvo + np.array([0, 0, G]);  # ボディに加わる重力の効果を加える #これおかしいのだが、これがないと動かない。

    for n in range(1,len(arr)-1): # ボディ以下のリンクの計算

        if n == j-6:

            arr[n+1].ddq = 1.0;

        else:

            arr[n+1].ddq = 0.0;

    ForwardAllKinematics(1);

    [f,t] = InverseDynamics(1,Dtime);

    u = np.hstack([f,t]);

    for n in range(2,len(arr)):

        u=np.hstack([u,arr[n].u]);

    #u = [f,t,arr[1:].u];

    return u

# In[30]:

# ForwardDynamics

# 単位ベクトル法に基づく順動力学の計算

def ForwardDynamics(u_joint):

    nDoF = len(arr)-1+6-1; #18次元

    #print("nDoF ", nDoF);

    A = np.zeros((nDoF,nDoF));

    b = InvDyn(0);

    #print("b ", len(b))

    for n in range(1,nDoF):

        A[:,n-1] = InvDyn(n) - b;

    # モータ電機子慣性モーメントの効果

    #for n in range(7,nDoF):

    for n in range(6,nDoF-1):

        j = n-6+1+1;

        A[n,n] = A[n,n] + arr[j].Ir * arr[j].gr**2; # grはスカラ

    #print("len(A) ", len(A))

    # 加速度の計算

    #u  = [0, 0, 0, 0, 0, 0, u_joint(2:end)];

    #u = np.hstack([0, 0, 0, 0, 0, 0],u_joint[2:]);

    u = np.hstack([np.zeros(6),u_joint[1:]])

    #print("len(u) ", len(u))

    ddq = np.matmul(np.linalg.pinv(A), (-b + u));

    arr[1].dvo = ddq[0:3];

    #print("arr[1].dvo ",arr[1].dvo)

    arr[1].dw  = ddq[3:6];

    #print("arr[1].dw ",arr[1].dw)

    #print("len(arr) ", len(arr))

    for j in range(2,len(arr)):

        arr[j].ddq = ddq[j+6-2];

# In[31]:

# IntegrateEuler

def IntegrateEuler(j,Dtime):

    if j == 0 : return;

    if j == 1 :

        [arr[j].p, arr[j].R] = SE3exp(j, Dtime);

        #print("len(arr[j].vo ",  len(arr[j].vo))

        #print("len(arr[j].dvo ", len(arr[j].dvo))

        arr[j].vo = arr[j].vo + arr[j].dvo * Dtime;

        arr[j].w  = arr[j].w  + arr[j].dw * Dtime;

    else :

        arr[j].q  = arr[j].q  + arr[j].dq * Dtime;

        arr[j].dq = arr[j].dq + arr[j].ddq * Dtime;

    IntegrateEuler(arr[j].sister, Dtime);

    IntegrateEuler(arr[j].child, Dtime);

# In[32]:

# DrawAllJoints 全関節を線で描画する再帰関数．vertexがあるリンクはvertexを一筆書きで描画（適当．．．）

def DrawAllJoints(j):

    # radius    = 0.02;

    # len       = 0.06;

    # joint_col = 0;

    if j != 0 : 

        if hasattr(arr[j], 'vertex') : #vertexメンバ変数を持っているかのチェック

            #print("joint ",j);

            vert = np.matmul(arr[j].R, arr[j].vertex.T);

            for k in range(3) :# 1:3

                vert[k,:] = vert[k,:] + arr[j].p[k];#    adding x,y,z to all vertex

            ax.plot(vert[0,:],vert[1,:],vert[2,:],"o-", c='b')

        i = arr[j].mother;

        if i != 0:

            ax.plot([arr[i].p[0],arr[j].p[0]],[arr[i].p[1],arr[j].p[1]],[arr[i].p[2],arr[j].p[2]],"o-", c='b')

        DrawAllJoints(arr[j].child);

        DrawAllJoints(arr[j].sister);

# In[34]:

#  プログラムを見やすくするため，リンクnameと同名の変数にID番号をセット

for n in range(len(arr)):

    name_set = arr[n].name + "=" + str(n);

    print(name_set)

    exec(name_set);

# In[35]:

# biped robot simulation

#  単位ベクトル法による動力学シミュレーション

#  床面との接触反力は足の長方形板の頂点のめり込みを簡易的に計算しているだけ．

#　剛体の接触力計算については．．．いい方法があれば教えて下さい．

G = 9.8;  # 重力加速度

arr = [0]*14;

SetupRobot(arr);# ロボットの初期設定

# 胴体の空間速度・加速度の初期化

BODY=1;

arr[BODY].vo = np.array([0., 0., 0.]);

arr[BODY].w  = np.array([0., 0., 0.]);

arr[BODY].dvo = np.array([0., 0., 0.]);

arr[BODY].dw  = np.array([0., 0., 0.]);

# 関節トルク

u_joint = np.zeros(len(arr)-1); # 全関節トルクを0に設定

#u_joint[RLEG_J2] = -10.0;   # 右股関節ピッチ軸にトルクを与える　[Nm]

# シミュレーション設定

Dtime = 0.001; # 1msでシミュレーションしないと簡易な接触力計算では発散してしまう．ルンゲクッタにすればましになるか？

EndTime = 0.7; # 0.7秒だけのトルクゼロの崩れるシミュレーション．

time = EndTime//Dtime+1; # 刻み数：最後の１は時刻ゼロも含めるため

com_m = np.zeros((int(time),3)); # 重心位置の初期化 プロットに使う？？

for k in range(int(time)):

    ForwardDynamics(u_joint); # 順運動学計算

    IntegrateEuler(1,Dtime); # 順運動学で求めたトルクから全関節のパラメータ更新

    com = calcCoM;  # 重心位置計算

    #com_m[k,:] = com;

    if k%10==0:

        fig, ax = plt.subplots(figsize=(6, 6), subplot_kw={'projection' : '3d'}) #ここで図を用意

        print(k)

        DrawAllJoints(1); # ロボットの簡易描画

        ax.set_xticks(np.linspace(-1.0, 1.0, 5)) # X軸の表示範囲

        ax.set_yticks(np.linspace(-1.0, 1.0, 5)) # Y軸の表示範囲

        ax.set_zticks(np.linspace(-0.5, 1.5, 5)) # Z軸の表示範囲

        plt.show();

# In[36]:

def clamp(n, smallest, largest):

    return max(smallest, min(n, largest))

# In[37]:

# 機構慣性の計算 2022/5/15

def AB():

    #const int n = subBody->numLinks();

    nDoF = len(arr)-1; #13次元

    #for(int i = n-1; i >= 0; --i){  for k in range(int(time)):

    for i in range(nDoF, 0, -1):

        #auto link = subBody->link(i); //n番目のリンクを初期化

        #link->pf()   -= link->f_ext(); // 外力を先に足しておく：もとはゼロのはず

        #link->ptau() -= link->tau_ext(); // 外力を先に足しておく

        # pf, ptauはバイアス項らしい？

        # compute articulated inertia (Eq.(6.48) of Kajita's textbook)

        #for(DyLink* child = link->child(); child; child = child->sibling()){ //子供と兄弟リンクを2回ループするだけ 

        for k in range(2): # //0: 子供, 1:兄弟リンクを2回ループするだけ #ここのループが怪しいか？ choreonoidだとsisterがゼロになるまでループしているっぽい。

            if k == 0:

                childsister = arr[i].child;

            elif k == 1:

                chidlsister = arr[i].sister;

            #if(child->isFreeJoint()){

            if childsister == 0:

                continue;

            elif hasattr(arr[childsister], 'isFixedJoint') and arr[childsister].isFixedJoint == 1: #isFixedJointメンバ変数を持っているかのチェック

                #if arr[childsister].isFixedJoint == 1:

                arr[i].Ivv += arr[childsister].Ivv;

                arr[i].Iwv += arr[childsister].Iwv;

                arr[i].Iww += arr[childsister].Iww;

            else:

                #### ここから6.48式の更新らしいが．．． ###

                #const Vector3 hhv_dd = child->hhv() / child->dd();

                #arr[i].hhv_dd = np.array([0, 0, 0]);#この初期化必要か？いらないならコメントアウト．

                hhv_dd = arr[childsister].hhv / arr[childsister].dd;#hhv/dd ただし，dd=s^T*I^A*s, hhv=I^A*s は 3x1の縦ベクトル

                hhw_dd = arr[childsister].hhw / arr[childsister].dd;#hhw/dd

                arr[i].Ivv += arr[childsister].Ivv - arr[childsister].hhv @ hhv_dd.T;#hhv*hhv_dd.tranで縦の3x1ベクトルと横の1x3ベクトルで３ｘ３の行列を生成．演算子は「@」

                arr[i].Iwv += arr[childsister].Iwv - arr[childsister].hhw @ hhv_dd.T;

                arr[i].Iww += arr[childsister].Iww - arr[childsister].hhw @ hhw_dd.T;

                #### ここまで6.48式の更新 ###

            # 式6.44の右辺第2項のバイアス項の計算？　P=I^s・\itaを計算しかしてない？運動量Pがpfで角運動量Lがptauのはず．

            arr[i].pf   += np.matmul(arr[childsister].Ivv, arr[childsister].cv) + np.matmul(arr[childsister].Iwv.T, arr[childsister].cw) + arr[childsister].pf;

            arr[i].ptau += np.matmul(arr[childsister].Iwv, arr[childsister].cv) + np.matmul(arr[childsister].Iww, arr[childsister].cw) + arr[childsister].ptau;

            if hasattr(arr[childsister], 'isFixedJoint') and arr[childsister].isFixedJoint == 1: #isFixedJointメンバ変数を持っているかのチェック

                uu_dd = arr[childsister].uu / arr[childsister].dd; #uu/dd

                arr[i].pf   += uu_dd * arr[childsister].hhv; #固定されているから理想の力がそのまま加わるということかな？

                arr[i].ptau += uu_dd * arr[childsister].hhw;

        #if(i > 0){

        if(i>1):

            #if(!link->isFixedJoint()){ #固定関節ではないとき

            if arr[i].isFixedJoint != 1: #固定関節ではないとき

                #// hh = Ia * s  6.48式の第3項で使うためなのか？

                #I^A・sの更新：hhv(I^A・sの上)とhhw(I^A・sの下)とddを更新

                arr[i].hhv = np.matmul(arr[i].Ivv, arr[i].hv) + np.matmul(arr[i].Iwv.T, arr[i].hw); # sv -> hv, sw -> hw

                arr[i].hhw = np.matmul(arr[i].Iwv, arr[i].hv) + np.matmul(arr[i].Iww, arr[i].hw);

                #// dd = Ia * s * s^T この式がわからんs^T * Ia * sならわかるが．6.48式の第3項の時には使うが？上のddはs^T*I^A*sと思っていたけど．．．

                arr[i].dd = arr[i].hv.dot(arr[i].hhv) + arr[i].hw.dot(arr[i].hhw);# + arr[i].Jm2; # ddとはなんだ？ Jm2とは？->/// Equivalent rotor inertia: n^2*Jm [kg.m^2] Link.hより nはギア比か？Jm2はスカラのはず sv -> hw, sw -> hw

                #print("arr[",i,"].dd",arr[i].dd) #ddはスカラになるはず。

                #print("Ivv ",arr[i].Ivv)

                #print("hhv ",arr[i].hhv)

                #print("Iwv ",arr[i].Iwv)

                #print("Iwv.T ",arr[i].Iwv.T)

                #print("Iww ",arr[i].Iww)

                #print("hhw ",arr[i].hhw)

                #// uu = u - hh^T*c + s^T*pp このuuってなんだ？この式もわからん．hh^T*cは6.49の第2項を展開したものでS^T*I^A*\dot{\ita}

                # clampは第2引数下限，第2引数上限を与えて，超えていたら上下限の値を返す関数．

                arr[i].uu = clamp(arr[i].u, arr[i].u_lower, arr[i].u_upper) - np.dot(arr[i].hhv, arr[i].cv) + np.dot(arr[i].hhw, arr[i].cw) + np.dot(arr[i].hv, arr[i].pf) + np.dot(arr[i].hw, arr[i].ptau); #ここはバイアス項：式6.49のb_2だが，式6.46のを見たほうが良い．

                    ## uuは式6.49の分子か？uのupperの意味？？ddで割れば式6.49の関節の角加速度が求まる！

    #return u

# In[38]:

# ForwardDynamics_ab

# Articurated Body Dynamicsを使った順動力学の計算

def ForwardDynamics_ab(joint):

    ForwardAllKinematics(1);

    [f,t] = InverseDynamics(1,Dtime);

    AB();

    # AB_phaze3

    # 加速度の計算

    #auto root = subBody->rootLink();

    #if(!root->isFreeJoint()){

    if arr[BODY].isFixedJoint == 1 : #固定関節

        arr[BODY].dvo =  [0., 0., 0.]; #root->dvo().setZero();

        arr[BODY].dw = [0., 0., 0.]; #root->dw().setZero();

    else:

        # - | Ivv  trans(Iwv) | * | dvo | = | pf   |

        #   | Iwv     Iww     |   | dw  |   | ptau |

        #Eigen::Matrix<double, 6, 6> M;

        #M << root->Ivv(), root->Iwv().transpose(),

        #    root->Iwv(), root->Iww();

        #Eigen::Matrix<double, 6, 1> f;

        #f << root->pf(),

        #    root->ptau();

        #f *= -1.0;

        #Eigen::Matrix<double, 6, 1> a(M.colPivHouseholderQr().solve(f));

        #root->dvo() = a.head<3>();

        #root->dw() = a.tail<3>();

        SE3dynamics(BODY,arr[BODY].pf, arr[BODY].ptau); #pfとptauの入力はどうする？

        #arr[BODY].dvo =  arr[BODY].dvo + np.array([0., 0., G]); #梶田さんのはこれでうまくいっているが、これやると吹っ飛ぶ。

    #const int n = subBody->numLinks();

    n = len(arr); # n = 13 

    for i in range(2,n): # (int i=1; i < n; ++i){ #2 から 13まで

        # auto link = subBody->link(i);

        # auto parent = link->parent();

        #if(link->isFixedJoint()){

        if arr[i].isFixedJoint == 1 : 

            arr[i].ddq = 0.0;

            arr[i].dvo = arr[arr[i].mother].dvo;

            arr[i].dw  = arr[arr[i].mother].dw; 

        else :

            #link->ddq() = (link->uu() - (link->hhv().dot(parent->dvo()) + link->hhw().dot(parent->dw()))) / link->dd();

            #link->dvo().noalias() = parent->dvo() + link->cv() + link->sv() * link->ddq();

            #link->dw().noalias()  = parent->dw()  + link->cw() + link->sw() * link->ddq();

            #print("arr[",i,"].dd=", arr[i].dd)

            arr[i].ddq = (arr[i].uu - np.dot(arr[i].hhv, arr[arr[i].mother].dvo) + np.dot(arr[i].hhw, arr[arr[i].mother].dw)) / arr[i].dd;

            arr[i].dvo = arr[arr[i].mother].dvo + arr[i].cv + arr[i].hv * arr[i].ddq; # sv -> hv 

            arr[i].dw  = arr[arr[i].mother].dw  + arr[i].cw + arr[i].hw * arr[i].ddq; # sw -> hw

# In[39]:

# biped robot simulation

#  Articulated Body Dynamicsを使ったシミュレーション

G = 9.8;  # 重力加速度

arr = [0]*14;

SetupRobot(arr);# ロボットの初期設定

# 胴体の空間速度・加速度の初期化

BODY=1;

arr[BODY].vo = np.array([0., 0., 0.]);

arr[BODY].w  = np.array([0., 0., 0.]);

arr[BODY].dvo = np.array([0., 0., 0.]);

arr[BODY].dw  = np.array([0., 0., 0.]);

# 関節トルク

#u_joint = np.zeros(len(arr)-1); # 全関節トルクを0に設定

#u_joint[RLEG_J2] = -10.0;   # 右股関節ピッチ軸にトルクを与える　[Nm]

# シミュレーション設定

Dtime = 0.001; # 1msでシミュレーションしないと簡易な接触力計算では発散してしまう．ルンゲクッタにすればましになるか？

EndTime = 0.7; # 0.7秒だけのトルクゼロの崩れるシミュレーション．

time = EndTime//Dtime+1; # 刻み数：最後の１は時刻ゼロも含めるため

com_m = np.zeros((int(time),3)); # 重心位置の初期化 プロットに使う？？

# 初期化

number = len(arr);

for i in range(0,number):

    arr[i].isFixedJoint = 0;

    arr[i].u_lower = -100;

    arr[i].u_upper = 100;

for k in range(int(time)):

    ForwardDynamics_ab(0); # 順運動学計算

    IntegrateEuler(1,Dtime); # 順運動学で求めたトルクから全関節のパラメータ更新

    com = calcCoM;  # 重心位置計算

    #com_m[k,:] = com;

    if k%10==0:

        fig, ax = plt.subplots(figsize=(6, 6), subplot_kw={'projection' : '3d'}) #ここで図を用意

        print(k)

        DrawAllJoints(1); # ロボットの簡易描画

        ax.set_xticks(np.linspace(-1.0, 1.0, 5)) # X軸の表示範囲

        ax.set_yticks(np.linspace(-1.0, 1.0, 5)) # Y軸の表示範囲

        ax.set_zticks(np.linspace(-0.5, 1.5, 5)) # Z軸の表示範囲

        plt.show();