def _calc_R(self, name, lambdify=True):
""" Uses Sympy to generate the rotation matrix for a joint or link
Parameters
----------
name : string
name of the joint, link, or end-effector
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
R = None
R_func = None
filename = name + '_R'
# check to see if we have the rotation matrix saved in file
R, R_func = self._load_from_file(filename, lambdify=True)
if R is None and R_func is None:
# if no saved file was loaded, generate function
print('Generating rotation matrix function.')
R = self._calc_T(name=name)[:3, :3]
# save to file
os_utils.makedirs('%s/%s' % (self.config_folder, filename))
cloudpickle.dump(
sp.Matrix(R),
open('%s/%s/%s' % (self.config_folder, filename, filename),
'wb'))
if R_func is None:
R_func = self._generate_and_save_function(filename=filename,
expression=R,
parameters=self.q)
return R_func
def _calc_dJ(self, name, x, lambdify=True):
""" Generate the derivative of the Jacobian
Uses Sympy to generate the derivative of the Jacobian
for a joint or link with respect to time
Parameters
----------
name : string
name of the joint, link, or end-effector
x : numpy.array
the [x,y,z] offset inside the reference frame of 'name' [meters]
if not specified, (0, 0, 0) is hard coded in, rather than using
variable (x, y, z), which results in significant speedups.
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
dJ = None
dJ_func = None
filename = name + '[0,0,0]' if np.allclose(x, 0) else name
filename += '_dJ'
# check to see if should try to load functions from file
dJ, dJ_func = self._load_from_file(filename, lambdify)
if dJ is None and dJ_func is None:
# if no saved file was loaded, generate function
print('Generating derivative of Jacobian ',
'function for %s' % filename)
J = self._calc_J(name, x=x, lambdify=False)
dJ = sp.Matrix(np.zeros(J.shape, dtype='float32'))
# calculate derivative of (x,y,z) wrt to time
# which each joint is dependent on
for ii in range(J.shape[0]):
for jj in range(J.shape[1]):
for kk in range(self.N_JOINTS):
dJ[ii, jj] += J[ii, jj].diff(self.q[kk]) * self.dq[kk]
dJ = sp.Matrix(dJ)
# save expression to file
os_utils.makedirs('%s/%s' % (self.config_folder, filename))
cloudpickle.dump(
dJ,
open('%s/%s/%s' % (self.config_folder, filename, filename),
'wb'))
if lambdify is False:
# if should return expression not function
return dJ
if dJ_func is None:
dJ_func = self._generate_and_save_function(filename=filename,
expression=dJ,
parameters=self.q +
self.dq + self.x)
return dJ_func
def _calc_Tx(self, name, x=None, lambdify=True):
"""Return transform from x in reference frame of 'name' to the origin
Uses Sympy to transform x from the reference frame of a joint
or link to the origin (world) coordinates.
Parameters
----------
name : string
name of the joint, link, or end-effector
x : numpy.array
the [x,y,z] offset inside the reference frame of 'name' [meters]
if not specified, (0, 0, 0) is hard coded in, rather than using
variable (x, y, z), which results in significant speedups.
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
Tx = None
Tx_func = None
filename = name + '[0,0,0]' if np.allclose(x, 0) else name
filename += '_Tx'
# check to see if we have our transformation saved in file
Tx, Tx_func = self._load_from_file(filename, lambdify)
if Tx is None and Tx_func is None:
print('Generating transform function for %s' % filename)
T = self._calc_T(name=name)
# transform x into world coordinates
if np.allclose(x, 0):
# if we're only interested in the origin, not including
# the x variables significantly speeds things up
Tx = T * sp.Matrix([0, 0, 0, 1])
else:
# if we're interested in other points in the given frame
# of reference, calculate transform with x variables
Tx = T * sp.Matrix(self.x + [1])
Tx = sp.Matrix(Tx)
# save to file
os_utils.makedirs('%s/%s' % (self.config_folder, filename))
cloudpickle.dump(
sp.Matrix(Tx),
open('%s/%s/%s.Tx' % (self.config_folder, filename, filename),
'wb'))
if lambdify is False:
# if should return expression not function
return Tx
if Tx_func is None:
Tx_func = self._generate_and_save_function(filename=filename,
expression=Tx,
parameters=self.q +
self.x)
return Tx_func
def __init__(self,
N_JOINTS,
N_LINKS,
ROBOT_NAME="robot",
use_cython=False,
MEANS=None,
SCALES=None):
self.N_JOINTS = N_JOINTS
self.N_LINKS = N_LINKS
self.ROBOT_NAME = ROBOT_NAME
self.use_cython = use_cython
# dictionaries set by the sub-config, used for scaling input into
# neural systems. Calculate by recording data from movement of interest
self.MEANS = MEANS # expected mean of joints angles / velocities
self.SCALES = SCALES # expected variance of joint angles / velocities
# create function placeholders and dictionaries
self._c = None
self._dJ = {}
self._g = None
self._J = {}
self._M = None
self._orientation = {}
self._R = {}
self._S = None
self._T_inv = {}
self._Tx = {}
self._KZ = sp.Matrix([0, 0, 1])
# inertia matrix lists, to be filled out by subclasses
self._M_LINKS = []
self._M_JOINTS = []
# specify / create the folder to save to and load from
self.config_folder = (cache_dir + '/%s/saved_functions/' % ROBOT_NAME)
# create a unique hash for the config file
hasher = hashlib.md5()
with open(sys.modules[self.__module__].__file__, 'rb') as afile:
buf = afile.read()
hasher.update(buf)
self.config_hash = hasher.hexdigest()
self.config_folder += self.config_hash
# make config folder if it doesn't exist
os_utils.makedirs(self.config_folder)
# set up our joint angle symbols
self.q = [sp.Symbol('q%i' % ii) for ii in range(self.N_JOINTS)]
self.dq = [sp.Symbol('dq%i' % ii) for ii in range(self.N_JOINTS)]
# set up an (x,y,z) offset
self.x = [sp.Symbol('x'), sp.Symbol('y'), sp.Symbol('z')]
self.gravity = sp.Matrix([[0, 0, -9.81, 0, 0, 0]]).T
def _calc_M(self, lambdify=True):
""" Uses Sympy to generate the inertia matrix in joint space
Parameters
----------
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
M = None
M_func = None
# check to see if we have our inertia matrix saved in file
M, M_func = self._load_from_file('M', lambdify)
if M is None and M_func is None:
# if no saved file was loaded, generate function
print('Generating inertia matrix function')
# get the Jacobians for each link's COM
J_links = [
self._calc_J('link%s' % ii, x=[0, 0, 0], lambdify=False)
for ii in range(self.N_LINKS)
]
J_joints = [
self._calc_J('joint%s' % ii, x=[0, 0, 0], lambdify=False)
for ii in range(self.N_JOINTS)
]
# sum together the effects of each arm segment's inertia
M = sp.zeros(self.N_JOINTS)
for ii in range(self.N_LINKS):
# transform each inertia matrix into joint space
M += (J_links[ii].T * self._M_LINKS[ii] * J_links[ii])
# sum together the effects of each joint's inertia on each motor
for ii in range(self.N_JOINTS):
# transform each inertia matrix into joint space
M += (J_joints[ii].T * self._M_JOINTS[ii] * J_joints[ii])
M = sp.Matrix(M)
# save to file
os_utils.makedirs('%s/M' % (self.config_folder))
cloudpickle.dump(M, open('%s/M/M' % self.config_folder, 'wb'))
if lambdify is False:
# if should return expression not function
return M
if M_func is None:
M_func = self._generate_and_save_function(filename='M',
expression=M,
parameters=self.q)
return M_func
def _calc_g(self, lambdify=True):
""" Generate the force of gravity in joint space
Uses Sympy to generate the force of gravity in joint space
Parameters
----------
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
g = None
g_func = None
# check to see if we have our gravity term saved in file
g, g_func = self._load_from_file('g', lambdify)
if g is None and g_func is None:
# if no saved file was loaded, generate function
print('Generating gravity compensation function')
# get the Jacobians for each link's COM
J_links = [
self._calc_J('link%s' % ii, x=[0, 0, 0], lambdify=False)
for ii in range(self.N_LINKS)
]
J_joints = [
self._calc_J('joint%s' % ii, x=[0, 0, 0], lambdify=False)
for ii in range(self.N_JOINTS)
]
# sum together the effects of each arm segment's inertia
g = sp.zeros(self.N_JOINTS, 1)
for ii in range(self.N_LINKS):
# transform each inertia matrix into joint space
g += (J_links[ii].T * self._M_LINKS[ii] * self.gravity)
# sum together the effects of each joint's inertia on each motor
for ii in range(self.N_JOINTS):
# transform each inertia matrix into joint space
g += (J_joints[ii].T * self._M_JOINTS[ii] * self.gravity)
g = sp.Matrix(g)
# save to file
os_utils.makedirs('%s/g' % self.config_folder)
cloudpickle.dump(g, open('%s/g/g' % self.config_folder, 'wb'))
if lambdify is False:
# if should return expression not function
return g
if g_func is None:
g_func = self._generate_and_save_function(filename='g',
expression=g,
parameters=self.q)
return g_func
def _calc_S(self, lambdify=True):
""" Uses Sympy to generate the centrifugal and Coriolis forces
Derivation from vector format 2 on slide 22 at:
www.diag.uniroma1.it/~deluca/rob2_en/03_LagrangianDynamics_1.pdf
NOTE: the full effects are calculated in the _calc_c method
Parameters
----------
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
S = None
S_func = None
# check to see if we have our term saved in file
S, S_func = self._load_from_file('S', lambdify)
if S is None and S_func is None:
# if no saved file was loaded, generate function
print('Generating centripetal and Coriolis compensation function')
# first get the inertia matrix
M = self._calc_M(lambdify=False)
# C_k = .5 * (\frac{\partial m_k}{\partial q} +
# \frac{\partial m_k}{\partial q}^T +
# \frac{\partial M}{\partia q_k})
# S_kj = sum_i (C_{kij}(q) * dq[i])
# where c_k and m_k are the kth element of c and column of M
S = sp.zeros(self.N_JOINTS, self.N_JOINTS)
for kk in range(self.N_JOINTS):
dMkdq = M[:, kk].jacobian(sp.Matrix(self.q))
Ck = 0.5 * (dMkdq + dMkdq.T - M.diff(self.q[kk]))
for jj in range(self.N_JOINTS):
S[kk] = np.sum([
Ck[ii, jj] * self.dq[ii] for ii in range(self.N_JOINTS)
])
S = sp.Matrix(S)
# save to file
os_utils.makedirs('%s/S' % self.config_folder)
cloudpickle.dump(S, open('%s/S/S' % self.config_folder, 'wb'))
if lambdify is False:
# if should return expression not function
return S
if S_func is None:
S_func = self._generate_and_save_function(filename='S',
expression=S,
parameters=self.q +
self.dq)
return S_func
def _calc_T_inv(self, name, x, lambdify=True):
""" Return the inverse transform matrix
Return the inverse transform matrix, which converts from
world coordinates into the robot's end-effector reference frame
Parameters
----------
name : string
name of the joint, link, or end-effector
x : numpy.array
the [x,y,z] offset inside the reference frame of 'name' [meters]
if not specified, (0, 0, 0) is hard coded in, rather than using
variable (x, y, z), which results in significant speedups.
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
T_inv = None
T_inv_func = None
filename = name + '[0,0,0]' if np.allclose(x, 0) else name
filename += '_Tinv'
# check to see if we have our transformation saved in file
T_inv, T_inv_func = self._load_from_file(filename, lambdify)
if T_inv is None and T_inv_func is None:
print('Generating inverse transform function for %s' % filename)
T = self._calc_T(name=name)
rotation_inv = T[:3, :3].T
translation_inv = -rotation_inv * T[:3, 3]
T_inv = rotation_inv.row_join(translation_inv).col_join(
sp.Matrix([[0, 0, 0, 1]]))
T_inv = sp.Matrix(T_inv)
# save to file
os_utils.makedirs('%s/%s' % (self.config_folder, filename))
cloudpickle.dump(
T_inv,
open('%s/%s.T_inv' % (self.config_folder, filename), 'wb'))
if lambdify is False:
# if should return expression not function
return T_inv
if T_inv_func is None:
T_inv_func = self._generate_and_save_function(filename=filename,
expression=T_inv,
parameters=self.q +
self.x)
return T_inv_func
def _calc_c(self, lambdify=True):
""" Uses Sympy to generate the centrifugal and Coriolis forces
Derivation from vector form 1 on slide 22 at:
www.diag.uniroma1.it/~deluca/rob2_en/03_LagrangianDynamics_1.pdf
NOTE: the partial effects are calculated in the _calc_S method
Parameters
----------
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
c = None
c_func = None
# check to see if we have our term saved in file
c, c_func = self._load_from_file('c', lambdify)
if c is None and c_func is None:
# if no saved file was loaded, generate function
print('Generating centripetal and Coriolis compensation function')
# first get the inertia matrix
M = self._calc_M(lambdify=False)
# c_k = dq.T * C_k * dq
# C_k = .5 * (\frac{\partial m_k}{\partial q} +
# \frac{\partial m_k}{\partial q}^T +
# \frac{\partial M}{\partia q_k})
# where c_k and m_k are the kth element of c and column of M
c = sp.zeros(self.N_JOINTS, 1)
for kk in range(self.N_JOINTS):
dMkdq = M[:, kk].jacobian(sp.Matrix(self.q))
Ck = 0.5 * (dMkdq + dMkdq.T - M.diff(self.q[kk]))
c[kk] = sp.Matrix(self.dq).T * Ck * sp.Matrix(self.dq)
c = sp.Matrix(c)
# save to file
os_utils.makedirs('%s/c' % self.config_folder)
cloudpickle.dump(c, open('%s/c/c' % self.config_folder, 'wb'))
if lambdify is False:
# if should return expression not function
return c
if c_func is None:
c_func = self._generate_and_save_function(filename='c',
expression=c,
parameters=self.q +
self.dq)
return c_func
def _generate_and_save_function(self, filename, expression, parameters):
""" Creates a folder, saves generated cython functions
Create a folder in the users cache directory, named based on a hash
of the current robot_config subclass.
If use_cython is True, uses the created folder to save the autowrap
binaries, so that they can be loaded in quickly later.
"""
# check for / create the save folder for this expression
folder = self.config_folder + '/' + filename
os_utils.makedirs(folder)
if self.use_cython is True:
# binaries saved by specifying tempdir parameter
function = autowrap(expression,
backend="cython",
args=parameters,
tempdir=folder)
function = sp.lambdify(parameters, expression, "numpy")
return function
def _calc_J(self, name, x, lambdify=True):
""" Uses Sympy to generate the Jacobian for a joint or link
Parameters
----------
name : string
name of the joint, link, or end-effector
x : numpy.array
the [x,y,z] offset inside the reference frame of 'name' [meters]
if not specified, (0, 0, 0) is hard coded in, rather than using
variable (x, y, z), which results in significant speedups.
lambdify : boolean, optional (Default: True)
if True returns a function to calculate the matrix.
If False returns the Sympy matrix
"""
J = None
J_func = None
filename = name + '[0,0,0]' if np.allclose(x, 0) else name
filename += '_J'
# check to see if should try to load functions from file
J, J_func = self._load_from_file(filename, lambdify)
if J is None and J_func is None:
# if no saved file was loaded, generate function
print('Generating Jacobian function for %s' % filename)
Tx = self._calc_Tx(name, x=x, lambdify=False)
# NOTE: calculating the Jacobian this way doesn't incur any
# real computational cost (maybe 30ms) and it simplifies adding
# the orientation information below (as opposed to using
# sympy's Tx.jacobian method)
J = []
# calculate derivative of (x,y,z) wrt to each joint
for ii in range(self.N_JOINTS):
J.append([])
J[ii].append(Tx[0].diff(self.q[ii])) # dx/dq[ii]
J[ii].append(Tx[1].diff(self.q[ii])) # dy/dq[ii]
J[ii].append(Tx[2].diff(self.q[ii])) # dz/dq[ii]
end_point = name.strip('link').strip('joint')
end_point = self.N_JOINTS if 'EE' in end_point else end_point
end_point = min(int(end_point) + 1, self.N_JOINTS)
# add on the orientation information up to the last joint
for ii in range(end_point):
J[ii] = J[ii] + list(self.J_orientation[ii])
# fill in the rest of the joints orientation info with 0
for ii in range(end_point, self.N_JOINTS):
J[ii] = J[ii] + [0, 0, 0]
J = sp.Matrix(J).T # correct the orientation of J
# save to file
os_utils.makedirs('%s/%s' % (self.config_folder, filename))
cloudpickle.dump(
J,
open('%s/%s/%s' % (self.config_folder, filename, filename),
'wb'))
if lambdify is False:
# if should return expression not function
return J
if J_func is None:
J_func = self._generate_and_save_function(filename=filename,
expression=J,
parameters=self.q +
self.x)
return J_func
def __init__(self, hand_attached=False, **kwargs):
self.hand_attached = hand_attached
N_LINKS = 7 if hand_attached is True else 6
super(Config, self).__init__(N_JOINTS=6,
N_LINKS=N_LINKS,
ROBOT_NAME='jaco2',
**kwargs)
if self.MEANS is None:
self.MEANS = { # expected mean of joint angles / velocities
'q':
np.ones(self.N_JOINTS) * np.pi,
'dq':
np.array(
[-0.01337, 0.00192, 0.00324, 0.02502, -0.02226, -0.01342])
}
if self.SCALES is None:
self.SCALES = { # expected variance of joint angles / velocities
'q':
np.ones(self.N_JOINTS) * np.pi * np.sqrt(self.N_JOINTS),
'dq':
(np.array([1.22826, 2.0, 1.42348, 2.58221, 2.50768, 1.27004]) *
np.sqrt(self.N_JOINTS))
}
self._T = {} # dictionary for storing calculated transforms
# set up saved functions folder to be in the abr_jaco repo
self.config_folder += ('with_hand'
if self.hand_attached is True else 'no_hand')
# make config folder if it doesn't exist
os_utils.makedirs(self.config_folder)
self.JOINT_NAMES = ['joint%i' % ii for ii in range(self.N_JOINTS)]
self.REST_ANGLES = np.array([None, 2.42, 2.42, 0.0, 0.0, 0.0],
dtype='float32')
# inertia values in VREP are divided by mass, account for that here
self._M_LINKS = [
sp.diag(0.5, 0.5, 0.5, 0.02, 0.02, 0.02), # link0
sp.diag(0.5, 0.5, 0.5, 0.02, 0.02, 0.02), # link1
sp.diag(0.5, 0.5, 0.5, 0.02, 0.02, 0.02), # link2
sp.diag(0.5, 0.5, 0.5, 0.02, 0.02, 0.02), # link3
sp.diag(0.5, 0.5, 0.5, 0.02, 0.02, 0.02), # link3
sp.diag(0.5, 0.5, 0.5, 0.02, 0.02, 0.02), # link4
sp.diag(0.25, 0.25, 0.25, 0.01, 0.01, 0.01)
] # link5
if self.hand_attached is True:
self._M_LINKS.append(sp.diag(0.37, 0.37, 0.37, 0.04, 0.04,
0.04)) # link6
# the joints don't weigh anything in VREP
self._M_JOINTS = [sp.zeros(6, 6) for ii in range(self.N_JOINTS)]
# ignoring lengths < 1e-6
self.L = [
[0.0, 0.0, 7.8369e-02], # link 0 offset
[-3.2712e-05, -1.7324e-05, 7.8381e-02], # joint 0 offset
[2.1217e-05, 4.8455e-05, -7.9515e-02], # link 1 offset
[-2.2042e-05, 1.3245e-04, -3.8863e-02], # joint 1 offset
[-1.9519e-03, 2.0902e-01, -2.8839e-02], # link 2 offset
[-2.3094e-02, -1.0980e-06, 2.0503e-01], # joint 2 offset
[-4.8786e-04, -8.1945e-02, -1.2931e-02], # link 3 offset
[2.5923e-04, -3.8935e-03, -1.2393e-01], # joint 3 offset
[-4.0053e-04, 1.2581e-02, -3.5270e-02], # link 4 offset
[-2.3603e-03, -4.8662e-03, 3.7097e-02], # joint 4 offset
[-5.2974e-04, 1.2272e-02, -3.5485e-02], # link 5 offset
[-1.9534e-03, 5.0298e-03, -3.7176e-02]
] # joint 5 offset
if self.hand_attached is True: # add in hand offset
self.L.append([0.0, 0.0, 0.0]) # offset for the end of fingers
self.L = np.array(self.L)
if self.hand_attached is True: # add in hand offset
self.L_HANDCOM = np.array([0.0, 0.0, -0.08]) # com of the hand
# ---- Transform Matrices ----
# Transform matrix : origin -> link 0
# no change of axes, account for offsets
self.Torgl0 = sp.Matrix([[1, 0, 0, self.L[0, 0]],
[0, 1, 0, self.L[0, 1]],
[0, 0, 1, self.L[0, 2]], [0, 0, 0, 1]])
# Transform matrix : link0 -> joint 0
# account for change of axes and offsets
self.Tl0j0 = sp.Matrix([[1, 0, 0, self.L[1, 0]],
[0, -1, 0, self.L[1, 1]],
[0, 0, -1, self.L[1, 2]], [0, 0, 0, 1]])
# Transform matrix : joint 0 -> link 1
# account for rotations due to q
self.Tj0l1a = sp.Matrix([[sp.cos(self.q[0]), -sp.sin(self.q[0]), 0, 0],
[sp.sin(self.q[0]),
sp.cos(self.q[0]), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# account for change of axes and offsets
self.Tj0l1b = sp.Matrix([[-1, 0, 0, self.L[2, 0]],
[0, -1, 0, self.L[2, 1]],
[0, 0, 1, self.L[2, 2]], [0, 0, 0, 1]])
self.Tj0l1 = self.Tj0l1a * self.Tj0l1b
# Transform matrix : link 1 -> joint 1
# account for axes rotation and offset
self.Tl1j1 = sp.Matrix([[1, 0, 0, self.L[3, 0]],
[0, 0, -1, self.L[3, 1]],
[0, 1, 0, self.L[3, 2]], [0, 0, 0, 1]])
# Transform matrix : joint 1 -> link 2
# account for rotations due to q
self.Tj1l2a = sp.Matrix([[sp.cos(self.q[1]), -sp.sin(self.q[1]), 0, 0],
[sp.sin(self.q[1]),
sp.cos(self.q[1]), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# account for axes rotation and offsets
self.Tj1l2b = sp.Matrix([[0, -1, 0, self.L[4, 0]],
[0, 0, 1, self.L[4, 1]],
[-1, 0, 0, self.L[4, 2]], [0, 0, 0, 1]])
self.Tj1l2 = self.Tj1l2a * self.Tj1l2b
# Transform matrix : link 2 -> joint 2
# account for axes rotation and offsets
self.Tl2j2 = sp.Matrix([[0, 0, 1, self.L[5,
0]], [1, 0, 0, self.L[5, 1]],
[0, 1, 0, self.L[5, 2]], [0, 0, 0, 1]])
# Transform matrix : joint 2 -> link 3
# account for rotations due to q
self.Tj2l3a = sp.Matrix([[sp.cos(self.q[2]), -sp.sin(self.q[2]), 0, 0],
[sp.sin(self.q[2]),
sp.cos(self.q[2]), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# account for axes rotation and offsets
self.Tj2l3b = sp.Matrix([[0.14262926, -0.98977618, 0, self.L[6, 0]],
[0, 0, 1, self.L[6, 1]],
[-0.98977618, -0.14262926, 0, self.L[6, 2]],
[0, 0, 0, 1]])
self.Tj2l3 = self.Tj2l3a * self.Tj2l3b
# Transform matrix : link 3 -> joint 3
# account for axes change and offsets
self.Tl3j3 = sp.Matrix([[-0.14262861, -0.98977628, 0, self.L[7, 0]],
[0.98977628, -0.14262861, 0, self.L[7, 1]],
[0, 0, 1, self.L[7, 2]], [0, 0, 0, 1]])
# Transform matrix: joint 3 -> link 4
# account for rotations due to q
self.Tj3l4a = sp.Matrix([[sp.cos(self.q[3]), -sp.sin(self.q[3]), 0, 0],
[sp.sin(self.q[3]),
sp.cos(self.q[3]), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# account for axes and rotation and offsets
self.Tj3l4b = sp.Matrix(
[[0.85536427, -0.51802699, 0, self.L[8, 0]],
[-0.45991232, -0.75940555, 0.46019982, self.L[8, 1]],
[-0.23839593, -0.39363848, -0.88781537, self.L[8, 2]],
[0, 0, 0, 1]])
self.Tj3l4 = self.Tj3l4a * self.Tj3l4b
# Transform matrix: link 4 -> joint 4
# no axes change, account for offsets
self.Tl4j4 = sp.Matrix(
[[-0.855753802, 0.458851168, 0.239041914, self.L[9, 0]],
[0.517383113, 0.758601438, 0.396028500, self.L[9, 1]],
[0, 0.462579144, -0.886577910, self.L[9, 2]], [0, 0, 0, 1]])
# Transform matrix: joint 4 -> link 5
# account for rotations due to q
self.Tj4l5a = sp.Matrix([[sp.cos(self.q[4]), -sp.sin(self.q[4]), 0, 0],
[sp.sin(self.q[4]),
sp.cos(self.q[4]), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
# account for axes and rotation and offsets
# no axes change, account for offsets
self.Tj4l5b = sp.Matrix(
[[0.89059413, 0.45479896, 0, self.L[10, 0]],
[-0.40329059, 0.78972966, -0.46225942, self.L[10, 1]],
[-0.2102351, 0.41168552, 0.88674474, self.L[10, 2]], [0, 0, 0,
1]])
self.Tj4l5 = self.Tj4l5a * self.Tj4l5b
# Transform matrix : link 5 -> joint 5
# account for axes change and offsets
self.Tl5j5 = sp.Matrix(
[[-0.890598824, 0.403618758, 0.209584432, self.L[11, 0]],
[-0.454789710, -0.790154512, -0.410879747, self.L[11, 1]],
[0, -0.461245863, 0.887272337, self.L[11, 2]], [0, 0, 0, 1]])
if self.hand_attached is True: # add in hand offset
# Transform matrix: joint 5 -> link 6 / hand COM
# account for rotations due to q
self.Tj5handcoma = sp.Matrix(
[[sp.cos(self.q[5]), -sp.sin(self.q[5]), 0, 0],
[sp.sin(self.q[5]),
sp.cos(self.q[5]), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
# account for axes changes and offsets
self.Tj5handcomb = sp.Matrix([[-1, 0, 0, self.L_HANDCOM[0]],
[0, 1, 0, self.L_HANDCOM[1]],
[0, 0, -1, self.L_HANDCOM[2]],
[0, 0, 0, 1]])
self.Tj5handcom = self.Tj5handcoma * self.Tj5handcomb
# no axes change, account for offsets
self.Thandcomfingers = sp.Matrix([[1, 0, 0, self.L[12, 0]],
[0, 1, 0, self.L[12, 1]],
[0, 0, 1, self.L[12, 2]],
[0, 0, 0, 1]])
# orientation part of the Jacobian (compensating for angular velocity)
self.J_orientation = [
self._calc_T('joint0')[:3, :3] * self._KZ, # joint 0 orientation
self._calc_T('joint1')[:3, :3] * self._KZ, # joint 1 orientation
self._calc_T('joint2')[:3, :3] * self._KZ, # joint 2 orientation
self._calc_T('joint3')[:3, :3] * self._KZ, # joint 3 orientation
self._calc_T('joint4')[:3, :3] * self._KZ, # joint 4 orientation
self._calc_T('joint5')[:3, :3] * self._KZ
] # joint 5 orientation
