def manipulability(self, q=None, J=None):
"""
Calculates the manipulability index (scalar) robot at the joint
configuration q. It indicates dexterity, that is, how isotropic the
robot's motion is with respect to the 6 degrees of Cartesian motion.
The measure is high when the manipulator is capable of equal motion
in all directions and low when the manipulator is close to a
singularity. One of J or q is required. Supply J if already
calculated to save computation time
:param q: The joint coordinates of the robot
:type q: float np.ndarray(n,)
:param J: The manipulator Jacobian in any frame
:type J: float np.ndarray(6,n)
:return: The manipulability index
:rtype: float
References: Analysis and control of robot manipulators with redundancy,
T. Yoshikawa,
Robotics Research: The First International Symposium (M. Brady and
R. Paul, eds.), pp. 735-747, The MIT press, 1984.
"""
if J is None:
if q is not None:
q = getvector(q, self.n)
J = self.jacob0(q)
else:
raise ValueError('One of q or J must be supplied')
else:
verifymatrix(J, (6, self.n))
return np.sqrt(np.linalg.det(J @ np.transpose(J)))
def hessian0(self, q=None, J0=None, endlink=None, startlink=None):
"""
The manipulator Hessian tensor maps joint acceleration to end-effector
spatial acceleration, expressed in the world-coordinate frame. This
function calulcates this based on the ETS of the robot. One of J0 or q
is required. Supply J0 if already calculated to save computation time
: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 J0: The manipulator Jacobian in the 0 frame
:type J0: float ndarray(6,n)
:param endlink: the final link which the Hessian represents
:type endlink: str or ELink
:param startlink: the first link which the Hessian represents
:type startlink: str or ELink
:return: The manipulator Hessian in 0 frame
:rtype: float ndarray(6,n,n)
H[i,j,k] is d2 u_i / dq_j dq_k
where u = {t_x, t_y, t_z, r_x, r_y, r_z}
J[i,j] is d u_i / dq_j
where u = {t_x, t_y, t_z, ζ_x, ζ_y, ζ_z}
v = J qd
a = Jd qd + J qdd
Jd = H qd
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
"""
endlink, startlink = self._get_limit_links(endlink, startlink)
path, n = self.get_path(endlink, startlink)
if J0 is None:
q = getvector(q, n)
J0 = self.jacob0(q, endlink=endlink)
else:
verifymatrix(J0, (6, n))
H = np.zeros((6, n, n))
for j in range(n):
for i in range(j, n):
H[:3, i, j] = np.cross(J0[3:, j], J0[:3, i])
H[3:, i, j] = np.cross(J0[3:, j], J0[3:, i])
if i != j:
H[:3, j, i] = H[:3, i, j]
return H
def _plot(robot,
block,
q,
dt,
limits=None,
vellipse=False,
fellipse=False,
jointaxes=True,
eeframe=True,
shadow=True,
name=True):
# Make an empty 3D figure
env = rp.backend.PyPlot()
trajn = 1
if q is None:
q = robot.q
try:
q = getvector(q, robot.n, 'col')
robot.q = q
except ValueError:
trajn = q.shape[1]
verifymatrix(q, (robot.n, trajn))
# Add the robot to the figure in readonly mode
if trajn == 1:
env.launch(robot.name + ' Plot', limits)
else:
env.launch(robot.name + ' Trajectory Plot', limits)
env.add(robot,
readonly=True,
jointaxes=jointaxes,
eeframe=eeframe,
shadow=shadow,
name=name)
if vellipse:
vell = robot.vellipse(centre='ee')
env.add(vell)
if fellipse:
fell = robot.fellipse(centre='ee')
env.add(fell)
if trajn != 1:
for i in range(trajn):
robot.q = q[:, i]
env.step()
time.sleep(dt / 1000)
# Keep the plot open
if block: # pragma: no cover
env.hold()
return env
def jacobm(self, q=None, J=None, H=None, endlink=None, startlink=None):
"""
Calculates the manipulability Jacobian. This measure relates the rate
of change of the manipulability to the joint velocities of the robot.
One of J or q is required. Supply J and H if already calculated to
save computation time
: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 J: The manipulator Jacobian in any frame
:type J: float ndarray(6,n)
:param H: The manipulator Hessian in any frame
:type H: float ndarray(6,n,n)
:param endlink: the final link which the Hessian represents
:type endlink: str or ELink
:param startlink: the first link which the Hessian represents
:type startlink: str or ELink
:return: The manipulability Jacobian
:rtype: float ndarray(n)
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
"""
endlink, startlink = self._get_limit_links(endlink, startlink)
path, n = self.get_path(endlink, startlink)
if J is None:
if q is None:
q = np.copy(self.q)
else:
q = getvector(q, n)
J = self.jacob0(q, startlink=startlink, endlink=endlink)
else:
verifymatrix(J, (6, n))
if H is None:
H = self.hessian0(J0=J, startlink=startlink, endlink=endlink)
else:
verifymatrix(H, (6, n, n))
manipulability = self.manipulability(q,
J=J,
startlink=startlink,
endlink=endlink)
b = np.linalg.inv(J @ np.transpose(J))
Jm = np.zeros((n, 1))
for i in range(n):
c = J @ np.transpose(H[:, :, i])
Jm[i, 0] = manipulability * \
np.transpose(c.flatten('F')) @ b.flatten('F')
return Jm
def gravload(self, q=None, grav=None):
"""
Compute gravity load
:param q: The joint angles/configuration of the robot
:type q: float ndarray(n)
:param grav: The gravity vector (Optional, if not supplied will
use the stored gravity values).
:type grav: float ndarray(3)
:return taug: The generalised joint force/torques due to gravity
:rtype taug: float ndarray(n)
``taug = gravload(q)`` calculates the joint gravity loading (n) for
the robot in the joint configuration ``q`` and using the default
gravitational acceleration specified in the SerialLink object.
``taug = gravload(q, grav)`` as above except the gravitational
acceleration is explicitly specified as `grav``.
If q is a matrix (nxm) each column is interpreted as a joint
configuration vector, and the result is a matrix (nxm) each column
being the corresponding joint torques.
"""
trajn = 1
if q is None:
q = self.q
if grav is None:
grav = getvector(np.copy(self.gravity), 3, 'row')
try:
q = getvector(q, self.n, 'row')
grav = getvector(grav, 3, 'row')
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
if grav.shape[0] < trajn:
grav = (grav.T @ np.ones((1, trajn))).T
verifymatrix(grav, (trajn, 3))
taug = np.zeros((trajn, self.n))
for i in range(trajn):
taug[i, :] = self.rne(q[i, :], np.zeros(self.n), np.zeros(self.n),
grav[i, :])
if trajn == 1:
return taug[0, :]
else:
return taug
def inertia(self, q=None):
"""
SerialLink.INERTIA Manipulator inertia matrix
I = inertia(q) is the symmetric joint inertia matrix (nxn) which
relates joint torque to joint acceleration for the robot at joint
configuration q.
I = inertia() as above except uses the stored q value of the robot
object.
If q is a matrix (nxk), each row is interpretted as a joint state
vector, and the result is a 3d-matrix (nxnxk) where each plane
corresponds to the inertia for the corresponding row of q.
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:return I: The inertia matrix
:rtype I: float ndarray(n,n)
:notes:
- The diagonal elements I(J,J) are the inertia seen by joint
actuator J.
- The off-diagonal elements I(J,K) are coupling inertias that
relate acceleration on joint J to force/torque on joint K.
- The diagonal terms include the motor inertia reflected through
the gear ratio.
"""
trajn = 1
try:
q = getvector(q, self.n, 'row')
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
In = np.zeros((trajn, self.n, self.n))
for i in range(trajn):
In[i, :, :] = self.rne((np.c_[q[i, :]] @ np.ones((1, self.n))).T,
np.zeros((self.n, self.n)),
np.eye(self.n),
grav=[0, 0, 0])
if trajn == 1:
return In[0, :, :]
else:
return In
def hessian0(self, q=None, J0=None, from_link=None, to_link=None):
"""
The manipulator Hessian tensor maps joint acceleration to end-effector
spatial acceleration, expressed in the world-coordinate frame. This
function calulcates this based on the ETS of the robot. One of J0 or q
is required. Supply J0 if already calculated to save computation time
: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 J0: The manipulator Jacobian in the 0 frame
:type J0: float ndarray(6,n)
:return: The manipulator Hessian in 0 frame
:rtype: float ndarray(6,n,n)
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
"""
if from_link is None:
from_link = self.base_link
if to_link is None:
to_link = self.ee_links[0]
path, n = self.get_path(from_link, to_link)
if J0 is None:
if q is None:
q = np.copy(self.q)
else:
q = getvector(q, n)
J0 = self.jacob0(q, from_link, to_link)
else:
verifymatrix(J0, (6, n))
H = np.zeros((6, n, n))
for j in range(n):
for i in range(j, n):
H[:3, i, j] = np.cross(J0[3:, j], J0[:3, i])
H[3:, i, j] = np.cross(J0[3:, j], J0[3:, i])
if i != j:
H[:3, j, i] = H[:3, i, j]
return H
def itorque(self, q, qdd):
"""
Inertia torque
:param qdd: The joint accelerations of the robot
:type qdd: float ndarray(n)
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:return taui: The inertia torque vector
:rtype taui: float ndarray(n)
``tauI = itorque(q, qdd)`` is the inertia force/torque vector (n) at
the specified joint configuration q (n) and acceleration qdd (n), and
n is the number of robot joints. taui = inertia(q) * qdd.
If q and qdd are matrices (nxk), each row is interpretted as a joint
state vector, and the result is a matrix (nxk) where each row is the
corresponding joint torques.
:notes:
- If the robot model contains non-zero motor inertia then this
will included in the result.
"""
trajn = 1
try:
q = getvector(q, self.n, 'row')
qdd = getvector(qdd, self.n, 'row')
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
verifymatrix(qdd, (trajn, self.n))
taui = np.zeros((trajn, self.n))
for i in range(trajn):
taui[i, :] = self.rne(q[i, :],
np.zeros(self.n),
qdd[i, :],
grav=[0, 0, 0])
if trajn == 1:
return taui[0, :]
else:
return taui
def cinertia(self, q=None):
"""
M = cinertia(q) is the nxn Cartesian (operational space) inertia
matrix which relates Cartesian force/torque to Cartesian
acceleration at the joint configuration q.
M = cinertia() as above except uses the stored q value of the robot
object.
If q is a matrix (nxk), each row is interpretted as a joint state
vector, and the result is a 3d-matrix (nxnxk) where each plane
corresponds to the cinertia for the corresponding row of q.
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:return M: The inertia matrix
:rtype M: float ndarray(n,n)
"""
trajn = 1
if q is None:
q = self.q
try:
q = getvector(q, self.n, 'row')
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
Mt = np.zeros((trajn, self.n, self.n))
for i in range(trajn):
J = self.jacob0(q[i, :])
Ji = np.linalg.pinv(J)
M = self.inertia(q[i, :])
Mt[i, :, :] = Ji.T @ M @ Ji
if trajn == 1:
return Mt[0, :, :]
else:
return Mt
def manipulability(self, q=None, J=None, from_link=None, to_link=None):
"""
Calculates the manipulability index (scalar) robot at the joint
configuration q. It indicates dexterity, that is, how isotropic the
robot's motion is with respect to the 6 degrees of Cartesian motion.
The measure is high when the manipulator is capable of equal motion
in all directions and low when the manipulator is close to a
singularity. One of J or q is required. Supply J if already
calculated to save computation time
: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 J: The manipulator Jacobian in any frame
:type J: float ndarray(6,n)
:return: The manipulability index
:rtype: float
:references:
- Analysis and control of robot manipulators with redundancy,
T. Yoshikawa,
- Robotics Research: The First International Symposium (M. Brady
and R. Paul, eds.), pp. 735-747, The MIT press, 1984.
"""
if from_link is None:
from_link = self.base_link
if to_link is None:
to_link = self.ee_links[0]
path, n = self.get_path(from_link, to_link)
if J is None:
if q is None:
q = np.copy(self.q)
else:
q = getvector(q, n)
J = self.jacob0(q, from_link, to_link)
else:
verifymatrix(J, (6, n))
return np.sqrt(np.linalg.det(J @ np.transpose(J)))
def jacobm(self, q=None, J=None, H=None):
"""
Calculates the manipulability Jacobian. This measure relates the rate
of change of the manipulability to the joint velocities of the robot.
One of J or q is required. Supply J and H if already calculated to
save computation time
:param q: The joint coordinates of the robot
:type q: float np.ndarray(n,)
:param J: The manipulator Jacobian in any frame
:type J: float np.ndarray(6,n)
:param H: The manipulator Hessian in any frame
:type H: float np.ndarray(6,n,n)
:return: The manipulability Jacobian
:rtype: float np.ndarray(n,1)
References: Maximising Manipulability in Resolved-Rate Motion Control,
J. Haviland and P. Corke
"""
if J is None:
if q is not None:
q = getvector(q, self.n)
J = self.jacob0(q)
else:
raise ValueError('One of q or J must be supplied')
else:
verifymatrix(J, (6, self.n))
if H is None:
H = self.hessian0(J0=J)
else:
verifymatrix(H, (6, self.n, self.n))
manipulability = self.manipulability(J=J)
b = np.linalg.inv(J @ np.transpose(J))
Jm = np.zeros((self.n, 1))
for i in range(self.n):
c = J @ np.transpose(H[:, :, i])
Jm[i, 0] = manipulability * \
np.transpose(c.flatten('F')) @ b.flatten('F')
return Jm
def I(self, I_new): # noqa
# Try for Inertia Matrix
try:
verifymatrix(I_new, (3, 3))
except (ValueError, TypeError):
# Try for the moments and products of inertia
# [Ixx Iyy Izz Ixy Iyz Ixz]
try:
Ia = getvector(I_new, 6)
I_new = np.array([[Ia[0], Ia[3], Ia[5]], [Ia[3], Ia[1], Ia[4]],
[Ia[5], Ia[4], Ia[2]]])
except ValueError:
# Try for the moments of inertia
# [Ixx Iyy Izz]
Ia = getvector(I_new, 3)
I_new = np.diag(Ia)
self._I = I_new
def hessian0(self, q=None, J0=None):
"""
The manipulator Hessian tensor maps joint acceleration to end-effector
spatial acceleration, expressed in the world-coordinate frame. This
function calulcates this based on the ETS of the robot. One of J0 or q
is required. Supply J0 if already calculated to save computation time
:param q: The joint coordinates of the robot
:type q: float np.ndarray(n,)
:param J0: The manipulator Jacobian in the 0 frame
:type J0: float np.ndarray(6,n)
:return: The manipulator Hessian in 0 frame
:rtype: float np.ndarray(6,n,n)
References: Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
"""
if J0 is None:
if q is not None:
q = getvector(q, self.n)
J0 = self.jacob0(q)
else:
raise ValueError('One of q or J0 must be supplied')
else:
verifymatrix(J0, (6, self.n))
H = np.zeros((6, self.n, self.n))
for j in range(self.n):
for i in range(j, self.n):
H[:3, i, j] = np.cross(J0[3:, j], J0[:3, i])
H[3:, i, j] = np.cross(J0[3:, j], J0[3:, i])
if i != j:
H[:3, j, i] = H[:3, i, j]
return H
def fkine(self, q=None):
'''
Evaluates the forward kinematics of a robot based on its ETS and
joint angles q.
T = fkine(q) evaluates forward kinematics for the robot at joint
configuration q.
T = fkine() as above except uses the stored q value of the
robot object.
Trajectory operation:
Calculates fkine for each point on a trajectory of joints q where
q is (nxm) and the returning SE3 in (m)
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:return: The transformation matrix representing the pose of the
end-effector
:rtype: SE3
:notes:
- The robot's base or tool transform, if present, are incorporated
into the result.
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
'''
trajn = 1
if q is None:
q = self.q
try:
q = getvector(q, self.n, 'col')
except ValueError:
trajn = q.shape[1]
verifymatrix(q, (self.n, trajn))
for i in range(trajn):
j = 0
tr = self.base
for link in self._fkpath:
if link.jtype == link.VARIABLE:
T = link.A(q[j, i])
j += 1
else:
T = link.A()
tr = tr * T
tr = tr * self.tool
if i == 0:
t = SE3(tr)
else:
t.append(tr)
return t
def coriolis(self, q, qd):
"""
Coriolis and centripetal term
``C = coriolis(q, qd)`` calculates the Coriolis/centripetal matrix
(nxn) for the robot in configuration q and velocity qd, where n is the
number of joints. The product c*qd is the vector of joint
force/torque due to velocity coupling. The diagonal elements are due
to centripetal effects and the off-diagonal elements are due to
Coriolis effects. This matrix is also known as the velocity coupling
matrix, since it describes the disturbance forces on any joint due to
velocity of all other joints.
If q and qd are matrices (nxk), each row is interpretted as a
joint state vector, and the result (nxnxk) is a 3d-matrix where
each plane corresponds to a row of q and qd.
:notes:
- Joint viscous friction is also a joint force proportional to
velocity but it is eliminated in the computation of this value.
- Computationally slow, involves n^2/2 invocations of RNE.
"""
trajn = 1
try:
q = getvector(q, self.n, 'row')
qd = getvector(qd, self.n, 'row')
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
verifymatrix(qd, (trajn, self.n))
r1 = self.nofriction(True, True)
C = np.zeros((trajn, self.n, self.n))
Csq = np.zeros((trajn, self.n, self.n))
# Find the torques that depend on a single finite joint speed,
# these are due to the squared (centripetal) terms
# set QD = [1 0 0 ...] then resulting torque is due to qd_1^2
for j in range(trajn):
for i in range(self.n):
QD = np.zeros(self.n)
QD[i] = 1
tau = r1.rne(q[j, :], QD, np.zeros(self.n), grav=[0, 0, 0])
Csq[j, :, i] = Csq[j, :, i] + tau
# Find the torques that depend on a pair of finite joint speeds,
# these are due to the product (Coridolis) terms
# set QD = [1 1 0 ...] then resulting torque is due to
# qd_1 qd_2 + qd_1^2 + qd_2^2
for k in range(trajn):
for i in range(self.n):
for j in range(i + 1, self.n):
# Find a product term qd_i * qd_j
QD = np.zeros(self.n)
QD[i] = 1
QD[j] = 1
tau = r1.rne(q[k, :], QD, np.zeros(self.n), grav=[0, 0, 0])
C[k, :, j] = C[k, :, j] + \
(tau - Csq[k, :, j] - Csq[k, :, i]) * qd[k, i] / 2
C[k, :, i] = C[k, :, i] + \
(tau - Csq[k, :, j] - Csq[k, :, i]) * qd[k, j] / 2
C[k, :, :] = C[k, :, :] + Csq[k, :, :] @ np.diag(qd[k, :])
if trajn == 1:
return C[0, :, :]
else:
return C
def pay(self, W, q=None, J=None, frame=1):
"""
tau = pay(W, J) Returns the generalised joint force/torques due to a
payload wrench W applied to the end-effector. Where the manipulator
Jacobian is J (6xn), and n is the number of robot joints.
tau = pay(W, q, frame) as above but the Jacobian is calculated at pose
q in the frame given by frame which is 0 for base frame, 1 for
end-effector frame.
Uses the formula tau = J'W, where W is a wrench vector applied at the
end effector, W = [Fx Fy Fz Mx My Mz]'.
Trajectory operation:
In the case q is nxm or J is 6xnxm then tau is nxm where each row
is the generalised force/torque at the pose given by corresponding
row of q.
:param W: A wrench vector applied at the end effector,
W = [Fx Fy Fz Mx My Mz]
:type W: float ndarray(6)
: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 J: The manipulator Jacobian (Optional, if not supplied will
use the q value).
:type J: float ndarray(6,n)
:param frame: The frame in which to torques are expressed in when J
is not supplied. 0 means base frame of the robot, 1 means end-
effector frame
:type frame: int
:return tau: The joint forces/torques due to w
:rtype tau: float ndarray(n)
:notes:
- Wrench vector and Jacobian must be from the same reference
frame.
- Tool transforms are taken into consideration when frame=1.
- Must have a constant wrench - no trajectory support for this
yet.
"""
try:
W = getvector(W, 6)
trajn = 0
except ValueError:
trajn = W.shape[0]
verifymatrix(W, (trajn, 6))
if trajn:
# A trajectory
if J is not None:
# Jacobian supplied
verifymatrix(J, (trajn, 6, self.n))
else:
# Use q instead
verifymatrix(q, (trajn, self.n))
J = np.zeros((trajn, 6, self.n))
for i in range(trajn):
if frame:
J[i, :, :] = self.jacobe(q[i, :])
else:
J[i, :, :] = self.jacob0(q[i, :])
else:
# Single configuration
if J is not None:
# Jacobian supplied
verifymatrix(J, (6, self.n))
else:
# Use q instead
if q is None:
q = np.copy(self.q)
else:
q = getvector(q, self.n)
if frame:
J = self.jacobe(q)
else:
J = self.jacob0(q)
if trajn == 0:
tau = -J.T @ W
else:
tau = np.zeros((trajn, self.n))
for i in range(trajn):
tau[i, :] = -J[i, :, :].T @ W[i, :]
return tau
def accel(self, q, qd, torque):
"""
Compute acceleration due to applied torque
: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)
:param torque: The joint torques of the robot
:type torque: float ndarray(n)
``qdd = accel(q, qd, torque)`` calculates a vector (n) of joint
accelerations that result from applying the actuator force/torque (n)
to the manipulator in state q (n) and qd (n), and n is the number of
robot joints.
If q, qd, torque are matrices (nxk) then qdd is a matrix (nxk) where
each row is the acceleration corresponding to the equivalent cols of
q, qd, torque.
:return qdd: The joint accelerations of the robot
:rtype qdd: float ndarray(n)
:notes:
- Useful for simulation of manipulator dynamics, in
conjunction with a numerical integration function.
- Uses the method 1 of Walker and Orin to compute the forward
dynamics.
- Featherstone's method is more efficient for robots with large
numbers of joints.
- Joint friction is considered.
:references:
- Efficient dynamic computer simulation of robotic mechanisms,
M. W. Walker and D. E. Orin,
ASME Journa of Dynamic Systems, Measurement and Control, vol.
104, no. 3, pp. 205-211, 1982.
"""
trajn = 1
try:
q = getvector(q, self.n, 'row')
qd = getvector(qd, self.n, 'row')
torque = getvector(torque, self.n, 'row')
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
verifymatrix(qd, (trajn, self.n))
verifymatrix(torque, (trajn, self.n))
qdd = np.zeros((trajn, self.n))
for i in range(trajn):
# Compute current manipulator inertia torques resulting from unit
# acceleration of each joint with no gravity.
qI = (np.c_[q[i, :]] @ np.ones((1, self.n))).T
qdI = np.zeros((self.n, self.n))
qddI = np.eye(self.n)
m = self.rne(qI, qdI, qddI, grav=[0, 0, 0])
# Compute gravity and coriolis torque torques resulting from zero
# acceleration at given velocity & with gravity acting.
tau = self.rne(q[i, :], qd[i, :], np.zeros((1, self.n)))
inter = np.expand_dims((torque[i, :] - tau), axis=1)
qdd[i, :] = (np.linalg.inv(m) @ inter).flatten()
if trajn == 1:
return qdd[0, :]
else:
return qdd
def paycap(self, w, tauR, frame=1, q=None):
"""
Static payload capacity of a robot
:param w: The payload wrench
:type w: float ndarray(n)
:param tauR: Joint torque matrix minimum and maximums
:type tauR: float ndarray(n,2)
:param frame: The frame in which to torques are expressed in when J
is not supplied. 'base' means base frame of the robot, 'ee' means
end-effector frame
:type frame: str
:param q: The joint angles/configuration of the robot.
:type q: float ndarray(n)
:return wmax: The maximum permissible payload wrench
:rtype wmax: float ndarray(6)
:return joint: The joint index (zero indexed) which hits its
force/torque limit
:rtype joint: int
``wmax, joint = paycap(q, w, f, tauR)`` returns the maximum permissible
payload wrench ``wmax`` (6) applied at the end-effector, and the index
of the joint (zero indexed) which hits its force/torque limit at that
wrench. ``q`` (n) is the manipulator pose, ``w`` the payload wrench
(6), ``f`` the wrench reference frame and tauR (nx2) is a matrix of
joint forces/torques (first col is maximum, second col minimum).
Trajectory operation:
In the case q is nxm then wmax is Mx6 and J is Mx1 where the rows are
the results at the pose given by corresponding row of q.
:notes:
- Wrench vector and Jacobian must be from the same reference frame
- Tool transforms are taken into consideration for frame=1.
"""
trajn = 1
if q is None:
q = self.q
try:
q = getvector(q, self.n, 'row')
w = getvector(w, 6, 'row')
except ValueError:
trajn = q.shape[1]
verifymatrix(q, (trajn, self.n))
verifymatrix(w, (trajn, 6))
verifymatrix(tauR, (self.n, 2))
wmax = np.zeros((trajn, 6))
joint = np.zeros(trajn, dtype=np.int)
for i in range(trajn):
tauB = self.gravload(q[i, :])
# tauP = self.rne(
# np.zeros(self.n), np.zeros(self.n),
# q, grav=[0, 0, 0], fext=w/np.linalg.norm(w))
tauP = self.pay(w[i, :] / np.linalg.norm(w[i, :]),
q=q[i, :],
frame=frame)
M = tauP > 0
m = tauP <= 0
TAUm = np.ones(self.n)
TAUM = np.ones(self.n)
for c in range(self.n):
TAUM[c] = tauR[c, 0]
TAUm[c] = tauR[c, 1]
WM = np.zeros(self.n)
WM[M] = (TAUM[M] - tauB[M]) / tauP[M]
WM[m] = (TAUm[m] - tauB[m]) / tauP[m]
WM[WM == np.NINF] = np.Inf
wmax[i, :] = WM
joint[i] = np.argmin(WM)
if trajn == 1:
return wmax[0, :], joint[0]
else:
return wmax, joint
def ikine(self,
T,
ilimit=500,
rlimit=100,
tol=1e-10,
Y=0.1,
Ymin=0,
mask=None,
q0=None,
search=False,
slimit=100,
transpose=None):
"""
Numerical inverse kinematics by optimization (Robot superclass)
:param T: The desired end-effector pose
:type T: SE3 with 1 or ``m`` values
:param ilimit: maximum number of iterations
:type ilimit: int (default 500)
:param rlimit: maximum number of consecutive step rejections
:type rlimit: int (default 100)
:param tol: final error tolerance
:type tol: float (default 1e-10)
:param Y: initial value of lambda
:type Y: float (default 0.1)
:param Ymin: minimum allowable value of lambda
:type Ymin: float (default 0)
:param mask: mask vector that correspond to translation in X, Y and Z
and rotation about X, Y and Z respectively.
:type mask: float ndarray(6)
:param q0: initial joint configuration (default all zeros)
:type q0: float ndarray(n) (default all zeros)
:param search: search over all configurations
:type search: bool
:param slimit: maximum number of search attempts
:type slimit: int (default 100)
:param transpose: use Jacobian transpose with step size A, rather
than Levenberg-Marquadt
:type transpose: float
:return: The calculated joint values
:rtype: float ndarray(n)
:return: IK solver failed
:rtype: bool or list of bool
:return: If failed, what went wrong
:rtype: List of str
``q, failure, reason = ikine(T)`` are the joint coordinates (n)
corresponding to the robot end-effector pose ``T`` which is an ``SE3``
instance. ``failure`` is True if the solver failed, and ``reason``
contains details of the failure.
This method can be used for robots with any number of degrees of
freedom.
**Trajectory operation:**
If ``T`` contains ``m`` > 1 values, ie. a trajectory, then returns the
joint coordinates corresponding to each of the pose values in ``T``.
``q`` (m,n) where ``n`` is the number of robot joints. The initial
estimate of ``q`` for each time step is taken as the solution from the
previous time step. Returns trajectory of joints ``q`` (m,n), list of
failure (m) and list of error reasons (m).
**Underactuated robots:**
For the case where the manipulator has fewer than 6 DOF the
solution space has more dimensions than can be spanned by the
manipulator joint coordinates.
In this case we specify the ``mask`` option where the ``mask`` vector (6)
specifies the Cartesian DOF (in the wrist coordinate frame) that will
be ignored in reaching a solution. The mask vector has six elements
that correspond to translation in X, Y and Z, and rotation about X, Y
and Z respectively. The value should be 0 (for ignore) or 1. The
number of non-zero elements should equal the number of manipulator
DOF.
For example when using a 3 DOF manipulator rotation orientation might
be unimportant in which case use the option: mask = [1 1 1 0 0 0].
For robots with 4 or 5 DOF this method is very difficult to use since
orientation is specified by T in world coordinates and the achievable
orientations are a function of the tool position.
.. note::
- Solution is computed iteratively.
- Implements a Levenberg-Marquadt variable step size solver.
- The tolerance is computed on the norm of the error between
current and desired tool pose. This norm is computed from
distances and angles without any kind of weighting.
- The inverse kinematic solution is generally not unique, and
depends on the initial guess q0 (defaults to 0).
- The default value of q0 is zero which is a poor choice for most
manipulators (eg. puma560, twolink) since it corresponds to a
kinematic singularity.
- Such a solution is completely general, though much less
efficient than specific inverse kinematic solutions derived
symbolically, like ikine6s or ikine3.
- This approach allows a solution to be obtained at a singularity,
but the joint angles within the null space are arbitrarily
assigned.
- Joint offsets, if defined, are added to the inverse kinematics
to generate q.
- Joint limits are not considered in this solution.
- The 'search' option peforms a brute-force search with initial
conditions chosen from the entire configuration space.
- If the search option is used any prismatic joint must have
joint limits defined.
:references:
- Robotics, Vision & Control, P. Corke, Springer 2011,
Section 8.4.
"""
if not isinstance(T, SE3):
T = SE3(T)
trajn = len(T)
err = []
try:
if q0 is not None:
if trajn == 1:
q0 = getvector(q0, self.n, 'row')
else:
verifymatrix(q0, (trajn, self.n))
else:
q0 = np.zeros((trajn, self.n))
except ValueError:
verifymatrix(q0, (trajn, self.n))
if mask is not None:
mask = getvector(mask, 6)
else:
mask = np.ones(6)
if search:
# Randomised search for a starting point
search = False
# quiet = True
qlim = self.qlim
qspan = qlim[1] - qlim[0] # range of joint motion
for k in range(slimit):
q0n = np.random.rand(self.n) * qspan + qlim[0, :]
# fprintf('Trying q = %s\n', num2str(q))
q, _, _ = self.ikine(T, ilimit, rlimit, tol, Y, Ymin, mask,
q0n, search, slimit, transpose)
if not np.sum(np.abs(q)) == 0:
return q, True, err
q = np.array([])
return q, False, err
if not self.n >= np.sum(mask):
raise ValueError('Number of robot DOF must be >= the same number '
'of 1s in the mask matrix')
W = np.diag(mask)
# Preallocate space for results
qt = np.zeros((len(T), self.n))
# Total iteration count
tcount = 0
# Rejected step count
rejcount = 0
failed = []
nm = 0
revolutes = []
for i in range(self.n):
revolutes.append(not self.links[i].sigma)
for i in range(len(T)):
iterations = 0
q = np.copy(q0[i, :])
Yl = Y
while True:
# Update the count and test against iteration limit
iterations += 1
if iterations > ilimit:
err.append('ikine: iteration limit {0} exceeded '
' (pose {1}), final err {2}'.format(
ilimit, i, nm))
failed.append(True)
break
e = tr2delta(self.fkine(q).A, T[i].A)
# Are we there yet
if np.linalg.norm(W @ e) < tol:
# print(iterations)
failed.append(False)
break
# Compute the Jacobian
J = self.jacobe(q)
JtJ = J.T @ W @ J
if transpose is not None:
# Do the simple Jacobian transpose with constant gain
dq = transpose * J.T @ e # lgtm [py/multiple-definition]
else:
# Do the damped inverse Gauss-Newton with
# Levenberg-Marquadt
dq = np.linalg.inv(JtJ + (
(Yl + Ymin) * np.eye(self.n))) @ J.T @ W @ e
# Compute possible new value of
qnew = q + dq
# And figure out the new error
enew = tr2delta(self.fkine(qnew).A, T[i].A)
# Was it a good update?
if np.linalg.norm(W @ enew) < np.linalg.norm(W @ e):
# Step is accepted
q = qnew
e = enew
Yl = Yl / 2
rejcount = 0
else:
# Step is rejected, increase the damping and retry
Yl = Yl * 2
rejcount += 1
if rejcount > rlimit:
err.append(
'ikine: rejected-step limit {0} exceeded '
'(pose {1}), final err {2}'.format(
rlimit, i, np.linalg.norm(W @ enew)))
failed.append(True)
break
# Wrap angles for revolute joints
k = (q > np.pi) & revolutes
q[k] -= 2 * np.pi
k = (q < -np.pi) & revolutes
q[k] += +2 * np.pi
nm = np.linalg.norm(W @ e)
qt[i, :] = q
tcount += iterations
if any(failed):
err.append('failed to converge: try a different '
'initial value of joint coordinates')
if trajn == 1:
qt = qt[0, :]
failed = failed[0]
return qt, failed, err
def fkine(self, q=None, from_link=None, to_link=None):
'''
Evaluates the forward kinematics of a robot based on its ETS and
joint angles q.
T = fkine(q) evaluates forward kinematics for the robot at joint
configuration q.
T = fkine() as above except uses the stored q value of the
robot object.
Trajectory operation:
Calculates fkine for each point on a trajectory of joints q where
q is (nxm) and the returning SE3 in (m)
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:return: The transformation matrix representing the pose of the
end-effector
:rtype: SE3
:notes:
- The robot's base or tool transform, if present, are incorporated
into the result.
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
'''
if from_link is None:
from_link = self.base_link
if to_link is None:
to_link = self.ee_links[0]
trajn = 1
if q is None:
q = self.q
path, n = self.get_path(from_link, to_link)
use_jindex = True
try:
q = getvector(q, self.n, 'col')
except ValueError:
try:
q = getvector(q, n, 'col')
use_jindex = False
j = 0
except ValueError:
trajn = q.shape[1]
verifymatrix(q, (self.n, trajn))
for i in range(trajn):
tr = self.base.A
for link in path:
if link.isjoint:
if use_jindex:
T = link.A(q[link.jindex, i], fast=True)
else:
T = link.A(q[j, i], fast=True)
j += 1
else:
T = link.A(fast=True)
tr = tr @ T
if i == 0:
t = SE3(tr)
else:
t.append(SE3(tr))
return t
def ikunc(self, T, q0=None, ilimit=1000):
"""
Inverse manipulator by optimization without joint limits (Robot
superclass)
q, success, err = ikunc(T) are the joint coordinates (n) corresponding
to the robot end-effector pose T which is an SE3 object or
homogenenous transform matrix (4x4), and n is the number of robot
joints. Also returns success and err which is the scalar final value
of the objective function.
q, success, err = robot.ikunc(T, q0, ilimit) as above but specify the
initial joint coordinates q0 used for the minimisation.
Trajectory operation:
In all cases if T is a vector of SE3 objects (m) or a homogeneous
transform sequence (4x4xm) then returns the joint coordinates
corresponding to each of the transforms in the sequence. q is mxn
where n is the number of robot joints. The initial estimate of q
for each time step is taken as the solution from the previous time
step.
:param T: The desired end-effector pose
:type T: SE3 or SE3 trajectory
:param ilimit: Iteration limit (default 1000)
:type ilimit: bool
:retrun q: The calculated joint values
:rtype q: float ndarray(n)
:retrun success: IK solved (True) or failed (False)
:rtype success: bool
:retrun error: Final pose error
:rtype error: float
.. note::
- Joint limits are not considered in this solution.
- Can be used for robots with arbitrary degrees of freedom.
- In the case of multiple feasible solutions, the solution
returned depends on the initial choice of q0
- Works by minimizing the error between the forward kinematics of
the joint angle solution and the end-effector frame as an
optimisation.
- The objective function (error) is described as:
sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega )
Where omega is some gain matrix, currently not modifiable.
"""
if not isinstance(T, SE3):
T = SE3(T)
trajn = len(T)
if q0 is None:
q0 = np.zeros((trajn, self.n))
verifymatrix(q0, (trajn, self.n))
qt = np.zeros((trajn, self.n))
success = []
err = []
omega = np.diag([1, 1, 1, 3 / self.reach])
def sumsqr(arr):
return np.sum(np.power(arr, 2))
for i in range(trajn):
Ti = T[i]
res = minimize(lambda q: sumsqr(
((np.linalg.inv(Ti.A) @ self.fkine(q).A) - np.eye(4)) @ omega),
q0[i, :],
options={
'gtol': 1e-6,
'maxiter': ilimit
})
qt[i, :] = res.x
success.append(res.success)
err.append(res.fun)
if trajn == 1:
return qt[0, :], success[0], err[0]
else:
return qt, success, err
def base(self, base_new):
if isinstance(base_new, sp.SE3):
self._base = base_new.A
else:
verifymatrix(base_new, (4, 4))
self._base = base_new
def _plot(robot,
block,
q,
dt,
limits=None,
vellipse=False,
fellipse=False,
jointaxes=True,
eeframe=True,
shadow=True,
name=True,
movie=None):
# Make an empty 3D figure
env = rp.backend.PyPlot()
trajn = 1
if q is None:
q = robot.q
try:
q = getvector(q, robot.n, 'col')
robot.q = q
except ValueError:
trajn = q.shape[1]
verifymatrix(q, (robot.n, trajn))
# Add the robot to the figure in readonly mode
if trajn == 1:
env.launch(robot.name + ' Plot', limits)
else:
env.launch(robot.name + ' Trajectory Plot', limits)
env.add(robot,
readonly=True,
jointaxes=jointaxes,
eeframe=eeframe,
shadow=shadow,
name=name)
if vellipse:
vell = robot.vellipse(centre='ee')
env.add(vell)
if fellipse:
fell = robot.fellipse(centre='ee')
env.add(fell)
if movie is not None:
if not _pil_exists:
raise RuntimeError(
'to save movies PIL must be installed:\npip3 install PIL')
images = [] # list of images saved from each plot
# make the background white, looks better than grey stipple
env.ax.w_xaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
env.ax.w_yaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
env.ax.w_zaxis.set_pane_color((1.0, 1.0, 1.0, 1.0))
if trajn != 1:
for i in range(trajn):
robot.q = q[:, i]
env.step()
time.sleep(dt / 1000)
if movie is not None:
# render the frame and save as a PIL image in the list
canvas = env.fig.canvas
img = PIL.Image.frombytes('RGB', canvas.get_width_height(),
canvas.tostring_rgb())
images.append(img)
if movie is not None:
# save it as an animated GIF
images[0].save(movie,
save_all=True,
append_images=images[1:],
optimize=False,
duration=dt,
loop=0)
# Keep the plot open
if block: # pragma: no cover
env.hold()
return env
def ikcon(self, T, q0=None):
"""
Inverse kinematics by optimization with joint limits (Robot superclass)
q, success, err = ikcon(T, q0) calculates the joint coordinates (1xn)
corresponding to the robot end-effector pose T which is an SE3 object
or homogenenous transform matrix (4x4), and N is the number of robot
joints. Initial joint coordinates Q0 used for the minimisation.
q, success, err = ikcon(T) as above but q0 is set to 0.
Trajectory operation:
In all cases if T is a vector of SE3 objects or a homogeneous
transform sequence (4x4xm) then returns the joint coordinates
corresponding to each of the transforms in the sequence. q is mxn
where n is the number of robot joints. The initial estimate of q
for each time step is taken as the solution from the previous time
step. Retruns trajectory of joints q (mxn), list of success (m) and
list of errors (m)
:param T: The desired end-effector pose
:type T: SE3 or SE3 trajectory
:param q0: initial joint configuration (default all zeros)
:type q0: float ndarray(n) (default all zeros)
:retrun q: The calculated joint values
:rtype q: float ndarray(n)
:retrun success: IK solved (True) or failed (False)
:rtype success: bool
:retrun error: Final pose error
:rtype error: float
.. note::
- Joint limits are considered in this solution.
- Can be used for robots with arbitrary degrees of freedom.
- In the case of multiple feasible solutions, the solution
returned depends on the initial choice of q0.
- Works by minimizing the error between the forward kinematics
of the joint angle solution and the end-effector frame as an
optimisation.
- The objective function (error) is described as:
sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega )
Where omega is some gain matrix, currently not modifiable.
"""
if not isinstance(T, SE3):
T = SE3(T)
trajn = len(T)
try:
if q0 is not None:
q0 = getvector(q0, self.n, 'row')
else:
q0 = np.zeros((trajn, self.n))
except ValueError:
verifymatrix(q0, (trajn, self.n))
# create output variables
qstar = np.zeros((trajn, self.n))
error = []
exitflag = []
omega = np.diag([1, 1, 1, 3 / self.reach])
def cost(q, T, omega):
return np.sum(((np.linalg.pinv(T.A) @ self.fkine(q).A - np.eye(4))
@ omega)**2)
bnds = Bounds(self.qlim[0, :], self.qlim[1, :])
for i in range(trajn):
Ti = T[i]
res = minimize(lambda q: cost(q, Ti, omega),
q0[i, :],
bounds=bnds,
options={'gtol': 1e-6})
qstar[i, :] = res.x
error.append(res.fun)
exitflag.append(res.success)
if trajn > 1:
return qstar, exitflag, error
else:
return qstar[0, :], exitflag[0], error[0]
