Exemplo n.º 1
0
    def get_cjoint_jac(self):
        from numpy.matlib import zeros
        from jacobians import serialKinematicJacobianPassive as jacobian

        n_endjoints = len(self.chains)
        from serialmechanism import n_pjoints
        n_pjnts = n_pjoints(l_from_l_of_l(self.pjoints))
        J = zeros((6 * (n_endjoints - 1), n_pjnts))

        # Put J0 more times in the first columns.
        J0 = jacobian(self.chains[0])
        n_pjoints0 = J0.shape[1]
        for i in range(n_endjoints - 1):
            J[(6 * i):(6 * i + 6), :n_pjoints0] = J0

        # Put -J1, -J2, ... in the diagonal of the non-yet-filled rest part
        # of J.
        np = n_pjoints0
        for i in range(1, n_endjoints):
            npi = n_pjoints(
                self.chains[i])  # MAYBE HERE should be # of passive joints
            Ji = jacobian(self.chains[i])
            J[(6 * (i - 1)):(6 * i), np:(np + npi)] = -Ji
            np += npi
        return J
Exemplo n.º 2
0
    def get_cjoint_jac(self):
        from numpy.matlib import zeros
        from jacobians import serialKinematicJacobianPassive as jacobian

        n_endjoints = len(self.chains)
        from  serialmechanism import n_pjoints
        n_pjnts = n_pjoints(l_from_l_of_l(self.pjoints))
        J = zeros((6 * n_endjoints, n_pjnts))
		# J = [J0 -J1  0  ...
		#	   J0  0  -J2 ...
		#			...
		#	   J0  0  ...    -Jb]

        # Put J0 more times in the first columns.
		# J0 = jacobian for main branch
		# n_points0 = number of passive joints in main branch
        J0 = jacobian(self.chains[0])
        n_pjoints0 = J0.shape[1]
        for i in range(n_endjoints - 1):
            J[(6 * i):(6 * i + 6), :n_pjoints0] = J0

        # Put -J1, -J2, ... in the diagonal of the non-yet-filled rest part
        # of J.
        np = n_pjoints0
        for i in range(1, n_endjoints):
			npi = n_pjoints(self.chains[i])
            Ji = jacobian(self.chains[i])
            J[(6 * i):(6 * i + 6), np:(np + npi)] = -Ji
            np += npi
Exemplo n.º 3
0
    def get_cjoint_jac(self):
        from numpy.matlib import zeros
        from jacobians import serialKinematicJacobianPassive as jacobian

        n_endjoints = len(self.chains)
        from serialmechanism import n_pjoints
        n_pjnts = n_pjoints(l_from_l_of_l(self.pjoints))
        J = zeros((6 * (n_endjoints - 1), n_pjnts))

        # Put J0 more times in the first columns.
        J0 = jacobian(self.chains[0])
        n_pjoints0 = J0.shape[1]
        for i in range(n_endjoints - 1):
            J[(6 * i):(6 * i + 6), :n_pjoints0] = J0

        # Put -J1, -J2, ... in the diagonal of the non-yet-filled rest part
        # of J.
        np = n_pjoints0
        for i in range(1, n_endjoints):
            npi = n_pjoints(self.chains[i]) # MAYBE HERE should be # of passive joints
            Ji = jacobian(self.chains[i])
            J[(6 * (i - 1)):(6 * i), np:(np + npi)] = -Ji
            np += npi
        return J
Exemplo n.º 4
0
from jacobians import serialKinematicJacobian as jacobian
from robots import table_rx90

kin = Kinematics(table_rx90)
kin.set_q([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
jnts = kin.joints

# This jacobian is obtained from the course 'Modeling and Control of Manipulators' of Wisama Khalil
numer_jac = matrix([[-10.2079, -408.5824, -349.2793,  0,       0,       0],
                     [101.7389, -40.9950,  -35.0448,   0,       0,       0],
                     [0,         102.2498, -191.7702,  0,       0,       0],
                     [0,         0.0998,    0.0998,   -0.4770,  0.4320, -0.7856],
                     [0,        -0.9950,   -0.9950,   -0.0479, -0.8823, -0.2665],
                     [1.0000,    0.0000,    0.0000,    0.8776,  0.1867,  0.5584]
                    ])
J = jacobian(jnts)
assert allclose(numer_jac, J, rtol=8e-04)

# From "Robots-manipulateurs de type série" - Wisama Khalil, Etienne Dombre, p.12.
# http://www.gdr-robotique.org/cours_de_robotique/?id=fd80a49ceaa004030c95cdacb020ec69.
# In French.
q0, q1, q2, q3, q4, q5 = kin.get_q()
c1 = cos(q0)
c2 = cos(q1)
c3 = cos(q2)
c4 = cos(q3)
c5 = cos(q4)
s1 = sin(q0)
s2 = sin(q1)
s3 = sin(q2)
s4 = sin(q3)
Exemplo n.º 5
0
from kinematics import Kinematics
from jacobians import serialKinematicJacobian as jacobian
from robots import table_rx90

kin = Kinematics(table_rx90)
kin.set_q([0.1, 0.2, 0.3, 0.4, 0.5, 0.6])
jnts = kin.joints

# This jacobian is obtained from the course 'Modeling and Control of Manipulators' of Wisama Khalil
numer_jac = matrix([[-10.2079, -408.5824, -349.2793, 0, 0, 0],
                    [101.7389, -40.9950, -35.0448, 0, 0, 0],
                    [0, 102.2498, -191.7702, 0, 0, 0],
                    [0, 0.0998, 0.0998, -0.4770, 0.4320, -0.7856],
                    [0, -0.9950, -0.9950, -0.0479, -0.8823, -0.2665],
                    [1.0000, 0.0000, 0.0000, 0.8776, 0.1867, 0.5584]])
J = jacobian(jnts)
assert allclose(numer_jac, J, rtol=8e-04)

# From "Robots-manipulateurs de type série" - Wisama Khalil, Etienne Dombre, p.12.
# http://www.gdr-robotique.org/cours_de_robotique/?id=fd80a49ceaa004030c95cdacb020ec69.
# In French.
q0, q1, q2, q3, q4, q5 = kin.get_q()
c1 = cos(q0)
c2 = cos(q1)
c3 = cos(q2)
c4 = cos(q3)
c5 = cos(q4)
s1 = sin(q0)
s2 = sin(q1)
s3 = sin(q2)
s4 = sin(q3)