🦴 Adds Simulator from OpenQuadruped/spot_mini_mini

2024-03-04 15:55:45 +01:00
parent e673a50fa2
commit 1753e539db
162 changed files with 11940 additions and 1 deletions
@@ -0,0 +1,224 @@
+#!/usr/bin/env python
+
+import numpy as np
+from Kinematics.LegKinematics import LegIK
+from Kinematics.LieAlgebra import RpToTrans, TransToRp, TransInv, RPY, TransformVector
+from collections import OrderedDict
+
+
+class SpotModel:
+    def __init__(
+        self,
+        shoulder_length=0.055,
+        elbow_length=0.10652,
+        wrist_length=0.145,
+        hip_x=0.23,
+        hip_y=0.075,
+        foot_x=0.23,
+        foot_y=0.185,
+        height=0.20,
+        com_offset=0.016,
+        shoulder_lim=[-0.548, 0.548],
+        elbow_lim=[-2.17, 0.97],
+        wrist_lim=[-0.1, 2.59],
+    ):
+        """
+        Spot Micro Kinematics
+        """
+        # COM offset in x direction
+        self.com_offset = com_offset
+
+        # Leg Parameters
+        self.shoulder_length = shoulder_length
+        self.elbow_length = elbow_length
+        self.wrist_length = wrist_length
+
+        # Leg Vector desired_positions
+
+        # Distance Between Hips
+        # Length
+        self.hip_x = hip_x
+        # Width
+        self.hip_y = hip_y
+
+        # Distance Between Feet
+        # Length
+        self.foot_x = foot_x
+        # Width
+        self.foot_y = foot_y
+
+        # Body Height
+        self.height = height
+
+        # Joint Parameters
+        self.shoulder_lim = shoulder_lim
+        self.elbow_lim = elbow_lim
+        self.wrist_lim = wrist_lim
+
+        # Dictionary to store Leg IK Solvers
+        self.Legs = OrderedDict()
+        self.Legs["FL"] = LegIK(
+            "LEFT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+        self.Legs["FR"] = LegIK(
+            "RIGHT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+        self.Legs["BL"] = LegIK(
+            "LEFT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+        self.Legs["BR"] = LegIK(
+            "RIGHT",
+            self.shoulder_length,
+            self.elbow_length,
+            self.wrist_length,
+            self.shoulder_lim,
+            self.elbow_lim,
+            self.wrist_lim,
+        )
+
+        # Dictionary to store Hip and Foot Transforms
+
+        # Transform of Hip relative to world frame
+        # With Body Centroid also in world frame
+        Rwb = np.eye(3)
+        self.WorldToHip = OrderedDict()
+
+        self.ph_FL = np.array([self.hip_x / 2.0, self.hip_y / 2.0, 0])
+        self.WorldToHip["FL"] = RpToTrans(Rwb, self.ph_FL)
+
+        self.ph_FR = np.array([self.hip_x / 2.0, -self.hip_y / 2.0, 0])
+        self.WorldToHip["FR"] = RpToTrans(Rwb, self.ph_FR)
+
+        self.ph_BL = np.array([-self.hip_x / 2.0, self.hip_y / 2.0, 0])
+        self.WorldToHip["BL"] = RpToTrans(Rwb, self.ph_BL)
+
+        self.ph_BR = np.array([-self.hip_x / 2.0, -self.hip_y / 2.0, 0])
+        self.WorldToHip["BR"] = RpToTrans(Rwb, self.ph_BR)
+
+        # Transform of Foot relative to world frame
+        # With Body Centroid also in world frame
+        self.WorldToFoot = OrderedDict()
+
+        self.pf_FL = np.array([self.foot_x / 2.0, self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["FL"] = RpToTrans(Rwb, self.pf_FL)
+
+        self.pf_FR = np.array([self.foot_x / 2.0, -self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["FR"] = RpToTrans(Rwb, self.pf_FR)
+
+        self.pf_BL = np.array([-self.foot_x / 2.0, self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["BL"] = RpToTrans(Rwb, self.pf_BL)
+
+        self.pf_BR = np.array([-self.foot_x / 2.0, -self.foot_y / 2.0, -self.height])
+        self.WorldToFoot["BR"] = RpToTrans(Rwb, self.pf_BR)
+
+    def HipToFoot(self, orn, pos, T_bf):
+        """
+        Converts a desired position and orientation wrt Spot's
+        home position, with a desired body-to-foot Transform
+        into a body-to-hip Transform, which is used to extract
+        and return the Hip To Foot Vector.
+
+        :param orn: A 3x1 np.array([]) with Spot's Roll, Pitch, Yaw angles
+        :param pos: A 3x1 np.array([]) with Spot's X, Y, Z coordinates
+        :param T_bf: Dictionary of desired body-to-foot Transforms.
+        :return: Hip To Foot Vector for each of Spot's Legs.
+        """
+
+        # Following steps in attached document: SpotBodyIK.
+        # TODO: LINK DOC
+
+        # Only get Rot component
+        Rb, _ = TransToRp(RPY(orn[0], orn[1], orn[2]))
+        pb = pos
+        T_wb = RpToTrans(Rb, pb)
+
+        # Dictionary to store vectors
+        HipToFoot_List = OrderedDict()
+
+        for i, (key, T_wh) in enumerate(self.WorldToHip.items()):
+            # ORDER: FL, FR, FR, BL, BR
+
+            # Extract vector component
+            _, p_bf = TransToRp(T_bf[key])
+
+            # Step 1, get T_bh for each leg
+            T_bh = np.dot(TransInv(T_wb), T_wh)
+
+            # Step 2, get T_hf for each leg
+
+            # VECTOR ADDITION METHOD
+            _, p_bh = TransToRp(T_bh)
+            p_hf0 = p_bf - p_bh
+
+            # TRANSFORM METHOD
+            T_hf = np.dot(TransInv(T_bh), T_bf[key])
+            _, p_hf1 = TransToRp(T_hf)
+
+            # They should yield the same result
+            if p_hf1.all() != p_hf0.all():
+                print("NOT EQUAL")
+
+            p_hf = p_hf1
+
+            HipToFoot_List[key] = p_hf
+
+        return HipToFoot_List
+
+    def IK(self, orn, pos, T_bf):
+        """
+        Uses HipToFoot() to convert a desired position
+        and orientation wrt Spot's home position into a
+        Hip To Foot Vector, which is fed into the LegIK solver.
+
+        Finally, the resultant joint angles are returned
+        from the LegIK solver for each leg.
+
+        :param orn: A 3x1 np.array([]) with Spot's Roll, Pitch, Yaw angles
+        :param pos: A 3x1 np.array([]) with Spot's X, Y, Z coordinates
+        :param T_bf: Dictionary of desired body-to-foot Transforms.
+        :return: Joint angles for each of Spot's joints.
+        """
+
+        # Following steps in attached document: SpotBodyIK.
+        # TODO: LINK DOC
+
+        # Modify x by com offset
+        pos[0] += self.com_offset
+
+        # 4 legs, 3 joints per leg
+        joint_angles = np.zeros((4, 3))
+
+        # print("T_bf: {}".format(T_bf))
+
+        # Steps 1 and 2 of pipeline here
+        HipToFoot = self.HipToFoot(orn, pos, T_bf)
+
+        for i, (key, p_hf) in enumerate(HipToFoot.items()):
+            # ORDER: FL, FR, FR, BL, BR
+
+            # print("LEG: {} \t HipToFoot: {}".format(key, p_hf))
+
+            # Step 3, compute joint angles from T_hf for each leg
+            joint_angles[i, :] = self.Legs[key].solve(p_hf)
+
+        # print("-----------------------------")
+
+        return joint_angles