#!/usr/bin/env python
"""
@author: Peter Corke
@author: Jesse Haviland
"""
# 2/8/95 changed D3 to 150.05mm which is closer to data from Lee, AKB86 and
# Tarn fixed errors in COG for links 2 and 3
# 29/1/91 to agree with data from Armstrong etal. Due to their use
# of modified D&H params, some of the offsets Ai, Di are
# offset, and for links 3-5 swap Y and Z axes.
# 14/2/91 to use Paul's value of link twist (alpha) to be consistant
# with ARCL. This is the -ve of Lee's values, which means the
# zero angle position is a righty for Paul, and lefty for Lee.
# all parameters are in SI units: m, radians, kg, kg.m2, N.m, N.m.s etc.
# from math import pi
import numpy as np
from roboticstoolbox import DHRobot, PrismaticDH, models
from spatialmath import SE3
from spatialmath import base
from math import pi
[docs]class P8(DHRobot):
"""
Class that models a Puma robot on an XY base
``P8()`` is an object which models an 8-axis robot comprising a Puma 560
robot on an XY base. Joints 0 and 1 are the base, joints 2-7 are the robot
arm.
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> print(robot)
Defined joint configurations are:
- qz, zero joint angle configuration, 'L' shaped configuration
.. note::
- SI units are used.
.. codeauthor:: Peter Corke
:seealso: :func:`models.DH.Puma560`
""" # noqa
def __init__(self):
# create the base
links = [
PrismaticDH(alpha=-pi / 2, qlim=[-1, 1]),
PrismaticDH(theta=-pi / 2, alpha=pi / 2, qlim=[-1, 1]),
]
# stick the Puma on top
puma = models.DH.Puma560()
links.extend(puma.links)
super().__init__(
links, name="P8", keywords=("mobile", "redundant"), base=SE3.Ry(pi / 2)
)
self.qr = np.zeros(8)
self.qz = np.zeros(8)
self.addconfiguration("qr", self.qr)
self.addconfiguration("qz", self.qz)
if __name__ == "__main__": # pragma nocover
robot = P8()
print(robot)
# robot.plot([.5, -0.6, 0, 0, 0, 0, 0, 0], block=True)