nao_footstep_clipping.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import almath
00004 
00005 
00006 # Struct with clipping params initialized with clipping values for the NAO robot
00007 class Params(object):
00008     # Various parameters describing step characteristics.
00009     min_step_x = -0.04
00010     min_step_y = 0.088
00011     max_step_x = 0.08
00012     max_step_y = 0.16
00013     max_step_theta = 0.5 # 0.5xxx
00014     min_step_theta = -max_step_theta
00015 
00016     # Bounding boxes of NAO's feet.
00017     foot_box_r_fl = almath.Pose2D(0.11, 0.038, 0.0)
00018     foot_box_r_fr = almath.Pose2D(0.11, -0.050, 0.0)
00019     foot_box_r_rr = almath.Pose2D(-0.047, -0.050, 0.0)
00020     foot_box_r_rl = almath.Pose2D(-0.047, 0.038, 0.0)
00021     foot_box_r = almath.vectorPose2D([foot_box_r_fl, foot_box_r_fr,
00022                                       foot_box_r_rr, foot_box_r_rl])
00023 
00024     foot_box_l_fl = almath.Pose2D(0.11, 0.050, 0.0)
00025     foot_box_l_fr = almath.Pose2D(0.11, -0.038, 0.0)
00026     foot_box_l_rr = almath.Pose2D(-0.047, -0.038, 0.0)
00027     foot_box_l_rl = almath.Pose2D(-0.047,  0.050, 0.0)
00028     foot_box_l = almath.vectorPose2D([foot_box_l_fl, foot_box_l_fr,
00029                                       foot_box_l_rr, foot_box_l_rl])
00030 
00031 
00032 def clip_data(min, max, value):
00033     ''' Clip value between two extremes. '''
00034     clipped = value
00035 
00036     if (clipped < min):
00037         clipped = min
00038     if (clipped > max):
00039         clipped = max
00040 
00041     return clipped
00042 
00043 
00044 def clip_footstep_on_gait_config(foot, is_left_support):
00045     ''' Clip the foot move so that it does not exceed the maximum
00046         size of steps.
00047         foot is an almath.Pose2D (x, y, theta position).
00048         is_left_support must be set to True if the move is on the right leg
00049         (the robot is supporting itself on the left leg).
00050     '''
00051 
00052     # Clip X.
00053     clipped_x = clip_data(Params.min_step_x, Params.max_step_x, foot.x)
00054     foot.x = clipped_x
00055 
00056     # Clip Y.
00057     if not is_left_support:
00058         clipped_y = clip_data(Params.min_step_y, Params.max_step_y, foot.y)
00059     else:
00060         clipped_y = clip_data(-Params.max_step_y, -Params.min_step_y, foot.y)
00061     foot.y = clipped_y
00062 
00063     # Clip Theta.
00064     clipped_theta = clip_data(Params.min_step_theta, Params.max_step_theta, foot.theta)
00065     foot.theta = clipped_theta
00066 
00067 
00068 def clip_footstep_with_ellipse(foot):
00069     ''' Clip the foot move inside an ellipse defined by the foot's dimansions.
00070         foot is an almath.Pose2D (x, y, theta position).
00071     '''
00072 
00073     # Apply an offset to have Y component of foot move centered on 0.
00074     if (foot.y < -Params.min_step_y):
00075         foot.y = foot.y + Params.min_step_y
00076     elif (foot.y > Params.min_step_y):
00077         foot.y = foot.y - Params.min_step_y
00078     else:
00079       return
00080 
00081     # Clip the foot move to an ellipse using ALMath method.
00082     if foot.x >= 0:
00083         almath.clipFootWithEllipse(Params.max_step_x, Params.max_step_y - Params.min_step_y, foot)
00084     else:
00085         almath.clipFootWithEllipse(Params.min_step_x, Params.max_step_y - Params.min_step_y, foot)
00086 
00087     # Correct the previous offset on Y component.
00088     if foot.y >=0:
00089         foot.y  = foot.y + Params.min_step_y
00090     else:
00091         foot.y = foot.y - Params.min_step_y
00092 
00093 
00094 def clip_footstep_to_avoid_collision(foot, is_left_support):
00095     ''' Clip the foot move to avoid collision with the other foot.
00096         foot is an almath.Pose2D (x, y, theta position).
00097         is_left_support must be set to True if the move is on the right leg
00098         (the robot is supporting itself on the left leg).
00099     '''
00100 
00101     # Use ALMath method.
00102     almath.avoidFootCollision(Params.foot_box_l, Params.foot_box_r, is_left_support, foot)
00103 
00104 
00105 def clip_footstep(foot, is_left_support):
00106     ''' Clipping functions to avoid any warnings and undesired effects
00107         when sending the footsteps to ALMotion.
00108         foot is an almath.Pose2D (x, y, theta position)
00109         is_left_support must be set to True if the move is on the right leg
00110         (the robot is supporting itself on the left leg).
00111     '''
00112 
00113     # First clip the foot move with gait config.
00114     clip_footstep_on_gait_config(foot, is_left_support)
00115     # Then clip it on an ellipse.
00116     clip_footstep_with_ellipse(foot)
00117     # Then make sure you do not have any collision.
00118     clip_footstep_to_avoid_collision(foot, is_left_support)
00119 
00120 
00121 def clip_footstep_tuple(foot, is_left_support):
00122     """
00123     Wrapper of clip_footstep to handle 'foot' as geometry_msgs.Pose2D.
00124     'foot' is a tuple (x, y, theta). A tuple (x_clip, y_clip, theta_clip) is
00125     returned.
00126     """
00127 
00128     foot = almath.Pose2D(*foot)
00129     clip_footstep(foot, is_left_support)
00130     return foot.x, foot.y, foot.theta


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Mon Oct 6 2014 02:39:17