#!/usr/bin/env python
from roboticstoolbox.robot.Robot import Robot
import numpy as np
[docs]class PR2(Robot):
def __init__(self):
links, name, urdf_string, urdf_filepath = self.URDF_read(
"pr2_description/robots/pr2.urdf.xacro", xacro_tld="pr2_description"
)
super().__init__(
links,
gripper_links=[links[53], links[73]],
name=name,
urdf_string=urdf_string,
urdf_filepath=urdf_filepath,
)
self.grippers[0].tool = self.link_dict["r_gripper_tool_frame"].A()
self.grippers[1].tool = self.link_dict["l_gripper_tool_frame"].A()
self.manufacturer = "Willow Garage"
self.qr = np.zeros(31)
self.qz = np.zeros(31)
self.addconfiguration("qr", self.qr)
self.addconfiguration("qz", self.qz)
self.qdlim = 2.0 * np.ones(31)
if __name__ == "__main__": # pragma nocover
r = PR2()
# i = 0
# for link in r.links:
# if link.isjoint:
# print(i, link.name)
# i += 1
# path, n, t = r.get_path(end=r.grippers[0])
# print(n)
# print(t)
# for l in path[1:]:
# if len(l.collision) > 0:
# print(l.isjoint)
# print(l.name)
# print(l.parent.name)
# print()
# for l in r.grippers[0].links:
# if len(l.collision) > 0:
# print(l.isjoint)
# print(l.name)
# print(l.parent.name)
# print()