Fix gripper in scene graph
@@ -110,18 +110,19 @@ class BaseERobot(Robot):
J. Haviland and P. Corke
""" # noqa E501
- def __init__(
- self, links, base_link=None, gripper_links=None, checkjindex=True, **kwargs
- ):
+ def __init__(self, links, gripper_links=None, checkjindex=True, **kwargs):
self._path_cache_fknm = {}
self._path_cache = {}
self._eye_fknm = eye(4)
# self._ets = []
+ self._all_links = []
self._linkdict = {}
self._n = 0
self._ee_links = []
- # self._base_link = None
+
+ for link in links:
+ self._all_links.append(link)
# Ordered links, we reorder the input elinks to be in depth first
# search order
@@ -176,6 +177,11 @@ class BaseERobot(Robot):
"Invalid link configuration provided, must have a base link"
)
+ # Scene node, set links between the links
+ for link in links:
+ if link.parent is not None:
+ link.scene_parent = link.parent
+
# Set up the gripper, make a list containing the root of all
# grippers
if gripper_links is not None:
@@ -213,7 +219,7 @@ class BaseERobot(Robot):
else:
for link in gripper_links:
# use the passed in value
- self.ee_links.append(link.parent)
+ self.ee_links.append(link.parent) # type: ignore
# assign the joint indices
if all([link.jindex is None for link in links]):
@@ -268,11 +274,6 @@ class BaseERobot(Robot):
# Initialise Robot object
super().__init__(orlinks, **kwargs)
- # Scene node, set links between the links
- for link in self.links:
- if link.parent is not None:
- link.scene_parent = link.parent
-
# SceneNode, set a reference to the first link
self.scene_children = [self.links[0]]
@@ -1290,6 +1291,7 @@ class ERobot(BaseERobot):
# --------------------------------------------------------------------- #
+ # TODO REMOVE THIS
def _reset_cache(self):
self._path_cache = {}
self._path_cache_fknm = {}
@@ -1304,7 +1306,7 @@ class ERobot(BaseERobot):
def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0):
- self._set_link_fk(self.q)
+ # self._set_link_fk(self.q)
ob = []
@@ -1332,6 +1334,9 @@ class ERobot(BaseERobot):
gi.set_alpha(collision_alpha)
ob.append(gi.to_dict())
+ # for o in ob:
+ # print(o)
+
return ob
def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0):
