def clipFootStepWithEllipse(footMove):
    ''' Clip the foot move inside an ellipse defined by the foot's dimansions.
      footMove is an almath.Pose2D (x, y, theta position).
  '''

    # Apply an offset to have Y component of foot move centered on 0.
    if (footMove.y < -minFootSeparation):
        footMove.y = footMove.y + minFootSeparation
    elif (footMove.y > minFootSeparation):
        footMove.y = footMove.y - minFootSeparation
    else:
        return

    # Clip the foot move to an ellipse using ALMath method.
    if footMove.x >= 0:
        almath.clipFootWithEllipse(maxStepX, maxStepY - minFootSeparation,
                                   footMove)
    else:
        almath.clipFootWithEllipse(minStepX, maxStepY - minFootSeparation,
                                   footMove)

    # Correct the previous offset on Y component.
    if footMove.y >= 0:
        footMove.y = footMove.y + minFootSeparation
    else:
        footMove.y = footMove.y - minFootSeparation
예제 #2
0
def clip_footstep_with_ellipse(foot):
    ''' Clip the foot move inside an ellipse defined by the foot's dimansions.
        foot is an almath.Pose2D (x, y, theta position).
    '''

    # Apply an offset to have Y component of foot move centered on 0.
    if (foot.y < -Params.min_step_y):
        foot.y = foot.y + Params.min_step_y
    elif (foot.y > Params.min_step_y):
        foot.y = foot.y - Params.min_step_y
    else:
        return

    # Clip the foot move to an ellipse using ALMath method.
    if foot.x >= 0:
        almath.clipFootWithEllipse(Params.max_step_x,
                                   Params.max_step_y - Params.min_step_y, foot)
    else:
        almath.clipFootWithEllipse(Params.min_step_x,
                                   Params.max_step_y - Params.min_step_y, foot)

    # Correct the previous offset on Y component.
    if foot.y >= 0:
        foot.y = foot.y + Params.min_step_y
    else:
        foot.y = foot.y - Params.min_step_y
예제 #3
0
def clipFootStepWithEllipse(footMove):
  ''' Clip the foot move inside an ellipse defined by the foot's dimansions.
      footMove is an almath.Pose2D (x, y, theta position).
  '''

  # Apply an offset to have Y component of foot move centered on 0.
  if (footMove.y < -minFootSeparation):
    footMove.y = footMove.y + minFootSeparation
  elif (footMove.y > minFootSeparation):
    footMove.y = footMove.y - minFootSeparation
  else:
    return

  # Clip the foot move to an ellipse using ALMath method.
  if footMove.x >= 0:
    almath.clipFootWithEllipse(maxStepX, maxStepY - minFootSeparation, footMove)
  else:
    almath.clipFootWithEllipse(minStepX, maxStepY - minFootSeparation, footMove)

  # Correct the previous offset on Y component.
  if footMove.y >=0:
    footMove.y  = footMove.y + minFootSeparation
  else:
    footMove.y = footMove.y - minFootSeparation
예제 #4
0
def clip_footstep_with_ellipse(foot):
    ''' Clip the foot move inside an ellipse defined by the foot's dimansions.
        foot is an almath.Pose2D (x, y, theta position).
    '''

    # Apply an offset to have Y component of foot move centered on 0.
    if (foot.y < -Params.min_step_y):
        foot.y = foot.y + Params.min_step_y
    elif (foot.y > Params.min_step_y):
        foot.y = foot.y - Params.min_step_y
    else:
      return

    # Clip the foot move to an ellipse using ALMath method.
    if foot.x >= 0:
        almath.clipFootWithEllipse(Params.max_step_x, Params.max_step_y - Params.min_step_y, foot)
    else:
        almath.clipFootWithEllipse(Params.min_step_x, Params.max_step_y - Params.min_step_y, foot)

    # Correct the previous offset on Y component.
    if foot.y >=0:
        foot.y  = foot.y + Params.min_step_y
    else:
        foot.y = foot.y - Params.min_step_y