Fixed Mico Gripper
#!/usr/bin/env python
import numpy as np
+from roboticstoolbox.robot.ELink import ELink
from roboticstoolbox.robot.ERobot import ERobot
@@ -33,18 +34,30 @@ class Mico(ERobot):
"kinova_description/urdf/j2n4s300_standalone.xacro"
)
+ gripper_base = links[6]
+
+ # Find the finger links
+ gripper_links = [link for link in links if link.parent == gripper_base]
+
+ # New intermediate link
+ gripper = ELink(name="gripper", parent=gripper_base)
+ links.append(gripper)
+
+ # Set the finger link parent to be the new gripper base link
+ for g_link in gripper_links:
+ g_link._parent = gripper
+
super().__init__(
links,
name=name,
manufacturer="Kinova",
+ gripper_links=[gripper],
urdf_string=urdf_string,
urdf_filepath=urdf_filepath,
)
- self.addconfiguration("qz", np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0]))
- self.addconfiguration(
- "qr", np.array([0, 45, 60, 0, 0, 0, 0, 0, 0, 0]) * np.pi / 180
- )
+ self.addconfiguration("qz", np.array([0, 0, 0, 0]))
+ self.addconfiguration("qr", np.array([0, 45, 60, 0]) * np.pi / 180)
if __name__ == "__main__": # pragma nocover
