Twisting about z axis does not account for all cases

[mithi]

Twisting about z axis does not account for all cases

[mithi]

First solution: Hexapod body will only twist when at least three hip angles at a time.

Second solution:

    # hexapod will only definitely NOT twist
    # if only two of the legs (currently on the ground)
    # has twisted its hips/coxia
    # i.e. only 2 legs with ground contact points have changed alpha angle
    # i.e. we don't care if the legs which are not on the ground twisted its hips

Thinking about other cases we should account for as well as other possible algorithms.

[mithi]

Currently implemented like this

hexapod-robot-simulator/hexapod/models.py

Line 222 in f67434e

def _find_if_might_twist(self, poses):

    def _find_if_might_twist(self, poses):
        # hexapod will only definitely NOT twist
        # if only two of the legs (currently on the ground)
        # has twisted its hips/coxia
        # i.e. only 2 legs with ground contact points have changed alpha angle
        # i.e. we don't care if the legs which are not on the ground twisted its hips
        did_change_count = 0

        for leg_point in self.ground_contacts:

            # find the leg id of the ground contact point
            right_or_left, front_mid_or_back, _ = leg_point.name.split("-")
            leg_placement = right_or_left + "-" + front_mid_or_back
            leg_id = self.body.VERTEX_NAMES.index(leg_placement)

            # alpha before new pose
            old_hip_angle = self.legs[leg_id].coxia_angle()

            # new alpha pose
            new_hip_angle = get_hip_angle(leg_id, poses)

            if not np.isclose(old_hip_angle, new_hip_angle or 0):
                did_change_count += 1
                if did_change_count >= 3:
                    return True

        return False

and

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):
        contact_dict = {}
        for leg_point in contact_list:
            name = leg_point.name
            contact_dict[name] = leg_point
        return contact_dict

    def _twist(v1, v2):
        # Note: theta is in radians
        # https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/
        theta = np.arctan2(v2.y, v2.x) - np.arctan2(v1.y, v1.x)

        # frame to rotate around z
        cos_theta = np.cos(theta)
        sin_theta = np.sin(theta)

        return np.array(
            [
                [cos_theta, -sin_theta, 0, 0],
                [sin_theta, cos_theta, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1],
            ]
        )

    # 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.keys():
        if key in new_contacts.keys():
            same_point_name = key
            break

    # We don't know how to rotate if we don't
    # know at least one point that's contacting 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 = Point(old.x, old.y, 0)
    new_vector = Point(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