Biped Jump: Using Jacobian

This example demonstrates a jumping controller using Jacobian transpose.

Screenshot

../_images/biped_jump.png

Code

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
 import pydart2 as pydart
 import numpy as np


 class JTController:
     """
     # Usage
     self.jt = JTController(self.skel)
     tau += self.jt.apply( ["l_hand", "r_hand"], f )
     """
     def __init__(self, _skel):
         self.skel = _skel

     def apply(self, bodynames, f):
         if not isinstance(bodynames, list):
             bodynames = [bodynames]
         f = np.array(f)

         tau = np.zeros(self.skel.ndofs)
         for bodyname in bodynames:
             J = self.skel.body(bodyname).linear_jacobian()
             JT = np.transpose(J)
             tau += JT.dot(f)
         return tau


 class Controller:
     def __init__(self, skel, h):
         self.h = h
         self.skel = skel
         ndofs = self.skel.ndofs
         self.qhat = self.skel.q
         self.Kp = np.diagflat([0.0] * 6 + [600.0] * (ndofs - 6))
         self.Kd = np.diagflat([0.0] * 6 + [40.0] * (ndofs - 6))

         # Init target poses
         self.init_target_poses()
         # Jacobian transpose
         self.jt = JTController(self.skel)

     def init_target_poses(self):
         skel = self.skel
         I_thigh = skel.dof_indices(["j_thigh_left_z", "j_thigh_right_z"])
         I_shin = skel.dof_indices(["j_shin_left", "j_shin_right"])
         I_heel = skel.dof_indices(["j_heel_left_1", "j_heel_right_1"])

         pose0 = self.skel.q
         pose0[I_thigh] = 1.2
         pose0[I_shin] = -2.0
         pose0[I_heel] = 0.8
         pose0[('j_bicep_left_y', 'j_bicep_right_y')] = 0.5, -0.5

         pose1 = self.skel.q
         pose1[('j_bicep_left_y', 'j_bicep_right_y')] = -2.0, 2.0
         pose1[('j_bicep_left_x', 'j_bicep_right_x')] = 0.5, -0.5

         pose2 = self.skel.q
         pose2[I_thigh] = 0.3  # Thighs

         self.target_poses = [pose0, pose1, pose2]
         self.target_times = [0.0, 0.4, 0.8]

     def update_target_pose(self):
         if len(self.target_times) == 0:
             return
         t = self.skel.world.t
         if self.target_times[0] <= t:
             self.qhat = self.target_poses[0]
             print('update pose! at %.4lf' % t)
             self.target_poses.pop(0)
             self.target_times.pop(0)

     def compute(self):
         self.update_target_pose()
         skel = self.skel

         invM = np.linalg.inv(skel.M + self.Kd * self.h)
         p = -self.Kp.dot(skel.q + skel.dq * self.h - self.qhat)
         d = -self.Kd.dot(skel.dq)
         qddot = invM.dot(-skel.c + p + d + skel.constraint_forces())
         tau = p + d - self.Kd.dot(qddot) * self.h

         t = self.skel.world.t
         if 0.3 < t and t < 0.5:  # Jump!
             heels = ['h_heel_left', 'h_heel_right']
             vf = self.jt.apply(heels, [0, -700, 0])
             tau += vf

         # Make sure the first six are zero
         tau[:6] = 0
         return tau


 if __name__ == '__main__':
     print('Example: bipedJump')

     pydart.init()
     print('pydart initialization OK')

     world = pydart.World(1.0 / 2000.0, './data/skel/fullbody1.skel')
     print('pydart create_world OK')

     # Initialize the pose
     skel = world.skeletons[1]
     q = skel.q
     q[(2, 4, 5)] = [0.02 * np.pi, -0.02, 0]
     skel.set_positions(q)
     print('skeleton position OK')

     # Initialize the controller
     skel.set_controller(Controller(skel, world.dt))
     print('create controller OK')

     pydart.gui.viewer.launch(world)