(self, step_input)
return step_output
Updates the state machine and toe movements per state.
Updates the state machine and toe movements per state.
Updates the state machine and toe movements per state .
def update(self, step_input):
    """Updates the state machine and toe movements per state."""
    base_com_pos = step_input.base_com_pos
    base_com_orn = step_input.base_com_orn
    base_com_pos_inv, base_com_orn_inv = self.bullet_client.invertTransform(
        base_com_pos, base_com_orn)

    dt = step_input.dt
    self.bullet_client.resetBasePositionAndOrientation(self.sphere_uid,
                                                       step_input.new_pos_world,
                                                       [0, 0, 0, 1])
    self.bullet_client.changeVisualShape(
        self.sphere_uid, -1, rgbaColor=self.colors[self.swing_foot_index])

    all_toes_pos_locals = []
    for toe_pos_world in step_input.toe_pos_world:
      toe_pos_local, _ = self.bullet_client.multiplyTransforms(
          base_com_pos_inv, base_com_orn_inv, toe_pos_world, [0, 0, 0, 1])
      all_toes_pos_locals.append(toe_pos_local)
    all_toes_pos_locals = np.array(all_toes_pos_locals)
    centroid_world = np.zeros(3)
    for v in self.support_vertices[self.swing_foot_index]:
      vtx_pos_world = step_input.toe_pos_world[v]
      centroid_world += vtx_pos_world
    centroid_world /= 3.

    sphere_z_offset = 0.05
    self.diff_world = base_com_pos - centroid_world
    self.diff_world[2] = 0.
    self.bullet_client.resetBasePositionAndOrientation(self.sphere_uid_centroid,
                                                       centroid_world,
                                                       [0, 0, 0, 1])
    self.bullet_client.resetBasePositionAndOrientation(
        self.sphere_uid_com,
        [base_com_pos[0], base_com_pos[1], sphere_z_offset], [0, 0, 0, 1])

    l = np.linalg.norm(self.diff_world)
    if self.is_far:
      bound = self.far_bound
    else:
      bound = self.close_bound

    if l > bound:
      self.diff_world *= self.max_shift * 0.5 / l
      if not self.is_far:
        self.is_far = True
    else:
      if self.is_far:
        self.is_far = False

    if not self.is_far:
      self.diff_world = np.zeros(3)
    for i in range(len(self.toe_pos_local_ref)):
      toe = self.toe_pos_local_ref[i]
      toe = [
          toe[0] + self.diff_world[0], toe[1] + self.diff_world[1],
          toe[2] + self.diff_world[2]
      ]
      self.toe_pos_local_ref[i] = toe

    self.local_diff_y = self.toe_pos_local_ref[0][
        1] + self.toe_pos_local_ref[1][1] - self.toe_pos_local_ref[
            2][1] - self.toe_pos_local_ref[3][1]

    self.yaw = 0
    if self.local_diff_y < -self.local_diff_y_threshold:
      self.yaw = 0.001
    if self.local_diff_y > self.local_diff_y_threshold:
      self.yaw = -0.001

    yaw_trans = self.bullet_client.getQuaternionFromEuler([0, 0, self.yaw])

    if not self.is_far:
      for i in range(len(self.toe_pos_local_ref)):
        toe = self.toe_pos_local_ref[i]
        toe, _ = self.bullet_client.multiplyTransforms([0, 0, 0], yaw_trans,
                                                       toe, [0, 0, 0, 1])
        self.toe_pos_local_ref[i] = toe

    new_toe_pos_world = []

    # Moves the swing foot to the target location.
    if self.move_swing_foot:
      if self.state_time <= 1:
        self.state_time += 4 * dt
    if self.state_time >= 1:
      self.move_swing_foot = False
      self.state_time = 0
      self.toe_pos_local_ref[self.swing_foot_index] = self.new_pos_local
    toe_pos_local_ref_copy = copy.deepcopy(self.toe_pos_local_ref)
    old_pos = self.toe_pos_local_ref[self.swing_foot_index]
    new_pos = [
        old_pos[0] * (1 - self.state_time) + self.new_pos_local[0] *
        (self.state_time), old_pos[1] * (1 - self.state_time) +
        self.new_pos_local[1] * (self.state_time),
        old_pos[2] * (1 - self.state_time) + self.new_pos_local[2] *
        (self.state_time) + self.amp * math.sin(self.state_time * math.pi)
    ]
    toe_pos_local_ref_copy[self.swing_foot_index] = new_pos
    for toe_pos_local in toe_pos_local_ref_copy:
      new_toe_pos_world.append(self.bullet_client.multiplyTransforms(
          base_com_pos, base_com_orn, toe_pos_local, [0, 0, 0, 1])[0])

    step_output = StepOutput(new_toe_pos_world)
    return step_output
def update ( self , step_input ) : base_com_pos = step_input . base_com_pos base_com_orn = step_input . base_com_orn base_com_pos_inv , base_com_orn_inv = self . bullet_client . invertTransform ( base_com_pos , base_com_orn ) dt = step_input . dt self . bullet_client . resetBasePositionAndOrientation ( self . sphere_uid , step_input . new_pos_world , [ 0 , 0 , 0 , 1 ] ) self . bullet_client . changeVisualShape ( self . sphere_uid , - 1 , rgbaColor = self . colors [ self . swing_foot_index ] ) all_toes_pos_locals = [ ] for toe_pos_world in step_input . toe_pos_world : toe_pos_local , _ = self . bullet_client . multiplyTransforms ( base_com_pos_inv , base_com_orn_inv , toe_pos_world , [ 0 , 0 , 0 , 1 ] ) all_toes_pos_locals . append ( toe_pos_local ) all_toes_pos_locals = np . array ( all_toes_pos_locals ) centroid_world = np . zeros ( 3 ) for v in self . support_vertices [ self . swing_foot_index ] : vtx_pos_world = step_input . toe_pos_world [ v ] centroid_world += vtx_pos_world centroid_world /= 3. sphere_z_offset = 0.05 self . diff_world = base_com_pos - centroid_world self . diff_world [ 2 ] = 0. self . bullet_client . resetBasePositionAndOrientation ( self . sphere_uid_centroid , centroid_world , [ 0 , 0 , 0 , 1 ] ) self . bullet_client . resetBasePositionAndOrientation ( self . sphere_uid_com , [ base_com_pos [ 0 ] , base_com_pos [ 1 ] , sphere_z_offset ] , [ 0 , 0 , 0 , 1 ] ) l = np . linalg . norm ( self . diff_world ) if self . is_far : bound = self . far_bound else : bound = self . close_bound if l > bound : self . diff_world *= self . max_shift * 0.5 / l if not self . is_far : self . is_far = True else : if self . is_far : self . is_far = False if not self . is_far : self . diff_world = np . zeros ( 3 ) for i in range ( len ( self . toe_pos_local_ref ) ) : toe = self . toe_pos_local_ref [ i ] toe = [ toe [ 0 ] + self . diff_world [ 0 ] , toe [ 1 ] + self . diff_world [ 1 ] , toe [ 2 ] + self . diff_world [ 2 ] ] self . toe_pos_local_ref [ i ] = toe self . local_diff_y = self . toe_pos_local_ref [ 0 ] [ 1 ] + self . toe_pos_local_ref [ 1 ] [ 1 ] - self . toe_pos_local_ref [ 2 ] [ 1 ] - self . toe_pos_local_ref [ 3 ] [ 1 ] self . yaw = 0 if self . local_diff_y < - self . local_diff_y_threshold : self . yaw = 0.001 if self . local_diff_y > self . local_diff_y_threshold : self . yaw = - 0.001 yaw_trans = self . bullet_client . getQuaternionFromEuler ( [ 0 , 0 , self . yaw ] ) if not self . is_far : for i in range ( len ( self . toe_pos_local_ref ) ) : toe = self . toe_pos_local_ref [ i ] toe , _ = self . bullet_client . multiplyTransforms ( [ 0 , 0 , 0 ] , yaw_trans , toe , [ 0 , 0 , 0 , 1 ] ) self . toe_pos_local_ref [ i ] = toe new_toe_pos_world = [ ] # Moves the swing foot to the target location. if self . move_swing_foot : if self . state_time <= 1 : self . state_time += 4 * dt if self . state_time >= 1 : self . move_swing_foot = False self . state_time = 0 self . toe_pos_local_ref [ self . swing_foot_index ] = self . new_pos_local toe_pos_local_ref_copy = copy . deepcopy ( self . toe_pos_local_ref ) old_pos = self . toe_pos_local_ref [ self . swing_foot_index ] new_pos = [ old_pos [ 0 ] * ( 1 - self . state_time ) + self . new_pos_local [ 0 ] * ( self . state_time ) , old_pos [ 1 ] * ( 1 - self . state_time ) + self . new_pos_local [ 1 ] * ( self . state_time ) , old_pos [ 2 ] * ( 1 - self . state_time ) + self . new_pos_local [ 2 ] * ( self . state_time ) + self . amp * math . sin ( self . state_time * math . pi ) ] toe_pos_local_ref_copy [ self . swing_foot_index ] = new_pos for toe_pos_local in toe_pos_local_ref_copy : new_toe_pos_world . append ( self . bullet_client . multiplyTransforms ( base_com_pos , base_com_orn , toe_pos_local , [ 0 , 0 , 0 , 1 ] ) [ 0 ] ) step_output = StepOutput ( new_toe_pos_world ) return step_output