def might_sanity_leg_lengths_check(hexapod, leg_name, points): if not ASSERTION_ENABLED: return coxia = length(vector_from_to(points[0], points[1])) femur = length(vector_from_to(points[1], points[2])) tibia = length(vector_from_to(points[2], points[3])) same_length = np.isclose(hexapod.coxia, coxia, atol=1) assert same_length, wrong_length_msg(leg_name, "coxia", coxia) same_length = np.isclose(hexapod.femur, femur, atol=1) assert same_length, wrong_length_msg(leg_name, "femur", femur) same_length = np.isclose(hexapod.tibia, tibia, atol=1) assert same_length, wrong_length_msg(leg_name, "tibia", tibia)
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 might_sanity_beta_gamma_check(beta, gamma, leg_name, points): if not ASSERTION_ENABLED: return coxia = vector_from_to(points[0], points[1]) femur = vector_from_to(points[1], points[2]) tibia = vector_from_to(points[2], points[3]) result_beta = angle_between(coxia, femur) same_beta = np.isclose(np.abs(beta), result_beta, atol=1) assert same_beta, f"{leg_name} leg: expected: |{beta}|, found: {result_beta}" # ❗❗IMPORTANT: Why is sometimes both are zero? femur_tibia_angle = angle_between(femur, tibia) is_90 = np.isclose(90, femur_tibia_angle + gamma, atol=1) if not is_90: alert_msg = f"{leg_name} leg:\ {femur_tibia_angle} (femur-tibia angle) + {gamma} (gamma) != 90." print(alert_msg)
def recompute_hexapod(dimensions, ik_parameters, poses): # make the hexapod with all angles = 0 # update the hexapod so that we know which given points are in contact with the ground old_hexapod = VirtualHexapod(dimensions) old_hexapod.update_stance(ik_parameters["hip_stance"], ik_parameters["leg_stance"]) old_contacts = deepcopy(old_hexapod.ground_contacts) # make a new hexapod with all angles = 0 # and update given the poses/ angles we've computed new_hexapod = VirtualHexapod(dimensions) new_hexapod.update(poses) new_contacts = deepcopy(new_hexapod.ground_contacts) # get two points that are on the ground before and after # updating to the given poses id1, id2 = find_two_same_leg_ids(old_contacts, new_contacts) old_p1 = deepcopy(old_hexapod.legs[id1].ground_contact()) old_p2 = deepcopy(old_hexapod.legs[id2].ground_contact()) new_p1 = deepcopy(new_hexapod.legs[id1].ground_contact()) new_p2 = deepcopy(new_hexapod.legs[id2].ground_contact()) # we must translate and rotate the hexapod with the pose # so that the hexapod is stepping on the old predefined ground contact points old_vector = vector_from_to(old_p1, old_p2) new_vector = vector_from_to(new_p1, new_p2) translate_vector = vector_from_to(new_p1, old_p1) _, twist_frame = find_twist_to_recompute_hexapod(new_vector, old_vector) new_hexapod.rotate_and_shift(twist_frame, 0) twisted_p2 = new_hexapod.legs[id2].foot_tip() translate_vector = vector_from_to(twisted_p2, old_p2) new_hexapod.move_xyz(translate_vector.x, translate_vector.y, 0) might_sanity_check_points(new_p1, new_p2, old_p1, old_p2, new_vector, old_vector) return new_hexapod
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
def store_initial_vectors(self): self.body_to_foot_vector = vector_from_to(self.body_contact, self.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( self.body_to_foot_vector, self.hexapod.z_axis ) self.unit_coxia_vector = get_unit_vector(projection) self.coxia_vector = scalar_multiply(self.unit_coxia_vector, self.hexapod.coxia) # coxia point / joint is the point connecting the coxia and tibia limbs coxia_point = add_vectors(self.body_contact, self.coxia_vector) if coxia_point.z < self.foot_tip.z: raise Exception(COXIA_ON_GROUND_ALERT_MSG)
def compute_beta_gamma_local_p2(self): # These values are needed to compute # p2 aka tibia joint (point between femur limb and tibia limb) self.coxia_to_foot_vector2d = vector_from_to(self.p1, self.p3) self.d = length(self.coxia_to_foot_vector2d) # If we can form this triangle # # this means we probably can reach the target ground contact point if is_triangle(self.hexapod.tibia, self.hexapod.femur, self.d): # CASE A: a triangle can be formed with # coxia to foot vector, hexapod's femur and tibia self.compute_when_triangle_can_form() else: # CASE B: It's impossible to reach target ground point self.compute_when_triangle_cannot_form() not_within_range, alert_msg = beta_gamma_not_in_range( self.beta, self.gamma, self.leg_name) if not_within_range: raise Exception(alert_msg)
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