up - working? 🐝 :honey:

Author: walchko

dev/test.py

@@ -31,18 +31,18 @@ def main():
     t = kevin()
     kc = KinematicChain.from_parameters(t)
 
-    j = [
-        [0,0,0,0,0],
-        [0,pi/4,-pi/4,-pi/2,0], # 204, 0, -32
-        # [0,0,-pi/2,0,0],
-        # np.deg2rad([90,131.17,-106.19,-114.98]),
-        np.deg2rad([-45.00, 77.41, -98.15, -69.27, 0]) # 110, -110, -40
-    ]
+    # j = [
+    #     [0,0,0,0,0],
+    #     [0,pi/4,-pi/4,-pi/2,0], # 204, 0, -32
+    #     # [0,0,-pi/2,0,0],
+    #     # np.deg2rad([90,131.17,-106.19,-114.98]),
+    #     np.deg2rad([-45.00, 77.41, -98.15, -69.27, 0]) # 110, -110, -40
+    # ]
 
-    for jj in j:
-        # print(jj)
-        t = kc.transform(jj)
-        print(t)
+    # for jj in j:
+    #     # print(jj)
+    #     t = kc.transform(jj)
+    #     print(t)
     #     print("----------------------")
 
     j = [
@@ -53,7 +53,13 @@ def main():
     ]
 
     for jj in j:
-        rads = kc.inverse(jj)
+        p = np.array([
+            [0,1,0, jj[0]],
+            [0,0,-1,jj[1]],
+            [-1,0,0,jj[2]],
+            [0,0,0,1]
+        ])
+        rads = kc.inverse(p)
         print(">>", np.rad2deg(rads))
 
 

dev/test2.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+"""Basic usage of the pybotics package."""
+import numpy as np
+from mdh import __version__ as version
+from mdh.link import mdh_params, JointType, RevoluteLink
+from mdh.kinematic_chain import KinematicChain
+
+pi = np.pi
+
+print(f">> {version}")
+
+# make it print better
+np.set_printoptions(suppress=True)
+
+def kevin():
+    mm = pi
+    """alpha a theta d"""
+    dh = [
+        {'alpha':    0, 'a':  0, 'theta': 0, 'd': 0, 'type': 1},
+        {'alpha': pi/2, 'a': 50, 'theta': 0, 'd': 0, 'type': 1},
+        {'alpha':    0, 'a': 65, 'theta': 0, 'd': 0, 'type': 1},
+        {'alpha': 0, 'a': 68, 'theta': 0, 'd': 0, 'type': 1, 'fixed': True}
+    ]
+
+    return dh
+
+def main():
+    # l = RevoluteLink(1,2,3,4)
+
+    t = kevin()
+    kc = KinematicChain.from_parameters(t)
+    print(f">> size: {kc.size}")
+
+    for l in kc:
+        print(f">> {l}")
+
+    t = kc.forward(np.deg2rad([0,0,0,0]))
+    print(t)
+
+    t = kc.forward(np.deg2rad([0,82.62,-154.1,0]))
+    print(t)
+
+    jnts = kc.inverse(t)
+    print(">> ", np.rad2deg(jnts))
+
+    p = np.array([
+        [0,1,0, 80],
+        [0,0,-1,0],
+        [-1,0,0,0],
+        [0,0,0,1]
+    ])
+
+    # p = np.array([
+    #     [1,0,0, 80],
+    #     [0,1,0,0],
+    #     [0,0,1,0],
+    #     [0,0,0,1]
+    # ])
+
+    jnts = kc.inverse(p)
+    print(">> ", np.rad2deg(jnts))
+
+if __name__ == "__main__":
+    main()

mdh/kinematic_chain.py

@@ -1,6 +1,6 @@
 import numpy as np # type: ignore
 import attr
-from typing import Any, Optional, Sequence, Sized, Union, Iterable, Dict, List
+# from typing import Any, Optional, Sequence, Sized, Union, Iterable, Dict, List
 # import scipy # type: ignore
 import scipy.optimize # type: ignore
 # from numpy.linalg import norm
@@ -21,12 +21,16 @@ class UnReachable(Exception):
     pass
 
 @attr.s
-class KinematicChain(Sized, Iterable):
-    _links = attr.ib(type=Sequence[Any])
+# class KinematicChain(Sized, Iterable):
+class KinematicChain:
+    # _links = attr.ib(type=Sequence[Any])
+    _links = attr.ib()
+    _size = attr.ib()
 
     def __len__(self):
         """Enables the length function, returns number links"""
-        return len(self._links)
+        # return len(self._links)
+        return self._size
 
     def __iter__(self):
         """Enables iteration of joints:  for j in chain: print(j)"""
@@ -39,8 +43,8 @@ def __getitem__(self, key):
 
     def transform(self, joints):
         """Calculates the transformation and returns a 4x4 matrix"""
-        if len(joints) != len(self._links):
-            raise Exception("inputs don't equal number of links")
+        if len(joints) != self.size:
+            raise Exception(f"inputs don't equal number of links {len(joints)} != {self.size}")
 
         t = np.eye(4)
         for l, q in zip(reversed(self._links), reversed(joints)):
@@ -63,14 +67,14 @@ def inverse(self, pos):
 
             FIXME: this only works for when the foot is straight down
             """
-            j = [[],[]] # type: Sequence[List]
+            joint_limits = [[],[]] # type: Sequence[List]
             for l in self:
-                j[0].append(l.min)
-                j[1].append(l.max)
-            # j[0][3] = -0.00001
-            # j[1][3] = 0.00001
-
-            joint_limits = np.array(j) # type: np.ndarray
+                joint_limits[0].append(l.min)
+                joint_limits[1].append(l.max)
+            # # j[0][3] = -0.00001
+            # # j[1][3] = 0.00001
+            # joint_limits = j
+            # joint_limits = np.array(j) # type: np.ndarray
             # print(joint_limits)
 
             # r = R.from_matrix([[1,0,0],[0,0,-1],[0,1,0]])
@@ -79,65 +83,81 @@ def inverse(self, pos):
 
             # make desired transform pose (position, oritentation)
             # change the -90 x-axis to something else for non-down??
-            x = pos[0]
-            y = pos[1]
-            c = R.from_euler('zyx', [-90,atan2(y,x)*180/pi,90], degrees=True)
-            ppos = np.eye(4)
-            ppos[:3,:3] = c.as_matrix()
-            ppos[:3, 3] = pos
+            # x = pos[0]
+            # y = pos[1]
+            # c = R.from_euler('zyx', [-90,atan2(y,x)*180/pi,90], degrees=True)
+            # ppos = np.eye(4)
+            # ppos[:3,:3] = c.as_matrix()
+            # ppos[:3, 3] = pos
 
             start = [0]*len(self._links)
 
             result = scipy.optimize.least_squares(
                 fun=_ik_cost_function,
                 x0=np.array(start),
                 bounds=joint_limits,
-                args=(ppos, self)
-            )  # type: scipy.optimize.OptimizeResult
+                args=(pos, self)
+            )
 
             if not result.success:  # pragma: no cover
                 raise UnReachable(f"Can't reach: {pos}m")
             # print(">> yeah!!!")
             # print(">>", np.rad2deg(result.x))
             actual_t = self.transform(result.x)
-            actual_pos = actual_t[:3,3]
+            actual_pos = actual_t #actual_t[:3,3]
             if np.allclose(actual_pos, pos, atol=1e-3):
                 # print(result)
                 return result.x
 
             raise UnReachable(f"Can't reach: {pos}")
 
+    @property
+    def size(self):
+        return self._size
+
     @classmethod
     def from_parameters(cls, params):
         """Builds a KinematicChain object from an input"""
         links = []
+        size = 0
         for l in params:
             if not isinstance(l, dict):
                 raise Exception(f"Invalid parameters: {l}")
 
-            for key in ['alpha', 'a', 'theta', 'd', 'type']:
+            for key in ['alpha', 'a', 'theta', 'd']:
                 if key not in l:
                     raise Exception(f"Missing parameter: {key}")
 
-            ll = mdh_params(l['alpha'], l['a'], l['theta'], l['d'], l['type'])
-
-            if ll.type == JointType.revolute:
-                link = RevoluteLink(a=ll.a, alpha=ll.alpha, d=ll.d, theta=ll.theta)
-            elif ll.type == JointType.prismatic:
-                raise NotImplementedError(f"from_parameters: {l.type}")
-            else:
-                raise Exception(f"Invalid parameter: {l.type}")
-
-            if "max_min" in l:
-                link.min = l["max_min"][1]
-                link.max = l["max_min"][0]
-            else:
-                link.min = -pi
-                link.max = pi
+            # ll = mdh_params(l['alpha'], l['a'], l['theta'], l['d'])
+            # print(f">> params: {ll}")
+            # print(l['alpha'])
+
+            # if ll.type == JointType.revolute:
+            # link = RevoluteLink(a=ll.a, alpha=ll.alpha, d=ll.d, theta=ll.theta)
+            # link = RevoluteLink(alpha=ll.alpha, a=ll.a, theta=ll.theta, d=ll.d)
+            link = RevoluteLink(l['alpha'], l['a'], l['theta'], l['d'])
+            # elif ll.type == JointType.prismatic:
+            #     raise NotImplementedError(f"from_parameters: {l.type}")
+            # else:
+            #     raise Exception(f"Invalid parameter: {l.type}")
+
+            # if "max_min" in l:
+            #     link.min = l["max_min"][1]
+            #     link.max = l["max_min"][0]
+            # else:
+            link.min = -pi
+            link.max = pi
 
             links.append(link)
 
-        ret = cls(links)
+            # if 'fixed' in l:
+            #     if l['fixed'] is not True:
+            #         size += 1
+            # else:
+            #     size += 1
+            size += 1
+
+        ret = cls(links, size)
 
         return ret
 

mdh/link.py

@@ -3,55 +3,51 @@
 from enum import IntFlag
 import numpy as np # type: ignore
 np.set_printoptions(suppress=True)
+from colorama import Fore
 
 JointType =  IntFlag('JointType', 'revolute prismatic revolute_theda revolute_alpha')
-mdh_params = namedtuple("mdh_params", "alpha a theta d type")
+mdh_params = namedtuple("mdh_params", "alpha a theta d ")
 
 from math import pi
-rad2deg = pi/180
-
-# @attr.s
-# class Link:
-#     """
-#     why? what is the value of this?
-#     prismatic/revolute have parameters that never chance ... need to freeze them!
-#     """
-#     # alpha = attr.ib(type=float)
-#     # a = attr.ib(type=float)
-#     # theta = attr.ib(type=float)
-#     # d = attr.ib(type=float)
+rad2deg = 180/pi
+deg2rad = pi/180
 
 @attr.s(slots=True)
 class RevoluteLink:
     """
     RevoluteLink about theta. All other parameters (alpha, a, d) are fixed and
     cannot be changed once the link is created.
     """
-    _alpha = attr.ib(type=float)
-    _a = attr.ib(type=float)
-    theta = attr.ib(type=float)
-    _d = attr.ib(type=float)
-    min = attr.ib(init=False, default=-np.pi/2, type=float)
-    max = attr.ib(init=False, default=np.pi/2, type=float)
+    _alpha = attr.ib()
+    _a = attr.ib()
+    theta = attr.ib()
+    _d = attr.ib()
+    min = attr.ib(default=-np.pi/2)
+    max = attr.ib(default=np.pi/2)
+
+    @max.validator
+    def max_check(self, attribute, value):
+        if self.min > value:
+            raise Exception()
 
     @property
-    def alpha(self) -> float:
+    def alpha(self):
         return self._alpha
 
     @property
-    def a(self) -> float:
+    def a(self):
         return self._a
 
     @property
-    def d(self) -> float:
+    def d(self):
         return self._d
-
-    @property
-    def type(self):
-        return JointType.revolute
+    #
+    # @property
+    # def type(self):
+    #     return JointType.revolute
 
     """Some of these params are immutable, how do I do that?"""
-    def transform(self, angle: float):
+    def transform(self, angle):
         """could move this to link"""
 
         crx = np.cos(self._alpha)
@@ -76,16 +72,8 @@ def transform(self, angle: float):
         return transform
 
     def __str__(self):
-        aa = self._alpha*rad2deg
-        t = self.theta*rad2deg
-        s = f"Revolute: alpha: {aa:4.1f}deg a: {self.a:4.1f}m theta: {t:4.1}deg d: {self.d:4.1f}"
-        return s
-
-# @attr.s
-# class Prismatic:
-#     min = attr.ib(type=float)
-#     max = attr.ib(type=float)
-#
-#     @property
-#     def type(self):
-#         return JointType.revolute
+        saa = f"{Fore.CYAN}{self._alpha*rad2deg:4.1f}{Fore.RESET}"
+        st = f"{Fore.CYAN}{self.theta*rad2deg:4.1f}{Fore.RESET}"
+        sa = f"{Fore.YELLOW}{self._a:4.1f}{Fore.RESET}"
+        sd = f"{Fore.YELLOW}{self._d:4.1f}{Fore.RESET}"
+        return f"Rev[deg]: alpha: {saa} a: {sa} theta: {st} d: {sd}"