def change_pose(self, alpha, beta, gamma):
self.alpha = alpha
self.beta = beta
self.gamma = gamma
# frame_ab is the pose of frame_b wrt frame_a
frame_01 = frame_yrotate_xtranslate(theta=-self.beta, x=self.a)
frame_12 = frame_yrotate_xtranslate(theta=90 - self.gamma, x=self.b)
frame_23 = frame_yrotate_xtranslate(theta=0, x=self.c)
frame_02 = np.matmul(frame_01, frame_12)
frame_03 = np.matmul(frame_02, frame_23)
new_frame = frame_zrotate_xytranslate(self.coxia_axis + self.alpha,
self.new_origin.x,
self.new_origin.y)
# find points wrt to body contact point
p0 = Point(0, 0, 0)
p1 = p0.get_point_wrt(frame_01)
p2 = p0.get_point_wrt(frame_02)
p3 = p0.get_point_wrt(frame_03)
# find points wrt to center of gravity
p0 = deepcopy(self.new_origin)
p0.name += "-body-contact"
p1 = p1.get_point_wrt(new_frame, name=self.name + "-coxia")
p2 = p2.get_point_wrt(new_frame, name=self.name + "-femur")
p3 = p3.get_point_wrt(new_frame, name=self.name + "-tibia")
self.all_points = [p0, p1, p2, p3]
self.ground_contact_point = self.compute_ground_contact()
def check_stability(a, b, c):
"""
Check if the points a, b, c form a stable triangle.
If the center of gravity p (0, 0) on xy plane
is inside projection (in the xy plane) of
the triangle defined by point a, b, c, then this is stable
"""
return is_point_inside_triangle(Point(0, 0, 0), a, b, c)
def compute_when_triangle_can_form(self):
theta = angle_opposite_of_last_side(self.d, self.hexapod.femur,
self.hexapod.tibia)
phi = angle_between(self.coxia_to_foot_vector2d, self.leg_x_axis)
self.beta = theta - phi # case 1 or 2
if self.p3.z > 0: # case 3
self.beta = theta + phi
z_ = self.hexapod.femur * np.sin(np.radians(self.beta))
x_ = self.p1.x + self.hexapod.femur * np.cos(np.radians(self.beta))
self.p2 = Point(x_, 0, z_)
femur_vector = vector_from_to(self.p1, self.p2)
tibia_vector = vector_from_to(self.p2, self.p3)
self.gamma = 90 - angle_between(femur_vector, tibia_vector)
if self.p2.z < self.p3.z:
raise Exception(cant_reach_alert_msg(self.leg_name, "blocking"))
def is_stable(p1, p2, p3, tol=0.001):
"""
Determines stability of the pose.
Determine if projection of 3D point p
onto the plane defined by p1, p2, p3
is within a triangle defined by p1, p2, p3.
"""
p = Point(0, 0, 0)
u = vector_from_to(p1, p2)
v = vector_from_to(p1, p3)
n = cross(u, v)
w = vector_from_to(p1, p)
n2 = dot(n, n)
beta = dot(cross(u, w), n) / n2
gamma = dot(cross(w, v), n) / n2
alpha = 1 - gamma - beta
# then coordinate of the projected point (p_) of point p
# p_ = alpha * p1 + beta * p2 + gamma * p3
min_val = -tol
max_val = 1 + tol
cond1 = min_val <= alpha <= max_val
cond2 = min_val <= beta <= max_val
cond3 = min_val <= gamma <= max_val
return cond1 and cond2 and cond3
def test_set_leg_point():
vh = VirtualHexapod(BASE_DIMENSIONS)
point = Point(1, 2, 3, "b")
vh.legs[0].p0 = Point(1, 2, 3, "b")
assert vh.legs[0].p0 == point
assert vh.legs[0].all_points[0] == point
