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 = Vector(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 compute_local_p0_p1_p3(self):
self.p0 = Vector(0, 0, 0)
self.p1 = Vector(self.hexapod.coxia, 0, 0)
# Find p3 aka foot tip (ground contact) with respect to the local leg frame
rho = angle_between(self.unit_coxia_vector, self.body_to_foot_vector)
body_to_foot_length = length(self.body_to_foot_vector)
p3x = body_to_foot_length * np.cos(np.radians(rho))
p3z = -body_to_foot_length * np.sin(np.radians(rho))
self.p3 = Vector(p3x, 0, p3z)
def test_shared_set_points():
points = [
Vector(1, 2, 3, "a"),
Vector(1, 2, 3, "b"),
Vector(1, 2, 3, "c"),
Vector(1, 2, 3, "d"),
]
vh = VirtualHexapod(BASE_DIMENSIONS)
update_hexapod_points(vh, 1, points)
for leg_point, point in zip(vh.legs[1].all_points, points):
assert leg_point is point
def update(self, poses, assume_ground_targets=True):
self.body_rotation_frame = None
might_twist = find_if_might_twist(self, poses)
old_contacts = deepcopy(self.ground_contacts)
# Update leg poses
for pose in poses.values():
i = pose["id"]
self.legs[i].change_pose(pose["coxia"], pose["femur"], pose["tibia"])
# Find new orientation of the body (new normal)
# distance of cog from ground and which legs are on the ground
if assume_ground_targets:
# We are positive that our assumed target ground contact points
# are correct then we don't have to test all possible cases
legs, n_axis, height = gc.compute_orientation_properties(self.legs)
else:
legs, n_axis, height = gc2.compute_orientation_properties(self.legs)
if n_axis is None:
raise Exception("❗Pose Unstable. COG not inside support polygon.")
# Tilt and shift the hexapod based on new normal
frame = frame_to_align_vector_a_to_b(n_axis, Vector(0, 0, 1))
self.rotate_and_shift(frame, height)
self._update_local_frame(frame)
# Twist around the new normal if you have to
self.ground_contacts = [leg.ground_contact() for leg in legs]
if might_twist:
twist_frame = find_twist_frame(old_contacts, self.ground_contacts)
self.rotate_and_shift(twist_frame)
might_print_hexapod(self, poses)
def __init__(self, hexapod, ik_parameters):
self.hexapod = hexapod
self.params = ik_parameters
self.leg_x_axis = Vector(1, 0, 0)
self.update_body_and_ground_contact_points()
self.poses = deepcopy(HEXAPOD_POSE)
self.legs_up_in_the_air = []
for i in range(hexapod.LEG_COUNT):
self.store_known_points(i)
self.store_initial_vectors()
self.compute_local_p0_p1_p3()
self.compute_beta_gamma_local_p2()
self.compute_alpha_and_twist_frame(i)
self.points = [self.p0, self.p1, self.p2, self.p3]
# Update point wrt world frame
self.update_to_global_points()
# Update hexapod's points to what we computed
update_hexapod_points(self.hexapod, i, self.points)
# Final p1, p2, p3, beta and gamma computed at this point
self.poses[i]["coxia"] = self.alpha
self.poses[i]["femur"] = self.beta
self.poses[i]["tibia"] = self.gamma
might_print_ik(self.poses, self.params, self.hexapod)
def find_twist_to_recompute_hexapod(a, b):
twist = angle_between(a, b)
z_axis = Vector(0, 0, -1)
is_ccw = is_counter_clockwise(a, b, z_axis)
if is_ccw:
twist = -twist
twist_frame = rotz(twist)
return twist, twist_frame
def find_twist_frame(old_ground_contacts, new_ground_contacts):
# This is the frame used to twist the model about the z axis
def _make_contact_dict(contact_list):
return {leg_point.name: leg_point for leg_point in contact_list}
def _twist(v1, v2):
# https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/
theta = atan2(v2.y, v2.x) - atan2(v1.y, v1.x)
return rotz(degrees(theta))
# Make dictionary mapping contact point name and leg_contact_point
old_contacts = _make_contact_dict(old_ground_contacts)
new_contacts = _make_contact_dict(new_ground_contacts)
# Find at least one point that's the same
same_point_name = None
for key in old_contacts:
if key in new_contacts:
same_point_name = key
break
# We don't know how to rotate if we don't
# know at least one point that's on the ground
# before and after the movement,
# so we assume that the hexapod didn't move
if same_point_name is None:
return np.eye(4)
old = old_contacts[same_point_name]
new = new_contacts[same_point_name]
# Get the projection of these points in the ground
old_vector = Vector(old.x, old.y, 0)
new_vector = Vector(new.x, new.y, 0)
twist_frame = _twist(new_vector, old_vector)
# ❗IMPORTANT: We are assuming that because the point
# is on the ground before and after
# They should be at the same point after movement
# I can't think of a case that contradicts this as of this moment
return twist_frame
def __init__(self, f, m, s):
self.f = f
self.m = m
self.s = s
self.cog = Vector(0, 0, 0, name="center-of-gravity")
self.head = Vector(0, s, 0, name="head")
self.vertices = [
Vector(m, 0, 0, name=Hexagon.VERTEX_NAMES[0]),
Vector(f, s, 0, name=Hexagon.VERTEX_NAMES[1]),
Vector(-f, s, 0, name=Hexagon.VERTEX_NAMES[2]),
Vector(-m, 0, 0, name=Hexagon.VERTEX_NAMES[3]),
Vector(-f, -s, 0, name=Hexagon.VERTEX_NAMES[4]),
Vector(f, -s, 0, name=Hexagon.VERTEX_NAMES[5]),
]
self.all_points = self.vertices + [self.cog, self.head]
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 = Vector(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 __init__(
self,
a,
b,
c,
alpha=0,
beta=0,
gamma=0,
coxia_axis=0,
new_origin=Vector(0, 0, 0),
name=None,
id_number=None,
):
self.a = a
self.b = b
self.c = c
self.new_origin = new_origin
self.coxia_axis = coxia_axis
self.id = id_number
self.name = name
self.change_pose(alpha, beta, gamma)
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 = Vector(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
given_dimensions = {
"front": 65,
"side": 101,
"middle": 122,
"coxia": 83,
"femur": 108,
"tibia": 177,
}
# ********************************
# Correct Body Vectors
# ********************************
correct_body_points = [
Vector(x=+116.86, y=+35.03, z=+65.62, name="right-middle"),
Vector(x=+33.27, y=+115.41, z=+65.62, name="right-front"),
Vector(x=-91.26, y=+78.09, z=+65.62, name="left-front"),
Vector(x=-116.86, y=-35.03, z=+65.62, name="left-middle"),
Vector(x=-33.27, y=-115.41, z=+65.62, name="left-back"),
Vector(x=+91.26, y=-78.09, z=+65.62, name="right-back"),
Vector(x=+0.00, y=+0.00, z=+65.62, name="center-of-gravity"),
Vector(x=-29.00, y=+96.75, z=+65.62, name="head"),
]
# ********************************
# Correct Leg Vectors
# ********************************
leg0_points = [
Vector(x=+116.86, y=+35.03, z=+65.62, name="right-middle-body-contact"),
given_dimensions = {
"front": 76,
"side": 92,
"middle": 123,
"coxia": 58,
"femur": 177,
"tibia": 151,
}
# ********************************
# Correct Body Vectors
# ********************************
correct_body_points = [
Vector(x=+123.00, y=+0.00, z=+0.00, name="right-middle"),
Vector(x=+76.00, y=+92.00, z=+0.00, name="right-front"),
Vector(x=-76.00, y=+92.00, z=+0.00, name="left-front"),
Vector(x=-123.00, y=+0.00, z=+0.00, name="left-middle"),
Vector(x=-76.00, y=-92.00, z=+0.00, name="left-back"),
Vector(x=+76.00, y=-92.00, z=+0.00, name="right-back"),
Vector(x=+0.00, y=+0.00, z=+0.00, name="center-of-gravity"),
Vector(x=+0.00, y=+92.00, z=+0.00, name="head"),
]
# ********************************
# Correct Leg Vectors
# ********************************
leg0_points = [
Vector(x=+123.00, y=+0.00, z=+0.00, name="right-middle-body-contact"),
def inverse_kinematics_update(hexapod, ik_parameters):
tx = ik_parameters["percent_x"] * hexapod.mid
ty = ik_parameters["percent_y"] * hexapod.side
tz = ik_parameters["percent_z"] * hexapod.tibia
rotx, roty, rotz = (
ik_parameters["rot_x"],
ik_parameters["rot_y"],
ik_parameters["rot_z"],
)
hexapod.update_stance(ik_parameters["hip_stance"],
ik_parameters["leg_stance"])
hexapod.detach_body_rotate_and_translate(rotx, roty, rotz, tx, ty, tz)
if body_contact_shoved_on_ground(hexapod):
raise Exception(BODY_ON_GROUND_ALERT_MSG)
x_axis = Vector(1, 0, 0)
legs_up_in_the_air = []
for i in range(hexapod.LEG_COUNT):
leg_name = hexapod.legs[i].name
body_contact = hexapod.body.vertices[i]
foot_tip = hexapod.legs[i].foot_tip()
body_to_foot_vector = vector_from_to(body_contact, foot_tip)
# find the coxia vector which is the vector
# from body contact point to joint between coxia and femur limb
projection = project_vector_onto_plane(body_to_foot_vector,
hexapod.z_axis)
unit_coxia_vector = get_unit_vector(projection)
coxia_vector = scalar_multiply(unit_coxia_vector, hexapod.coxia)
# coxia point / joint is the point connecting the coxia and tibia limbs
coxia_point = add_vectors(body_contact, coxia_vector)
if coxia_point.z < foot_tip.z:
raise Exception(COXIA_ON_GROUND_ALERT_MSG)
# *******************
# 1. Compute p0, p1 and (possible) p3 wrt leg frame
# *******************
p0 = Vector(0, 0, 0)
p1 = Vector(hexapod.coxia, 0, 0)
# Find p3 aka foot tip (ground contact) with respect to the local leg frame
rho = angle_between(unit_coxia_vector, body_to_foot_vector)
body_to_foot_length = length(body_to_foot_vector)
p3x = body_to_foot_length * np.cos(np.radians(rho))
p3z = -body_to_foot_length * np.sin(np.radians(rho))
p3 = Vector(p3x, 0, p3z)
# *******************
# 2. Compute p2, beta, gamma and final p3 wrt leg frame
# *******************
# These values are needed to compute
# p2 aka tibia joint (point between femur limb and tibia limb)
coxia_to_foot_vector2d = vector_from_to(p1, p3)
d = length(coxia_to_foot_vector2d)
# If we can form this triangle this means
# we can probably can reach the target ground contact point
if is_triangle(hexapod.tibia, hexapod.femur, d):
# .................................
# CASE A: a triangle can be formed with
# coxia to foot vector, hexapod's femur and tibia
# .................................
theta = angle_opposite_of_last_side(d, hexapod.femur,
hexapod.tibia)
phi = angle_between(coxia_to_foot_vector2d, x_axis)
beta = theta - phi # case 1 or 2
if p3.z > 0: # case 3
beta = theta + phi
z_ = hexapod.femur * np.sin(np.radians(beta))
x_ = p1.x + hexapod.femur * np.cos(np.radians(beta))
p2 = Vector(x_, 0, z_)
femur_vector = vector_from_to(p1, p2)
tibia_vector = vector_from_to(p2, p3)
gamma = 90 - angle_between(femur_vector, tibia_vector)
if p2.z < p3.z:
raise Exception(cant_reach_alert_msg(leg_name, "blocking"))
else:
# .................................
# CASE B: It's impossible to reach target ground point
# .................................
if d + hexapod.tibia < hexapod.femur:
raise Exception(cant_reach_alert_msg(leg_name, "femur"))
if d + hexapod.femur < hexapod.tibia:
raise Exception(cant_reach_alert_msg(leg_name, "tibia"))
# Then hexapod.femur + hexapod.tibia < d:
legs_up_in_the_air.append(leg_name)
LEGS_TOO_SHORT, alert_msg = legs_too_short(legs_up_in_the_air)
if LEGS_TOO_SHORT:
raise Exception(alert_msg)
femur_tibia_direction = get_unit_vector(coxia_to_foot_vector2d)
femur_vector = scalar_multiply(femur_tibia_direction,
hexapod.femur)
p2 = add_vectors(p1, femur_vector)
tibia_vector = scalar_multiply(femur_tibia_direction,
hexapod.tibia)
p3 = add_vectors(p2, tibia_vector)
# Find beta and gamma
gamma = 0.0
leg_x_axis = Vector(1, 0, 0)
beta = angle_between(leg_x_axis, femur_vector)
if femur_vector.z < 0:
beta = -beta
# Final p1, p2, p3, beta and gamma computed at this point
not_within_range, alert_msg = beta_gamma_not_in_range(
beta, gamma, leg_name)
if not_within_range:
raise Exception(alert_msg)
# *******************
# 3. Compute alpha and twist_frame
# Find frame used to twist the leg frame wrt to hexapod's body contact point's x axis
# *******************
alpha, twist_frame = find_twist_frame(hexapod, unit_coxia_vector)
alpha = compute_twist_wrt_to_world(alpha, hexapod.body.COXIA_AXES[i])
alpha_limit, alert_msg = angle_above_limit(alpha, ALPHA_MAX_ANGLE,
leg_name, "(alpha/coxia)")
if alpha_limit:
raise Exception(alert_msg)
# *******************
# 4. Update hexapod points and finally update the pose
# *******************
points = [p0, p1, p2, p3]
might_print_points(points, leg_name)
# Convert points from local leg coordinate frame to world coordinate frame
for point in points:
point.update_point_wrt(twist_frame)
if ASSERTION_ENABLED:
assert hexapod.body_rotation_frame is not None, "No rotation frame!"
point.update_point_wrt(hexapod.body_rotation_frame)
point.move_xyz(body_contact.x, body_contact.y, body_contact.z)
might_sanity_leg_lengths_check(hexapod, leg_name, points)
might_sanity_beta_gamma_check(beta, gamma, leg_name, points)
# Update hexapod's points to what we computed
update_hexapod_points(hexapod, i, points)
poses[i]["coxia"] = alpha
poses[i]["femur"] = beta
poses[i]["tibia"] = gamma
might_print_ik(poses, ik_parameters, hexapod)
return poses, hexapod
def _init_local_frame(self):
self.x_axis = Vector(1, 0, 0, name="hexapod x axis")
self.y_axis = Vector(0, 1, 0, name="hexapod y axis")
self.z_axis = Vector(0, 0, 1, name="hexapod z axis")
class VirtualHexapod:
LEG_COUNT = 6
__slots__ = (
"body",
"legs",
"dimensions",
"coxia",
"femur",
"tibia",
"front",
"side",
"mid",
"body_rotation_frame",
"ground_contacts",
"x_axis",
"y_axis",
"z_axis",
)
def __init__(self, dimensions):
self._store_attributes(dimensions)
self._init_legs()
self._init_local_frame()
def update(self, poses, assume_ground_targets=True):
self.body_rotation_frame = None
might_twist = find_if_might_twist(self, poses)
old_contacts = deepcopy(self.ground_contacts)
# Update leg poses
for pose in poses.values():
i = pose["id"]
self.legs[i].change_pose(pose["coxia"], pose["femur"], pose["tibia"])
# Find new orientation of the body (new normal)
# distance of cog from ground and which legs are on the ground
if assume_ground_targets:
# We are positive that our assumed target ground contact points
# are correct then we don't have to test all possible cases
legs, n_axis, height = gc.compute_orientation_properties(self.legs)
else:
legs, n_axis, height = gc2.compute_orientation_properties(self.legs)
if n_axis is None:
raise Exception("❗Pose Unstable. COG not inside support polygon.")
# Tilt and shift the hexapod based on new normal
frame = frame_to_align_vector_a_to_b(n_axis, Vector(0, 0, 1))
self.rotate_and_shift(frame, height)
self._update_local_frame(frame)
# Twist around the new normal if you have to
self.ground_contacts = [leg.ground_contact() for leg in legs]
if might_twist:
twist_frame = find_twist_frame(old_contacts, self.ground_contacts)
self.rotate_and_shift(twist_frame)
might_print_hexapod(self, poses)
def detach_body_rotate_and_translate(self, rx, ry, rz, tx, ty, tz):
# Detach the body of the hexapod from the legs
# then rotate and translate body as if a separate entity
frame = frame_rotxyz(rx, ry, rz)
self.body_rotation_frame = frame
for point in self.body.all_points:
point.update_point_wrt(frame)
point.move_xyz(tx, ty, tz)
self._update_local_frame(frame)
def move_xyz(self, tx, ty, tz):
for point in self.body.all_points:
point.move_xyz(tx, ty, tz)
for leg in self.legs:
for point in leg.all_points:
point.move_xyz(tx, ty, tz)
def update_stance(self, hip_stance, leg_stance):
pose = deepcopy(HEXAPOD_POSE)
pose[1]["coxia"] = -hip_stance # right_front
pose[2]["coxia"] = hip_stance # left_front
pose[4]["coxia"] = -hip_stance # left_back
pose[5]["coxia"] = hip_stance # right_back
for leg in pose.values():
leg["femur"] = leg_stance
leg["tibia"] = -leg_stance
self.update(pose)
def sum_of_dimensions(self):
f, m, s = self.front, self.mid, self.side
a, b, c = self.coxia, self.femur, self.tibia
return f + m + s + a + b + c
def _store_attributes(self, dimensions):
self.body_rotation_frame = None
self.dimensions = dimensions
self.coxia = dimensions["coxia"]
self.femur = dimensions["femur"]
self.tibia = dimensions["tibia"]
self.front = dimensions["front"]
self.mid = dimensions["middle"]
self.side = dimensions["side"]
self.body = Hexagon(self.front, self.mid, self.side)
def _init_legs(self):
self.legs = []
for i in range(VirtualHexapod.LEG_COUNT):
linkage = Linkage(
self.coxia,
self.femur,
self.tibia,
coxia_axis=Hexagon.COXIA_AXES[i],
new_origin=self.body.vertices[i],
name=Hexagon.VERTEX_NAMES[i],
id_number=i,
)
self.legs.append(linkage)
self.ground_contacts = [leg.ground_contact() for leg in self.legs]
def rotate_and_shift(self, frame, height=0):
for vertex in self.body.all_points:
vertex.update_point_wrt(frame, height)
for leg in self.legs:
leg.update_leg_wrt(frame, height)
def _init_local_frame(self):
self.x_axis = Vector(1, 0, 0, name="hexapod x axis")
self.y_axis = Vector(0, 1, 0, name="hexapod y axis")
self.z_axis = Vector(0, 0, 1, name="hexapod z axis")
def _update_local_frame(self, frame):
# Update the x, y, z axis centered at cog of hexapod
self.x_axis.update_point_wrt(frame)
self.y_axis.update_point_wrt(frame)
self.z_axis.update_point_wrt(frame)
},
5: {
"coxia": -5,
"femur": 17,
"tibia": 2,
"name": "right-back",
"id": 5
},
}
# ********************************
# Correct Body Vectors
# ********************************
correct_body_points = [
Vector(x=+97.74, y=+69.20, z=+123.97, name="right-middle"),
Vector(x=-3.68, y=+111.78, z=+103.96, name="right-front"),
Vector(x=-120.97, y=+28.74, z=+146.94, name="left-front"),
Vector(x=-97.74, y=-69.20, z=+195.60, name="left-middle"),
Vector(x=+3.68, y=-111.78, z=+215.60, name="left-back"),
Vector(x=+120.97, y=-28.74, z=+172.63, name="right-back"),
Vector(x=+0.00, y=+0.00, z=+159.78, name="center-of-gravity"),
Vector(x=-62.33, y=+70.26, z=+125.45, name="head"),
]
# ********************************
# Leg Vectors
# ********************************
leg0_points = [
Vector(x=+97.74, y=+69.20, z=+123.97, name="right-middle-body-contact"),
},
5: {
"name": "right-back",
"id": 5,
"coxia": -38.67228081907621,
"femur": 18.790558327957655,
"tibia": -30.220554892132796,
},
}
# ********************************
# Correct Body Vectors
# ********************************
correct_body_points = [
Vector(x=+112.68, y=+45.33, z=+126.66, name="right-middle"),
Vector(x=+5.57, y=+122.87, z=+116.68, name="right-front"),
Vector(x=-90.76, y=+84.12, z=+95.32, name="left-front"),
Vector(x=-112.68, y=-45.33, z=+76.69, name="left-middle"),
Vector(x=-5.57, y=-122.87, z=+86.67, name="left-back"),
Vector(x=+90.76, y=-84.12, z=+108.03, name="right-back"),
Vector(x=+0.00, y=+0.00, z=+101.67, name="center-of-gravity"),
Vector(x=-42.60, y=+103.49, z=+106.00, name="head"),
]
# ********************************
# Leg Vectors
# ********************************
leg0_points = [
Vector(x=+112.68, y=+45.33, z=+126.66, name="right-middle-body-contact"),