parameters: (robot, n_steps, stepping_foot, dx, dy)
return_statement: return request
docstring: Sets up the problem to step forward with one foot (stepping_foot)
      Suppose stepping_foot = "r_foot"    
      Then problem constrains r_foot to move by (dx, dy), while the center
      of mass lies above the support polygon of l_foot
docstring_summary: Sets up the problem to step forward with one foot (stepping_foot)
      Suppose stepping_foot = "r_foot"    
      Then problem constrains r_foot to move by (dx, dy), while the center
      of mass lies above the support polygon of l_foot
docstring_tokens: Sets up the problem to step forward with one foot ( stepping_foot ) Suppose stepping_foot = r_foot Then problem constrains r_foot to move by ( dx dy ) while the center of mass lies above the support polygon of l_foot
function: def step_forward_request(robot, n_steps, stepping_foot, dx, dy): 
      """
      Sets up the problem to step forward with one foot (stepping_foot)
      Suppose stepping_foot = "r_foot"    
      Then problem constrains r_foot to move by (dx, dy), while the center
      of mass lies above the support polygon of l_foot
      """
      
      
      
      planted_foot = opposite_foot(stepping_foot)
      
      request = request_skeleton(n_steps)
      
      # fixed pose of planted foot
      planted_link = robot.GetLink(planted_foot)
      planted_transform = planted_link.GetTransform()
      planted_xyz, planted_wxyz = xyzQuatFromMatrix(planted_transform)
      
      # final pose of stepping foot
      stepping_link = robot.GetLink(stepping_foot)
      stepping_init_transform = stepping_link.GetTransform()    
      stepping_init_xyz = stepping_init_transform[:3,3]    
      
      z_angle = np.arctan2(dy, dx)
      stepping_final_xyz = stepping_init_xyz + np.array([dx, dy, 0])
      stepping_final_wxyz = rave.quatFromAxisAngle([0,0,1], z_angle)
      
      for i in xrange(1, n_steps):
          request["constraints"].extend([
              {
                  "type":"pose",
                  "name":"planted_pose",
                  "params":{
                      "xyz":list(planted_xyz),
                      "wxyz":list(planted_wxyz),
                      "link":planted_foot,
                      "timestep":i
                  }
              },
              {
                  "type":"zmp","name":"zmp_%i"%i,
                  "params":{"planted_links":[planted_foot], "timestep":i}
              }
          ])
          #if i < n_steps-1:
              #request["constraints"].append({
                  #"type":"foot_height",
                  #"params":{
                      #"height": .1,
                      #"timestep": i,
                      #"link":stepping_foot}})
          
      request["constraints"].append(
          {
              "type":"pose",
              "name":"step_pose",
              "params":{
                  "xyz" : list(stepping_final_xyz),
                  "wxyz" : list(stepping_final_wxyz),
                  "link" : stepping_foot,
                  "timestep":(n_steps-1)
              }
          }
      )
      return request
function_tokens: def step_forward_request ( robot , n_steps , stepping_foot , dx , dy ) : planted_foot = opposite_foot ( stepping_foot ) request = request_skeleton ( n_steps ) # fixed pose of planted foot planted_link = robot . GetLink ( planted_foot ) planted_transform = planted_link . GetTransform ( ) planted_xyz , planted_wxyz = xyzQuatFromMatrix ( planted_transform ) # final pose of stepping foot stepping_link = robot . GetLink ( stepping_foot ) stepping_init_transform = stepping_link . GetTransform ( ) stepping_init_xyz = stepping_init_transform [ : 3 , 3 ] z_angle = np . arctan2 ( dy , dx ) stepping_final_xyz = stepping_init_xyz + np . array ( [ dx , dy , 0 ] ) stepping_final_wxyz = rave . quatFromAxisAngle ( [ 0 , 0 , 1 ] , z_angle ) for i in xrange ( 1 , n_steps ) : request [ "constraints" ] . extend ( [ { "type" : "pose" , "name" : "planted_pose" , "params" : { "xyz" : list ( planted_xyz ) , "wxyz" : list ( planted_wxyz ) , "link" : planted_foot , "timestep" : i } } , { "type" : "zmp" , "name" : "zmp_%i" % i , "params" : { "planted_links" : [ planted_foot ] , "timestep" : i } } ] ) #if i < n_steps-1: #request["constraints"].append({ #"type":"foot_height", #"params":{ #"height": .1, #"timestep": i, #"link":stepping_foot}}) request [ "constraints" ] . append ( { "type" : "pose" , "name" : "step_pose" , "params" : { "xyz" : list ( stepping_final_xyz ) , "wxyz" : list ( stepping_final_wxyz ) , "link" : stepping_foot , "timestep" : ( n_steps - 1 ) } } ) return request