계속해서 kinematicMotion.py파일을 살펴봅시다!

def positions(self,t,kb_offset={}):

				...

        Tt=(self.t0+self.t1+self.t2+self.t3)
        Tt2=Tt/2

        td=(t*1000)%Tt
        t2=(t*1000-Tt2)%Tt
        rtd=(t*1000)%Tt # rear time delta
        rt2=(t*1000-Tt2)%Tt
        
        Fx=p.readUserDebugParameter(self.IDfrontOffset)
        Rx=-p.readUserDebugParameter(self.IDrearOffset)

        print(td, t2, rt2, rtd)
        Fy=-100
        Ry=-100
        r=np.array([self.calcLeg(td,Fx,Fy,spf),self.calcLeg(t2,Fx,Fy,-spf),self.calcLeg(rt2,Rx,Ry,spr),self.calcLeg(rtd,Rx,Ry,-spr)])
        return r

• 여기까지가 보행에 필요한 코드 리뷰였습니다.
• 그럼, 이전 pybullet 강의를 상기하면서 다음 부분을 살펴보겠습니다.

pybullet_automatic_gait.py

if result_dict['StartStepping']:
    robot.feetPosition(trotting.positions(d-3, result_dict))
else:
    robot.feetPosition(Lp)

spotmicroai.py

def feetPosition(self,Lp):
        self.Lp=Lp

...

def step(self):
...
	# Calculate Angles with the input of FeetPos,BodyRotation and BodyPosition
  self.angles = self.kin.calcIK(self.Lp, self.rot, self.pos)

  # list comprehension usage
  # [word for sentence in text for word in sentence]
  # print([ (angle * 180 / 3.1415) for singleFootAngle in angles for angle in singleFootAngle ])

  for lx, leg in enumerate(['front_left', 'front_right', 'rear_left', 'rear_right']):
      for px, part in enumerate(['shoulder', 'leg', 'foot']):
          j = self.jointNameToId[leg+"_"+part]
          p.setJointMotorControl2(bodyIndex=quadruped,
                                  jointIndex=j,
                                  controlMode=p.POSITION_CONTROL,
                                  targetPosition=self.angles[lx][px]*self.dirs[lx][px],
                                  positionGain=kp,
                                  velocityGain=kd,
                                  force=maxForce)

• 이전 강의에서 살펴보았던 setJointMotorControl2 함수의 input으로 self.angles 가 들어가고, 이 self.angles 는 Inverse Kinematic를 통해 얻어진 값임을 알 수 있습니다.
• 최종적으로 setJointMotorControl2 를 통해 pybullet상의 URDF 포맷으로 구성된 로봇이 보행을 시작할 것입니다.

실습을 통해 Trotting 코드를 이해해보자

https://www.youtube.com/watch?v=wQsmsr0oR6c

• kinematicMotion.py

퀴즈!! 고양이처럼 보행하도록!!

# front left / front right / rear left / rear right
r=np.array([self.calcLeg(td,Fx,Fy,spf),self.calcLeg(rtd,Fx,Fy,-spf),self.calcLeg(rtd,Rx,Ry,spr),self.calcLeg(td,Rx,Ry,-spr)])

# Cat Trotting Example
Tt4=Tt/4

"""
How can implement cat Trotting??
"""

return r