original_docstring:  create constraint on ankle position and orientation 
docstring: create constraint on ankle position and orientation
docstring_tokens: create constraint on ankle position and orientation
code: def generate_grounding_constraint_from_heel_and_toe(self, frames, ankle_joint_name, heel_joint_name, toe_joint_name, frame_idx, end_frame):
          """ create constraint on ankle position and orientation """
          # get target global ankle orientation based on the direction between grounded heel and toe
          ct = self.get_previous_joint_position_from_buffer(frames, frame_idx, end_frame, toe_joint_name)
          ct[1] = self.scene_interface.get_height(ct[0], ct[2])
          ch = self.get_previous_joint_position_from_buffer(frames, frame_idx, end_frame, heel_joint_name)
          ch[1] = self.scene_interface.get_height(ct[0], ct[2])
          target_direction = normalize(ct - ch)
          t = self.skeleton.nodes[toe_joint_name].get_global_position(frames[frame_idx])
          h = self.skeleton.nodes[heel_joint_name].get_global_position(frames[frame_idx])
          original_direction = normalize(t-h)
  
          global_delta_q = quaternion_from_vector_to_vector(original_direction, target_direction)
          global_delta_q = normalize(global_delta_q)
  
          m = self.skeleton.nodes[ankle_joint_name].get_global_matrix(frames[frame_idx])
          m[:3, 3] = [0, 0, 0]
          oq = quaternion_from_matrix(m)
          oq = normalize(oq)
          orientation = normalize(quaternion_multiply(global_delta_q, oq))
  
          self.orientation_constraint_buffer[frame_idx][ankle_joint_name] = orientation  # save orientation to buffer
  
          # set target ankle position based on the  grounded heel and the global target orientation of the ankle
          m = quaternion_matrix(orientation)[:3,:3]
          target_heel_offset = np.dot(m, self.skeleton.nodes[heel_joint_name].offset)
          ca = ch - target_heel_offset
          #print "set ankle constraint both", ch, ca, target_heel_offset
          constraint = MotionGroundingConstraint(frame_idx, ankle_joint_name, ca, None, orientation)
          constraint.toe_position = ct
          constraint.heel_position = ch
          return constraint
code_tokens: def generate_grounding_constraint_from_heel_and_toe ( self , frames , ankle_joint_name , heel_joint_name , toe_joint_name , frame_idx , end_frame ) : ct = self . get_previous_joint_position_from_buffer ( frames , frame_idx , end_frame , toe_joint_name ) ct [ 1 ] = self . scene_interface . get_height ( ct [ 0 ] , ct [ 2 ] ) ch = self . get_previous_joint_position_from_buffer ( frames , frame_idx , end_frame , heel_joint_name ) ch [ 1 ] = self . scene_interface . get_height ( ct [ 0 ] , ct [ 2 ] ) target_direction = normalize ( ct - ch ) t = self . skeleton . nodes [ toe_joint_name ] . get_global_position ( frames [ frame_idx ] ) h = self . skeleton . nodes [ heel_joint_name ] . get_global_position ( frames [ frame_idx ] ) original_direction = normalize ( t - h ) global_delta_q = quaternion_from_vector_to_vector ( original_direction , target_direction ) global_delta_q = normalize ( global_delta_q ) m = self . skeleton . nodes [ ankle_joint_name ] . get_global_matrix ( frames [ frame_idx ] ) m [ : 3 , 3 ] = [ 0 , 0 , 0 ] oq = quaternion_from_matrix ( m ) oq = normalize ( oq ) orientation = normalize ( quaternion_multiply ( global_delta_q , oq ) ) self . orientation_constraint_buffer [ frame_idx ] [ ankle_joint_name ] = orientation m = quaternion_matrix ( orientation ) [ : 3 , : 3 ] target_heel_offset = np . dot ( m , self . skeleton . nodes [ heel_joint_name ] . offset ) ca = ch - target_heel_offset constraint = MotionGroundingConstraint ( frame_idx , ankle_joint_name , ca , None , orientation ) constraint . toe_position = ct constraint . heel_position = ch return constraint
short_docstring: create constraint on ankle position and orientation
short_docstring_tokens: create constraint on ankle position and orientation
comment: """ create constraint on ankle position and orientation """$SEP$# get target global ankle orientation based on the direction between grounded heel and toe$SEP$# save orientation to buffer$SEP$# set target ankle position based on the  grounded heel and the global target orientation of the ankle$SEP$#print "set ankle constraint both", ch, ca, target_heel_offset