Ejemplo n.º 1
0
 def canLift(self, leg_index):
     """ Takes in a set of leg indices, and reports if all of them can be lifted simultaneously
     """
     foot_pos = self.getFootPositionsInBodyFrame()
     COM = self.getCOM()
     g = self.imu_orientation[0:7:3] # This should be changed to align with
         # the acceleration vector of the imu, not the auto-corrected downward vector.
     on_ground = [self.getLegs()[i].isFootOnGround() for i in range(NUM_LEGS)]
     for i in leg_index:
         on_ground[i] = False
     on_ground_indices = nonzero(on_ground)[0]
     possible_triangles = formPermutations(on_ground_indices,3)
     can_lift = False
     for tri in possible_triangles:
         if inTriangle(COM,foot_pos[tri[0]],foot_pos[tri[1]],foot_pos[tri[2]],g):
             can_lift = True
             break
     return can_lift
Ejemplo n.º 2
0
 def canLift(self, leg_index):
     """ Takes in a set of leg indices, and reports if all of them can be lifted simultaneously
     """
     foot_pos = self.getFootPositionsInBodyFrame()
     COM = [0, 0, 0]  # Calculate this for real in another section of the body model?
     g = [
         0,
         0,
         -1,
     ]  # self.imu_orientation # DELETE THIS COMMENT once someone confirms this is in the form of a vector pointing parallel to gravity
     on_ground = [self.getLegs()[i].isFootOnGround() for i in range(NUM_LEGS)]
     for i in leg_index:
         on_ground[i] = False
     on_ground_indices = nonzero(on_ground)[0]
     possible_triangles = formPermutations(on_ground_indices, 3)
     can_lift = False
     for tri in possible_triangles:
         if inTriangle(COM, foot_pos[tri[0]], foot_pos[tri[1]], foot_pos[tri[2]], g):
             can_lift = True
             break
     return can_lift
Ejemplo n.º 3
0
 def canLift(self, leg_index):
     """ Takes in a set of leg indices, and reports if all of them can be lifted simultaneously
     """
     foot_pos = self.getFootPositionsInBodyFrame()
     COM = [
         0, 0, 0
     ]  # Calculate this for real in another section of the body model?
     g = [
         0, 0, -1
     ]  # self.imu_orientation # DELETE THIS COMMENT once someone confirms this is in the form of a vector pointing parallel to gravity
     on_ground = [
         self.getLegs()[i].isFootOnGround() for i in range(NUM_LEGS)
     ]
     for i in leg_index:
         on_ground[i] = False
     on_ground_indices = nonzero(on_ground)[0]
     possible_triangles = formPermutations(on_ground_indices, 3)
     can_lift = False
     for tri in possible_triangles:
         if inTriangle(COM, foot_pos[tri[0]], foot_pos[tri[1]],
                       foot_pos[tri[2]], g):
             can_lift = True
             break
     return can_lift