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
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
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
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)
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)
