move some code to dynamics

petercorke · petercorke · commit 0983b360fea9 · 2020-09-10T20:18:52.000+10:00
add flesh to jointdynamics and twists
jointdynamics runs but not agreeing with MATLAB
diff --git a/roboticstoolbox/robot/Dynamics.py b/roboticstoolbox/robot/Dynamics.py
@@ -223,6 +223,71 @@ def pay(self, W, q=None, J=None, frame=1):
 
         return tau
 
+    def payload(self, m, p=np.zeros(3)):
+        """
+        payload(m, p) adds payload mass adds a payload with point mass m at
+        position p in the end-effector coordinate frame.
+
+        payload(m) adds payload mass adds a payload with point mass m at
+        in the end-effector coordinate frame.
+
+        payload(0) removes added payload.
+
+        :param m: mass (kg)
+        :type m: float
+        :param p: position in end-effector frame
+        :type p: float ndarray(3,1)
+
+        """
+
+        p = getvector(p, 3, out='col')
+        lastlink = self.links[self.n - 1]
+
+        lastlink.m = m
+        lastlink.r = p
+
+    def jointdynamics(self, q, qd=None):
+        """
+        Transfer function of joint actuator
+
+        :param q: The joint angles/configuration of the robot
+        :type q: float ndarray(n)
+        :param qd: The joint velocities of the robot
+        :type qd: float ndarray(n)
+        :return: transfer function denominators
+        :rtype: list of 2-tuples
+
+        - ``tf = jointdynamics(qd, q)`` calculates a vector of n continuous-time
+          transfer functions that represent the transfer function
+          1/(Js+B) for each joint based on the dynamic parameters of the robot
+          and the configuration q (n). n is the number of robot joints.
+
+        - ``tf = jointdynamics(q, qd)`` as above but include the linearized effects
+          of Coulomb friction when operating at joint velocity QD (1xN).
+        """
+
+        tf = []
+        for j, link in enumerate(self.links):
+            
+            # compute inertia for this joint
+            zero = np.zeros((self.n))
+            qdd = np.zeros((self.n))
+            qdd[j] = 1
+            M = self.rne(q, zero, qdd, grav=[0, 0, 0])
+            J = link.Jm + M[j] / abs(link.G) ** 2
+            
+            # compute friction
+            B = link.B
+            if qd is not None:
+                # add linearized Coulomb friction at the operating point
+                if qd > 0:
+                    B += link.Tc[0] / qd[j]
+                elif qd < 0:
+                    B += link.Tc[1] / qd[j]
+            tf.append((J, B))
+
+        return tf
+
     def friction(self, qd):
         """
         tau = friction(qd) calculates the vector of joint friction
diff --git a/roboticstoolbox/robot/SerialLink.py b/roboticstoolbox/robot/SerialLink.py
@@ -10,7 +10,7 @@
 from spatialmath.base.argcheck import \
     getvector, ismatrix, isscalar, verifymatrix
 from spatialmath.base.transforms3d import tr2delta, tr2eul
-from spatialmath import SE3
+from spatialmath import SE3, Twist3
 from scipy.optimize import minimize, Bounds, LinearConstraint
 from frne import init, frne, delete
 from roboticstoolbox.backend.PyPlot.functions import \
@@ -483,51 +483,7 @@ def isspherical(self):
         else:
             return False
 
-    def payload(self, m, p=np.zeros(3)):
-        """
-        payload(m, p) adds payload mass adds a payload with point mass m at
-        position p in the end-effector coordinate frame.
-
-        payload(m) adds payload mass adds a payload with point mass m at
-        in the end-effector coordinate frame.
-
-        payload(0) removes added payload.
-
-        :param m: mass (kg)
-        :type m: float
-        :param p: position in end-effector frame
-        :type p: float ndarray(3,1)
-
-        """
-
-        p = getvector(p, 3, out='col')
-        lastlink = self.links[self.n - 1]
-
-        lastlink.m = m
-        lastlink.r = p
-
-    def jointdynamics(self, q, qd):
-        """
-        Transfer function of joint actuator.
 
-        tf = jointdynamics(qd, q) calculates a vector of n continuous-time
-        transfer function objects that represent the transfer function
-        1/(Js+B) for each joint based on the dynamic parameters of the robot
-        and the configuration q (n). n is the number of robot joints.
-
-        tf = jointdynamics(qd) as above except uses the stored q value of the
-        robot object.
-
-        :param q: The joint angles/configuration of the robot (Optional,
-            if not supplied will use the stored q values)
-        :type q: float ndarray(n)
-        :param qd: The joint velocities of the robot
-        :type qd: float ndarray(n)
-
-        """
-
-        # TODO a tf object implementation?
-        pass
 
     def isprismatic(self):
         """
@@ -625,35 +581,52 @@ def toradians(self, q):
 
     def twists(self, q=None):
         """
-        Joint axis twists.
-
-        tw, T = twists(q) calculates a vector of Twist objects (n) that
-        represent the axes of the joints for the robot with joint coordinates
-        q (n). Also returns T0 which is an SE3 object representing the pose of
-        the tool.
-
-        tw, T = twists() as above except uses the stored q value of the
-        robot object.
-
-        :param q: The joint angles/configuration of the robot (Optional,
-            if not supplied will use the stored q values)
-        :type q: float ndarray(n)
+        Joint axis as  twists
 
+        :param q: The joint angles/configuration of the robot
+        :type q: array_like (n)
         :return tw: a vector of Twist objects
         :rtype tw: float ndarray(n,)
         :return T0: Represents the pose of the tool
         :rtype T0: SE3
 
+        - ``tw, T0 = twists(q)`` calculates a vector of Twist objects (n) that
+          represent the axes of the joints for the robot with joint coordinates
+          ``q`` (n). Also returns T0 which is an SE3 object representing the pose of
+          the tool.
+
+        - ``tw, T0 = twists()`` as above but the joint coordinates are taken to be
+          zero.
+
         """
 
-        pass
+        if q is None:
+            q = np.zeros((self.n))
 
-        # if q is None:
-        #     q = np.copy(self.q)
-        # else:
-        #     q = getvector(q, self.n)
+        T = self.fkine_all(q)
+        tw = Twist3.Alloc(self.n)
+        if not self.mdh:
+            # DH case
+            for j in range(self.n):
+                if j == 0:
+                    if self.links[j].sigma == 0:
+                        tw[j] = Twist3.R([0, 0, 1], [0, 0, 0])  # revolute
+                    else:
+                        tw[j] = Twist3.P([0, 0, 1])  # prismatic
+                else:
+                    if self.links[j].sigma == 0:
+                        tw[j] = Twist3.R(T[j-1].a, T[j-1].t)  # revolute
+                    else:
+                        tw[j] = Twist3.P(T[j-1].a)  # prismatic
+        else:
+            # MDH case
+            for j in range(self.n):
+                if self.links[j].sigma == 0:
+                    tw[j] = Twist3.R(T[j].a, T[j].t)
+                else:
+                    tw[j] = Twist3.P(T[j].a, T[j].t)
 
-        # TODO Implement this
+        return tw, T[-1]
 
     def fkine(self, q=None):
         '''
@@ -715,10 +688,10 @@ def fkine(self, q=None):
 
         return t
 
-    def allfkine(self, q=None):
+    def fkine_all(self, q=None):
         '''
         Tall = allfkine(q) evaluates fkine for each joint within a robot and
-        returns a trajecotry of poses.
+        returns a sequence of link frame poses.
 
         Tall = allfkine() as above except uses the stored q value of the
         robot object.
