@@ -573,20 +573,144 @@ def itorque(self, qdd, q=None):
573573 else :
574574 return taui
575575
576- def gravjac (self , q = None , grav = None ):
577- """
578- tauB = gravjac(q, grav) calculates the generalised joint force/torques
579- due to gravity.
576+ # NOT CONVINCED WE NEED THIS, AND IT'S ORPHAN CODE
577+ # def gravjac(self, q, grav=None):
578+ # """
579+ # Compute gravity load and Jacobian
580+
581+ # :param q: The joint angles/configuration of the robot
582+ # :type q: float ndarray(n)
583+ # :param grav: The gravity vector (Optional, if not supplied will
584+ # use the stored gravity values).
585+ # :type grav: float ndarray(3,)
586+
587+ # :return tau: The generalised joint force/torques due to gravity
588+ # :rtype tau: float ndarray(n,)
589+
590+ # ``tauB = gravjac(q, grav)`` calculates the generalised joint force/torques
591+ # due to gravity and the Jacobian
592+
593+ # Trajectory operation:
594+ # If q is nxm where n is the number of robot joints then a
595+ # trajectory is assumed where each row of q corresponds to a robot
596+ # configuration. tau (nxm) is the generalised joint torque, each row
597+ # corresponding to an input pose, and jacob0 (6xnxm) where each
598+ # plane is a Jacobian corresponding to an input pose.
599+
600+ # :notes:
601+ # - The gravity vector is defined by the SerialLink property if not
602+ # explicitly given.
603+ # - Does not use inverse dynamics function RNE.
604+ # - Faster than computing gravity and Jacobian separately.
605+
606+ # Written by Bryan Moutrie
607+
608+ # :seealso: :func:`gravload`
609+ # """
610+
611+ # # TODO use np.cross instead
612+ # def _cross3(self, a, b):
613+ # c = np.zeros(3)
614+ # c[2] = a[0] * b[1] - a[1] * b[0]
615+ # c[0] = a[1] * b[2] - a[2] * b[1]
616+ # c[1] = a[2] * b[0] - a[0] * b[2]
617+ # return c
618+
619+ # def makeJ(O, A, e, r):
620+ # J[3:6,:] = A
621+ # for j in range(r):
622+ # if r[j]:
623+ # J[0:3,j] = cross3(A(:,j),e-O(:,j));
624+ # else:
625+ # J[:,j] = J[[4 5 6 1 2 3],j]; %J(1:3,:) = 0;
626+
627+ # if grav is None:
628+ # grav = np.copy(self.gravity)
629+ # else:
630+ # grav = getvector(grav, 3)
631+
632+ # try:
633+ # if q is not None:
634+ # q = getvector(q, self.n, 'col')
635+ # else:
636+ # q = np.copy(self.q)
637+ # q = getvector(q, self.n, 'col')
638+
639+ # poses = 1
640+ # except ValueError:
641+ # poses = q.shape[1]
642+ # verifymatrix(q, (self.n, poses))
643+
644+ # if not self.mdh:
645+ # baseAxis = self.base.a
646+ # baseOrigin = self.base.t
647+
648+ # tauB = np.zeros((self.n, poses))
649+
650+ # # Forces
651+ # force = np.zeros((3, self.n))
652+
653+ # for joint in range(self.n):
654+ # force[:, joint] = np.squeeze(self.links[joint].m * grav)
655+
656+ # # Centre of masses (local frames)
657+ # r = np.zeros((4, self.n))
658+ # for joint in range(self.n):
659+ # r[:, joint] = np.r_[np.squeeze(self.links[joint].r), 1]
660+
661+ # for pose in range(poses):
662+ # com_arr = np.zeros((3, self.n))
663+
664+ # T = self.fkine_all(q[:, p
57AE
ose])
665+
666+ # jointOrigins = np.zeros((3, self.n))
667+ # jointAxes = np.zeros((3, self.n))
668+ # for i in range(self.n):
669+ # jointOrigins[:, i] = T[i].t
670+ # jointAxes[:, i] = T[i].a
671+
672+ # if not self.mdh:
673+ # jointOrigins = np.c_[
674+ # baseOrigin, jointOrigins[:, :-1]
675+ # ]
676+ # jointAxes = np.c_[
677+ # baseAxis, jointAxes[:, :-1]
678+ # ]
679+
680+ # # Backwards recursion
681+ # for joint in range(self.n - 1, -1, -1):
682+ # # C.o.M. in world frame, homog
683+ # com = T[joint].A @ r[:, joint]
684+
685+ # # Add it to the distal others
686+ # com_arr[:, joint] = com[0:3]
687+
688+ # t = np.zeros(3)
689+
690+ # # for all links distal to it
691+ # for link in range(joint, self.n):
692+ # if not self.links[joint].sigma:
693+ # # Revolute joint
694+ # d = com_arr[:, link] - jointOrigins[:, joint]
695+ # t = t + self._cross3(d, force[:, link])
696+ # # Though r x F would give the applied torque
697+ # # and not the reaction torque, the gravity
698+ # # vector is nominally in the positive z
699+ # # direction, not negative, hence the force is
700+ # # the reaction force
701+ # else:
702+ # # Prismatic joint
703+ # # Force on prismatic joint
704+ # t = t + force[:, link]
705+
706+ # tauB[joint, pose] = t.T @ jointAxes[:, joint]
707+
708+ # if poses == 1:
709+ # return tauB[:, 0]
710+ # else:
711+ # return tauB
580712
581- tauB = gravjac() as above except uses the stored q and gravitational
582- acceleration of the robot object.
583713
584- Trajectory operation:
585- If q is nxm where n is the number of robot joints then a
586- trajectory is assumed where each row of q corresponds to a robot
587- configuration. tau (nxm) is the generalised joint torque, each row
588- corresponding to an input pose, and jacob0 (6xnxm) where each
589- plane is a Jacobian corresponding to an input pose.
590714
591715 :param q : The joint angles / configuration of the robot (Optional ,
592716 if not supplied will use the stored q values ).
0 commit comments