From 9903c162e3ce9a7c3a4cf40341cdbb9e04115b2b Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Wed, 29 Apr 2020 15:22:40 +0200 Subject: [PATCH] Rename class from Point to Vector (#93) --- hexapod/ground_contact_solver/shared.py | 4 +- hexapod/ik_solver/ik_solver.py | 14 ++--- hexapod/ik_solver/ik_solver2.py | 12 ++--- hexapod/ik_solver/recompute_hexapod.py | 6 +-- hexapod/linkage.py | 8 +-- hexapod/models.py | 30 +++++------ hexapod/points.py | 23 ++++---- tests/helpers.py | 4 +- tests/kinematics_cases/case1.py | 70 ++++++++++++------------- tests/kinematics_cases/case2.py | 70 ++++++++++++------------- tests/pattern_cases/case1.py | 70 ++++++++++++------------- tests/pattern_cases/case2.py | 70 ++++++++++++------------- tests/test_ik.py | 10 ++-- 13 files changed, 196 insertions(+), 195 deletions(-) diff --git a/hexapod/ground_contact_solver/shared.py b/hexapod/ground_contact_solver/shared.py index 489c398..8ce04dd 100644 --- a/hexapod/ground_contact_solver/shared.py +++ b/hexapod/ground_contact_solver/shared.py @@ -1,6 +1,6 @@ from math import isclose from hexapod.points import ( - Point, + Vector, dot, cross, vector_from_to, @@ -48,7 +48,7 @@ def is_stable(p1, p2, p3, tol=0.001): onto the plane defined by p1, p2, p3 is within a triangle defined by p1, p2, p3. """ - p = Point(0, 0, 0) + p = Vector(0, 0, 0) u = vector_from_to(p1, p2) v = vector_from_to(p1, p3) n = cross(u, v) diff --git a/hexapod/ik_solver/ik_solver.py b/hexapod/ik_solver/ik_solver.py index 958f8df..9534711 100644 --- a/hexapod/ik_solver/ik_solver.py +++ b/hexapod/ik_solver/ik_solver.py @@ -18,7 +18,7 @@ might_print_points, ) from hexapod.points import ( - Point, + Vector, length, add_vectors, scalar_multiply, @@ -60,7 +60,7 @@ def inverse_kinematics_update(hexapod, ik_parameters): if body_contact_shoved_on_ground(hexapod): raise Exception(BODY_ON_GROUND_ALERT_MSG) - x_axis = Point(1, 0, 0) + x_axis = Vector(1, 0, 0) legs_up_in_the_air = [] for i in range(hexapod.LEG_COUNT): @@ -83,15 +83,15 @@ def inverse_kinematics_update(hexapod, ik_parameters): # ******************* # 1. Compute p0, p1 and (possible) p3 wrt leg frame # ******************* - p0 = Point(0, 0, 0) - p1 = Point(hexapod.coxia, 0, 0) + 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 = Point(p3x, 0, p3z) + p3 = Vector(p3x, 0, p3z) # ******************* # 2. Compute p2, beta, gamma and final p3 wrt leg frame @@ -119,7 +119,7 @@ def inverse_kinematics_update(hexapod, ik_parameters): z_ = hexapod.femur * np.sin(np.radians(beta)) x_ = p1.x + hexapod.femur * np.cos(np.radians(beta)) - p2 = Point(x_, 0, z_) + 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) @@ -149,7 +149,7 @@ def inverse_kinematics_update(hexapod, ik_parameters): # Find beta and gamma gamma = 0.0 - leg_x_axis = Point(1, 0, 0) + leg_x_axis = Vector(1, 0, 0) beta = angle_between(leg_x_axis, femur_vector) if femur_vector.z < 0: beta = -beta diff --git a/hexapod/ik_solver/ik_solver2.py b/hexapod/ik_solver/ik_solver2.py index cd195ca..68d4cd4 100644 --- a/hexapod/ik_solver/ik_solver2.py +++ b/hexapod/ik_solver/ik_solver2.py @@ -17,7 +17,7 @@ might_print_points, ) from hexapod.points import ( - Point, + Vector, length, add_vectors, scalar_multiply, @@ -85,7 +85,7 @@ class IKSolver: def __init__(self, hexapod, ik_parameters): self.hexapod = hexapod self.params = ik_parameters - self.leg_x_axis = Point(1, 0, 0) + self.leg_x_axis = Vector(1, 0, 0) self.update_body_and_ground_contact_points() self.poses = deepcopy(HEXAPOD_POSE) @@ -149,15 +149,15 @@ def store_initial_vectors(self): raise Exception(COXIA_ON_GROUND_ALERT_MSG) def compute_local_p0_p1_p3(self): - self.p0 = Point(0, 0, 0) - self.p1 = Point(self.hexapod.coxia, 0, 0) + 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 = Point(p3x, 0, p3z) + self.p3 = Vector(p3x, 0, p3z) def compute_beta_gamma_local_p2(self): # These values are needed to compute @@ -194,7 +194,7 @@ def compute_when_triangle_can_form(self): 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_) + 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) diff --git a/hexapod/ik_solver/recompute_hexapod.py b/hexapod/ik_solver/recompute_hexapod.py index c191ea2..99dc6e6 100644 --- a/hexapod/ik_solver/recompute_hexapod.py +++ b/hexapod/ik_solver/recompute_hexapod.py @@ -4,7 +4,7 @@ from hexapod.points import ( angle_between, is_counter_clockwise, - Point, + Vector, rotz, vector_from_to, length, @@ -91,7 +91,7 @@ def find_two_same_leg_ids(old_contacts, new_contacts): def find_twist_to_recompute_hexapod(a, b): twist = angle_between(a, b) - z_axis = Point(0, 0, -1) + z_axis = Vector(0, 0, -1) is_ccw = is_counter_clockwise(a, b, z_axis) if is_ccw: twist = -twist @@ -101,7 +101,7 @@ def find_twist_to_recompute_hexapod(a, b): def should_be_on_ground_msg(point): - return f"Point should be on the ground:\n{point}, z != 0" + return f"Vector should be on the ground:\n{point}, z != 0" def might_sanity_check_points(new_p1, new_p2, old_p1, old_p2, new_vector, old_vector): diff --git a/hexapod/linkage.py b/hexapod/linkage.py index 4c86532..bb48b46 100644 --- a/hexapod/linkage.py +++ b/hexapod/linkage.py @@ -59,7 +59,7 @@ from copy import deepcopy import numpy as np from hexapod.points import ( - Point, + Vector, frame_yrotate_xtranslate, frame_zrotate_xytranslate, ) @@ -92,7 +92,7 @@ def __init__( beta=0, gamma=0, coxia_axis=0, - new_origin=Point(0, 0, 0), + new_origin=Vector(0, 0, 0), name=None, id_number=None, ): @@ -143,7 +143,7 @@ def change_pose(self, alpha, beta, gamma): ) # find points wrt to body contact point - p0 = Point(0, 0, 0) + 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) @@ -174,7 +174,7 @@ def compute_ground_contact(self): def __str__(self): leg_string = f"{self!r}\n" - leg_string += f"Points of {self.name} leg:\n" + leg_string += f"Vectors of {self.name} leg:\n" for point in self.all_points: leg_string += f" {point}\n" diff --git a/hexapod/models.py b/hexapod/models.py index 9f45ca9..27cb4d0 100644 --- a/hexapod/models.py +++ b/hexapod/models.py @@ -12,7 +12,7 @@ from hexapod.templates.pose_template import HEXAPOD_POSE from hexapod.points import ( - Point, + Vector, frame_to_align_vector_a_to_b, frame_rotxyz, rotz, @@ -75,15 +75,15 @@ def __init__(self, f, m, s): self.m = m self.s = s - self.cog = Point(0, 0, 0, name="center-of-gravity") - self.head = Point(0, s, 0, name="head") + self.cog = Vector(0, 0, 0, name="center-of-gravity") + self.head = Vector(0, s, 0, name="head") self.vertices = [ - Point(m, 0, 0, name=Hexagon.VERTEX_NAMES[0]), - Point(f, s, 0, name=Hexagon.VERTEX_NAMES[1]), - Point(-f, s, 0, name=Hexagon.VERTEX_NAMES[2]), - Point(-m, 0, 0, name=Hexagon.VERTEX_NAMES[3]), - Point(-f, -s, 0, name=Hexagon.VERTEX_NAMES[4]), - Point(f, -s, 0, name=Hexagon.VERTEX_NAMES[5]), + 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] @@ -139,7 +139,7 @@ def update(self, poses, assume_ground_targets=True): 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, Point(0, 0, 1)) + 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) @@ -225,9 +225,9 @@ def rotate_and_shift(self, frame, height=0): leg.update_leg_wrt(frame, height) def _init_local_frame(self): - self.x_axis = Point(1, 0, 0, name="hexapod x axis") - self.y_axis = Point(0, 1, 0, name="hexapod y axis") - self.z_axis = Point(0, 0, 1, name="hexapod z axis") + 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 @@ -310,8 +310,8 @@ def _twist(v1, v2): new = new_contacts[same_point_name] # Get the projection of these points in the ground - old_vector = Point(old.x, old.y, 0) - new_vector = Point(new.x, new.y, 0) + old_vector = Vector(old.x, old.y, 0) + new_vector = Vector(new.x, new.y, 0) twist_frame = _twist(new_vector, old_vector) diff --git a/hexapod/points.py b/hexapod/points.py index 0af8088..737cb58 100644 --- a/hexapod/points.py +++ b/hexapod/points.py @@ -1,4 +1,4 @@ -# This module contains the Point class +# This module contains the Vector class # and functions for manipulating vectors # and finding properties and relationships of vectors # computing reference frames @@ -8,7 +8,7 @@ from settings import DEBUG_MODE -class Point: +class Vector: __slots__ = ("x", "y", "z", "name") def __init__(self, x, y, z, name=None): @@ -25,7 +25,7 @@ def get_point_wrt(self, reference_frame, name=None): """ p = np.array([self.x, self.y, self.z, 1]) p = np.matmul(reference_frame, p) - return Point(p[0], p[1], p[2], name) + return Vector(p[0], p[1], p[2], name) def update_point_wrt(self, reference_frame, z=0): p = np.array([self.x, self.y, self.z, 1]) @@ -47,13 +47,14 @@ def vec(self): return self.x, self.y, self.z def __repr__(self): - return f"Point(x={self.x:>+8.2f}, y={self.y:>+8.2f}, z={self.z:>+8.2f}, name='{self.name}')" + s = "Vector(x={self.x:>+8.2f}, y={self.y:>+8.2f}, z={self.z:>+8.2f}, name='{self.name}')" + return s def __str__(self): return repr(self) def __eq__(self, other, percent_tol=0.0075): - if not isinstance(other, Point): + if not isinstance(other, Vector): return False tol = length(self) * percent_tol @@ -197,11 +198,11 @@ def _return_sin_and_cos(theta): # get vector pointing from point a to point b def vector_from_to(a, b): - return Point(b.x - a.x, b.y - a.y, b.z - a.z) + return Vector(b.x - a.x, b.y - a.y, b.z - a.z) def scale(v, d): - return Point(v.x / d, v.y / d, v.z / d) + return Vector(v.x / d, v.y / d, v.z / d) def dot(a, b): @@ -212,7 +213,7 @@ def cross(a, b): x = a.y * b.z - a.z * b.y y = a.z * b.x - a.x * b.z z = a.x * b.y - a.y * b.x - return Point(x, y, z) + return Vector(x, y, z) def length(v): @@ -220,15 +221,15 @@ def length(v): def add_vectors(a, b): - return Point(a.x + b.x, a.y + b.y, a.z + b.z) + return Vector(a.x + b.x, a.y + b.y, a.z + b.z) def subtract_vectors(a, b): - return Point(a.x - b.x, a.y - b.y, a.z - b.z) + return Vector(a.x - b.x, a.y - b.y, a.z - b.z) def scalar_multiply(p, s): - return Point(s * p.x, s * p.y, s * p.z) + return Vector(s * p.x, s * p.y, s * p.z) def get_unit_vector(v): diff --git a/tests/helpers.py b/tests/helpers.py index 82fe147..87ba2da 100644 --- a/tests/helpers.py +++ b/tests/helpers.py @@ -15,7 +15,7 @@ def assert_hexapod_points_equal( hexapod, correct_body_points, correct_leg_points, description ): def msg(a, b): - return f"Unequal Points\nexpected: {a}\n....found: {b}\n(case: {description})" + return f"Unequal Vectors\nexpected: {a}\n....found: {b}\n(case: {description})" for point_a, point_b in zip(correct_body_points, hexapod.body.all_points): assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b) @@ -27,7 +27,7 @@ def msg(a, b): def assert_two_hexapods_equal(hexapod1, hexapod2, description): def msg(a, b): - return f"Unequal Points\n1: {a}\n2:{b}\n(case: {description})" + return f"Unequal Vectors\n1: {a}\n2:{b}\n(case: {description})" for point_a, point_b in zip(hexapod1.body.all_points, hexapod2.body.all_points): assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b) diff --git a/tests/kinematics_cases/case1.py b/tests/kinematics_cases/case1.py index 48a0abc..23f5a27 100644 --- a/tests/kinematics_cases/case1.py +++ b/tests/kinematics_cases/case1.py @@ -1,4 +1,4 @@ -from hexapod.points import Point +from hexapod.points import Vector description = "Kinematics Random Pose #1" @@ -30,64 +30,64 @@ # ******************************** -# Correct Body Points +# Correct Body Vectors # ******************************** correct_body_points = [ - Point(x=+97.74, y=+69.20, z=+123.97, name="right-middle"), - Point(x=-3.68, y=+111.78, z=+103.96, name="right-front"), - Point(x=-120.97, y=+28.74, z=+146.94, name="left-front"), - Point(x=-97.74, y=-69.20, z=+195.60, name="left-middle"), - Point(x=+3.68, y=-111.78, z=+215.60, name="left-back"), - Point(x=+120.97, y=-28.74, z=+172.63, name="right-back"), - Point(x=+0.00, y=+0.00, z=+159.78, name="center-of-gravity"), - Point(x=-62.33, y=+70.26, z=+125.45, name="head"), + 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 Points +# Leg Vectors # ******************************** leg0_points = [ - Point(x=+97.74, y=+69.20, z=+123.97, name="right-middle-body-contact"), - Point(x=+147.72, y=+67.82, z=+124.03, name="right-middle-coxia"), - Point(x=+271.07, y=+83.36, z=+162.03, name="right-middle-femur"), - Point(x=+353.52, y=-0.00, z=+0.00, name="right-middle-tibia"), + Vector(x=+97.74, y=+69.20, z=+123.97, name="right-middle-body-contact"), + Vector(x=+147.72, y=+67.82, z=+124.03, name="right-middle-coxia"), + Vector(x=+271.07, y=+83.36, z=+162.03, name="right-middle-femur"), + Vector(x=+353.52, y=-0.00, z=+0.00, name="right-middle-tibia"), ] leg1_points = [ - Point(x=-3.68, y=+111.78, z=+103.96, name="right-front-body-contact"), - Point(x=-26.03, y=+151.90, z=+84.19, name="right-front-coxia"), - Point(x=-29.64, y=+218.89, z=+195.55, name="right-front-femur"), - Point(x=-69.47, y=+205.68, z=+0.00, name="right-front-tibia"), + Vector(x=-3.68, y=+111.78, z=+103.96, name="right-front-body-contact"), + Vector(x=-26.03, y=+151.90, z=+84.19, name="right-front-coxia"), + Vector(x=-29.64, y=+218.89, z=+195.55, name="right-front-femur"), + Vector(x=-69.47, y=+205.68, z=+0.00, name="right-front-tibia"), ] leg2_points = [ - Point(x=-120.97, y=+28.74, z=+146.94, name="left-front-body-contact"), - Point(x=-165.74, y=+48.88, z=+137.44, name="left-front-coxia"), - Point(x=-164.27, y=+107.00, z=+253.72, name="left-front-femur"), - Point(x=-339.26, y=+165.39, z=+176.44, name="left-front-tibia"), + Vector(x=-120.97, y=+28.74, z=+146.94, name="left-front-body-contact"), + Vector(x=-165.74, y=+48.88, z=+137.44, name="left-front-coxia"), + Vector(x=-164.27, y=+107.00, z=+253.72, name="left-front-femur"), + Vector(x=-339.26, y=+165.39, z=+176.44, name="left-front-tibia"), ] leg3_points = [ - Point(x=-97.74, y=-69.20, z=+195.60, name="left-middle-body-contact"), - Point(x=-142.46, y=-88.97, z=+206.04, name="left-middle-coxia"), - Point(x=-248.46, y=-160.12, z=+181.51, name="left-middle-femur"), - Point(x=-183.54, y=-213.39, z=+0.00, name="left-middle-tibia"), + Vector(x=-97.74, y=-69.20, z=+195.60, name="left-middle-body-contact"), + Vector(x=-142.46, y=-88.97, z=+206.04, name="left-middle-coxia"), + Vector(x=-248.46, y=-160.12, z=+181.51, name="left-middle-femur"), + Vector(x=-183.54, y=-213.39, z=+0.00, name="left-middle-tibia"), ] leg4_points = [ - Point(x=+3.68, y=-111.78, z=+215.60, name="left-back-body-contact"), - Point(x=-1.93, y=-156.20, z=+237.87, name="left-back-coxia"), - Point(x=+0.55, y=-90.17, z=+349.83, name="left-back-femur"), - Point(x=-10.63, y=-244.11, z=+222.63, name="left-back-tibia"), + Vector(x=+3.68, y=-111.78, z=+215.60, name="left-back-body-contact"), + Vector(x=-1.93, y=-156.20, z=+237.87, name="left-back-coxia"), + Vector(x=+0.55, y=-90.17, z=+349.83, name="left-back-femur"), + Vector(x=-10.63, y=-244.11, z=+222.63, name="left-back-tibia"), ] leg5_points = [ - Point(x=+120.97, y=-28.74, z=+172.63, name="right-back-body-contact"), - Point(x=+169.97, y=-37.86, z=+176.57, name="right-back-coxia"), - Point(x=+292.24, y=-43.54, z=+220.36, name="right-back-femur"), - Point(x=+353.93, y=-139.96, z=+56.35, name="right-back-tibia"), + Vector(x=+120.97, y=-28.74, z=+172.63, name="right-back-body-contact"), + Vector(x=+169.97, y=-37.86, z=+176.57, name="right-back-coxia"), + Vector(x=+292.24, y=-43.54, z=+220.36, name="right-back-femur"), + Vector(x=+353.93, y=-139.96, z=+56.35, name="right-back-tibia"), ] correct_leg_points = [ diff --git a/tests/kinematics_cases/case2.py b/tests/kinematics_cases/case2.py index 9530cd9..98cbce1 100644 --- a/tests/kinematics_cases/case2.py +++ b/tests/kinematics_cases/case2.py @@ -1,4 +1,4 @@ -from hexapod.points import Point +from hexapod.points import Vector description = "Kinematics Random Pose #2" @@ -65,66 +65,66 @@ } # ******************************** -# Correct Body Points +# Correct Body Vectors # ******************************** correct_body_points = [ - Point(x=+112.68, y=+45.33, z=+126.66, name="right-middle"), - Point(x=+5.57, y=+122.87, z=+116.68, name="right-front"), - Point(x=-90.76, y=+84.12, z=+95.32, name="left-front"), - Point(x=-112.68, y=-45.33, z=+76.69, name="left-middle"), - Point(x=-5.57, y=-122.87, z=+86.67, name="left-back"), - Point(x=+90.76, y=-84.12, z=+108.03, name="right-back"), - Point(x=+0.00, y=+0.00, z=+101.67, name="center-of-gravity"), - Point(x=-42.60, y=+103.49, z=+106.00, name="head"), + 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 Points +# Leg Vectors # ******************************** leg0_points = [ - Point(x=+112.68, y=+45.33, z=+126.66, name="right-middle-body-contact"), - Point(x=+171.42, y=+18.46, z=+133.92, name="right-middle-coxia"), - Point(x=+304.49, y=-42.16, z=+148.91, name="right-middle-femur"), - Point(x=+272.70, y=+0.00, z=-0.00, name="right-middle-tibia"), + Vector(x=+112.68, y=+45.33, z=+126.66, name="right-middle-body-contact"), + Vector(x=+171.42, y=+18.46, z=+133.92, name="right-middle-coxia"), + Vector(x=+304.49, y=-42.16, z=+148.91, name="right-middle-femur"), + Vector(x=+272.70, y=+0.00, z=-0.00, name="right-middle-tibia"), ] leg1_points = [ - Point(x=+5.57, y=+122.87, z=+116.68, name="right-front-body-contact"), - Point(x=+61.49, y=+153.21, z=+129.97, name="right-front-coxia"), - Point(x=+189.21, y=+222.64, z=+151.78, name="right-front-femur"), - Point(x=+149.60, y=+203.72, z=-0.00, name="right-front-tibia"), + Vector(x=+5.57, y=+122.87, z=+116.68, name="right-front-body-contact"), + Vector(x=+61.49, y=+153.21, z=+129.97, name="right-front-coxia"), + Vector(x=+189.21, y=+222.64, z=+151.78, name="right-front-femur"), + Vector(x=+149.60, y=+203.72, z=-0.00, name="right-front-tibia"), ] leg2_points = [ - Point(x=-90.76, y=+84.12, z=+95.32, name="left-front-body-contact"), - Point(x=-150.83, y=+107.65, z=+87.44, name="left-front-coxia"), - Point(x=-276.55, y=+141.74, z=+155.58, name="left-front-femur"), - Point(x=-259.37, y=+163.25, z=+0.00, name="left-front-tibia"), + Vector(x=-90.76, y=+84.12, z=+95.32, name="left-front-body-contact"), + Vector(x=-150.83, y=+107.65, z=+87.44, name="left-front-coxia"), + Vector(x=-276.55, y=+141.74, z=+155.58, name="left-front-femur"), + Vector(x=-259.37, y=+163.25, z=+0.00, name="left-front-tibia"), ] leg3_points = [ - Point(x=-112.68, y=-45.33, z=+76.69, name="left-middle-body-contact"), - Point(x=-176.39, y=-50.56, z=+64.89, name="left-middle-coxia"), - Point(x=-293.99, y=-70.60, z=+150.78, name="left-middle-femur"), - Point(x=-340.16, y=-60.64, z=-0.00, name="left-middle-tibia"), + Vector(x=-112.68, y=-45.33, z=+76.69, name="left-middle-body-contact"), + Vector(x=-176.39, y=-50.56, z=+64.89, name="left-middle-coxia"), + Vector(x=-293.99, y=-70.60, z=+150.78, name="left-middle-femur"), + Vector(x=-340.16, y=-60.64, z=-0.00, name="left-middle-tibia"), ] leg4_points = [ - Point(x=-5.57, y=-122.87, z=+86.67, name="left-back-body-contact"), - Point(x=-58.45, y=-158.23, z=+73.33, name="left-back-coxia"), - Point(x=-165.26, y=-229.31, z=+145.09, name="left-back-femur"), - Point(x=-217.06, y=-264.36, z=+0.00, name="left-back-tibia"), + Vector(x=-5.57, y=-122.87, z=+86.67, name="left-back-body-contact"), + Vector(x=-58.45, y=-158.23, z=+73.33, name="left-back-coxia"), + Vector(x=-165.26, y=-229.31, z=+145.09, name="left-back-femur"), + Vector(x=-217.06, y=-264.36, z=+0.00, name="left-back-tibia"), ] leg5_points = [ - Point(x=+90.76, y=-84.12, z=+108.03, name="right-back-body-contact"), - Point(x=+121.84, y=-141.20, z=+106.97, name="right-back-coxia"), - Point(x=+180.23, y=-268.69, z=+151.07, name="right-back-femur"), - Point(x=+191.91, y=-223.89, z=+0.00, name="right-back-tibia"), + Vector(x=+90.76, y=-84.12, z=+108.03, name="right-back-body-contact"), + Vector(x=+121.84, y=-141.20, z=+106.97, name="right-back-coxia"), + Vector(x=+180.23, y=-268.69, z=+151.07, name="right-back-femur"), + Vector(x=+191.91, y=-223.89, z=+0.00, name="right-back-tibia"), ] diff --git a/tests/pattern_cases/case1.py b/tests/pattern_cases/case1.py index 1d3d4e1..4861cd0 100644 --- a/tests/pattern_cases/case1.py +++ b/tests/pattern_cases/case1.py @@ -1,4 +1,4 @@ -from hexapod.points import Point +from hexapod.points import Vector description = "Patterns Random Pose #1" alpha = 42 @@ -19,65 +19,65 @@ } # ******************************** -# Correct Body Points +# Correct Body Vectors # ******************************** correct_body_points = [ - Point(x=+123.00, y=+0.00, z=+0.00, name="right-middle"), - Point(x=+76.00, y=+92.00, z=+0.00, name="right-front"), - Point(x=-76.00, y=+92.00, z=+0.00, name="left-front"), - Point(x=-123.00, y=+0.00, z=+0.00, name="left-middle"), - Point(x=-76.00, y=-92.00, z=+0.00, name="left-back"), - Point(x=+76.00, y=-92.00, z=+0.00, name="right-back"), - Point(x=+0.00, y=+0.00, z=+0.00, name="center-of-gravity"), - Point(x=+0.00, y=+92.00, z=+0.00, name="head"), + 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 Points +# Correct Leg Vectors # ******************************** leg0_points = [ - Point(x=+123.00, y=+0.00, z=+0.00, name="right-middle-body-contact"), - Point(x=+166.10, y=+38.81, z=+0.00, name="right-middle-coxia"), - Point(x=+219.60, y=+86.98, z=+161.70, name="right-middle-femur"), - Point(x=+278.24, y=+139.77, z=+32.95, name="right-middle-tibia"), + Vector(x=+123.00, y=+0.00, z=+0.00, name="right-middle-body-contact"), + Vector(x=+166.10, y=+38.81, z=+0.00, name="right-middle-coxia"), + Vector(x=+219.60, y=+86.98, z=+161.70, name="right-middle-femur"), + Vector(x=+278.24, y=+139.77, z=+32.95, name="right-middle-tibia"), ] leg1_points = [ - Point(x=+76.00, y=+92.00, z=+0.00, name="right-front-body-contact"), - Point(x=+79.04, y=+149.92, z=+0.00, name="right-front-coxia"), - Point(x=+82.80, y=+221.81, z=+161.70, name="right-front-femur"), - Point(x=+86.93, y=+300.60, z=+32.95, name="right-front-tibia"), + Vector(x=+76.00, y=+92.00, z=+0.00, name="right-front-body-contact"), + Vector(x=+79.04, y=+149.92, z=+0.00, name="right-front-coxia"), + Vector(x=+82.80, y=+221.81, z=+161.70, name="right-front-femur"), + Vector(x=+86.93, y=+300.60, z=+32.95, name="right-front-tibia"), ] leg2_points = [ - Point(x=-76.00, y=+92.00, z=+0.00, name="left-front-body-contact"), - Point(x=-133.92, y=+95.04, z=+0.00, name="left-front-coxia"), - Point(x=-205.81, y=+98.80, z=+161.70, name="left-front-femur"), - Point(x=-284.60, y=+102.93, z=+32.95, name="left-front-tibia"), + Vector(x=-76.00, y=+92.00, z=+0.00, name="left-front-body-contact"), + Vector(x=-133.92, y=+95.04, z=+0.00, name="left-front-coxia"), + Vector(x=-205.81, y=+98.80, z=+161.70, name="left-front-femur"), + Vector(x=-284.60, y=+102.93, z=+32.95, name="left-front-tibia"), ] leg3_points = [ - Point(x=-123.00, y=+0.00, z=+0.00, name="left-middle-body-contact"), - Point(x=-166.10, y=-38.81, z=+0.00, name="left-middle-coxia"), - Point(x=-219.60, y=-86.98, z=+161.70, name="left-middle-femur"), - Point(x=-278.24, y=-139.77, z=+32.95, name="left-middle-tibia"), + Vector(x=-123.00, y=+0.00, z=+0.00, name="left-middle-body-contact"), + Vector(x=-166.10, y=-38.81, z=+0.00, name="left-middle-coxia"), + Vector(x=-219.60, y=-86.98, z=+161.70, name="left-middle-femur"), + Vector(x=-278.24, y=-139.77, z=+32.95, name="left-middle-tibia"), ] leg4_points = [ - Point(x=-76.00, y=-92.00, z=+0.00, name="left-back-body-contact"), - Point(x=-79.04, y=-149.92, z=+0.00, name="left-back-coxia"), - Point(x=-82.80, y=-221.81, z=+161.70, name="left-back-femur"), - Point(x=-86.93, y=-300.60, z=+32.95, name="left-back-tibia"), + Vector(x=-76.00, y=-92.00, z=+0.00, name="left-back-body-contact"), + Vector(x=-79.04, y=-149.92, z=+0.00, name="left-back-coxia"), + Vector(x=-82.80, y=-221.81, z=+161.70, name="left-back-femur"), + Vector(x=-86.93, y=-300.60, z=+32.95, name="left-back-tibia"), ] leg5_points = [ - Point(x=+76.00, y=-92.00, z=+0.00, name="right-back-body-contact"), - Point(x=+133.92, y=-95.04, z=+0.00, name="right-back-coxia"), - Point(x=+205.81, y=-98.80, z=+161.70, name="right-back-femur"), - Point(x=+284.60, y=-102.93, z=+32.95, name="right-back-tibia"), + Vector(x=+76.00, y=-92.00, z=+0.00, name="right-back-body-contact"), + Vector(x=+133.92, y=-95.04, z=+0.00, name="right-back-coxia"), + Vector(x=+205.81, y=-98.80, z=+161.70, name="right-back-femur"), + Vector(x=+284.60, y=-102.93, z=+32.95, name="right-back-tibia"), ] diff --git a/tests/pattern_cases/case2.py b/tests/pattern_cases/case2.py index b5c70f4..f576b94 100644 --- a/tests/pattern_cases/case2.py +++ b/tests/pattern_cases/case2.py @@ -1,4 +1,4 @@ -from hexapod.points import Point +from hexapod.points import Vector description = "Patterns Random Pose #2" alpha = -28.5 @@ -19,64 +19,64 @@ } # ******************************** -# Correct Body Points +# Correct Body Vectors # ******************************** correct_body_points = [ - Point(x=+116.86, y=+35.03, z=+65.62, name="right-middle"), - Point(x=+33.27, y=+115.41, z=+65.62, name="right-front"), - Point(x=-91.26, y=+78.09, z=+65.62, name="left-front"), - Point(x=-116.86, y=-35.03, z=+65.62, name="left-middle"), - Point(x=-33.27, y=-115.41, z=+65.62, name="left-back"), - Point(x=+91.26, y=-78.09, z=+65.62, name="right-back"), - Point(x=+0.00, y=+0.00, z=+65.62, name="center-of-gravity"), - Point(x=-29.00, y=+96.75, z=+65.62, name="head"), + 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 Points +# Correct Leg Vectors # ******************************** leg0_points = [ - Point(x=+116.86, y=+35.03, z=+65.62, name="right-middle-body-contact"), - Point(x=+198.11, y=+18.03, z=+65.62, name="right-middle-coxia"), - Point(x=+230.77, y=+11.20, z=+168.34, name="right-middle-femur"), - Point(x=+284.31, y=+0.00, z=+0.00, name="right-middle-tibia"), + Vector(x=+116.86, y=+35.03, z=+65.62, name="right-middle-body-contact"), + Vector(x=+198.11, y=+18.03, z=+65.62, name="right-middle-coxia"), + Vector(x=+230.77, y=+11.20, z=+168.34, name="right-middle-femur"), + Vector(x=+284.31, y=+0.00, z=+0.00, name="right-middle-tibia"), ] leg1_points = [ - Point(x=+33.27, y=+115.41, z=+65.62, name="right-front-body-contact"), - Point(x=+102.73, y=+160.84, z=+65.62, name="right-front-coxia"), - Point(x=+130.66, y=+179.11, z=+168.34, name="right-front-femur"), - Point(x=+176.44, y=+209.04, z=+0.00, name="right-front-tibia"), + Vector(x=+33.27, y=+115.41, z=+65.62, name="right-front-body-contact"), + Vector(x=+102.73, y=+160.84, z=+65.62, name="right-front-coxia"), + Vector(x=+130.66, y=+179.11, z=+168.34, name="right-front-femur"), + Vector(x=+176.44, y=+209.04, z=+0.00, name="right-front-tibia"), ] leg2_points = [ - Point(x=-91.26, y=+78.09, z=+65.62, name="left-front-body-contact"), - Point(x=-136.69, y=+147.55, z=+65.62, name="left-front-coxia"), - Point(x=-154.96, y=+175.48, z=+168.34, name="left-front-femur"), - Point(x=-184.90, y=+221.26, z=+0.00, name="left-front-tibia"), + Vector(x=-91.26, y=+78.09, z=+65.62, name="left-front-body-contact"), + Vector(x=-136.69, y=+147.55, z=+65.62, name="left-front-coxia"), + Vector(x=-154.96, y=+175.48, z=+168.34, name="left-front-femur"), + Vector(x=-184.90, y=+221.26, z=+0.00, name="left-front-tibia"), ] leg3_points = [ - Point(x=-116.86, y=-35.03, z=+65.62, name="left-middle-body-contact"), - Point(x=-198.11, y=-18.03, z=+65.62, name="left-middle-coxia"), - Point(x=-230.77, y=-11.20, z=+168.34, name="left-middle-femur"), - Point(x=-284.31, y=+0.00, z=+0.00, name="left-middle-tibia"), + Vector(x=-116.86, y=-35.03, z=+65.62, name="left-middle-body-contact"), + Vector(x=-198.11, y=-18.03, z=+65.62, name="left-middle-coxia"), + Vector(x=-230.77, y=-11.20, z=+168.34, name="left-middle-femur"), + Vector(x=-284.31, y=+0.00, z=+0.00, name="left-middle-tibia"), ] leg4_points = [ - Point(x=-33.27, y=-115.41, z=+65.62, name="left-back-body-contact"), - Point(x=-102.73, y=-160.84, z=+65.62, name="left-back-coxia"), - Point(x=-130.66, y=-179.11, z=+168.34, name="left-back-femur"), - Point(x=-176.44, y=-209.04, z=+0.00, name="left-back-tibia"), + Vector(x=-33.27, y=-115.41, z=+65.62, name="left-back-body-contact"), + Vector(x=-102.73, y=-160.84, z=+65.62, name="left-back-coxia"), + Vector(x=-130.66, y=-179.11, z=+168.34, name="left-back-femur"), + Vector(x=-176.44, y=-209.04, z=+0.00, name="left-back-tibia"), ] leg5_points = [ - Point(x=+91.26, y=-78.09, z=+65.62, name="right-back-body-contact"), - Point(x=+136.69, y=-147.55, z=+65.62, name="right-back-coxia"), - Point(x=+154.96, y=-175.48, z=+168.34, name="right-back-femur"), - Point(x=+184.90, y=-221.26, z=+0.00, name="right-back-tibia"), + Vector(x=+91.26, y=-78.09, z=+65.62, name="right-back-body-contact"), + Vector(x=+136.69, y=-147.55, z=+65.62, name="right-back-coxia"), + Vector(x=+154.96, y=-175.48, z=+168.34, name="right-back-femur"), + Vector(x=+184.90, y=-221.26, z=+0.00, name="right-back-tibia"), ] correct_leg_points = [ diff --git a/tests/test_ik.py b/tests/test_ik.py index 13460be..604a6d0 100644 --- a/tests/test_ik.py +++ b/tests/test_ik.py @@ -1,7 +1,7 @@ from copy import deepcopy from hexapod.const import BASE_DIMENSIONS from hexapod.models import VirtualHexapod -from hexapod.points import Point +from hexapod.points import Vector from hexapod.ik_solver import ik_solver, ik_solver2 from hexapod.ik_solver.shared import update_hexapod_points @@ -50,10 +50,10 @@ def test_points_ik2_dont_assume_ground_targets(): def test_shared_set_points(): points = [ - Point(1, 2, 3, "a"), - Point(1, 2, 3, "b"), - Point(1, 2, 3, "c"), - Point(1, 2, 3, "d"), + Vector(1, 2, 3, "a"), + Vector(1, 2, 3, "b"), + Vector(1, 2, 3, "c"), + Vector(1, 2, 3, "d"), ] vh = VirtualHexapod(BASE_DIMENSIONS)