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
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
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
