diff --git a/src/hpp/gepetto/__init__.py b/src/hpp/gepetto/__init__.py index 16ae4e5..6bdc0c9 100644 --- a/src/hpp/gepetto/__init__.py +++ b/src/hpp/gepetto/__init__.py @@ -18,5 +18,5 @@ # . from .path_player import PathPlayer # noqa: F401 -from .viewer import Color, Viewer # noqa: F401 +from .viewer import Color, Viewer, displayGripper, displayHandle # noqa: F401 from .viewer_factory import ViewerFactory # noqa: F401 diff --git a/src/hpp/gepetto/viewer.py b/src/hpp/gepetto/viewer.py index 6ca1a63..e919fe5 100644 --- a/src/hpp/gepetto/viewer.py +++ b/src/hpp/gepetto/viewer.py @@ -680,3 +680,39 @@ def drawRobotAABB(self): ) self.client.gui.setWireFrameMode(n, "WIREFRAME") self.client.gui.refresh() + + +def displayGripper(v, name): + """ + Display gripper in gepetto-gui + Retrieve the joint and pose information of the gripper in the robot model + and display a frame. + """ + robot = v.robot + joint, pose = robot.getGripperPositionInJoint(name) + gname = "gripper__" + name.replace("/", "_") + v.client.gui.addXYZaxis(gname, [0, 1, 0, 1], 0.005, 0.015) + if joint != "universe": + link = robot.getLinkNames(joint)[0] + v.client.gui.addToGroup(gname, robot.name + "/" + link) + else: + v.client.gui.addToGroup(gname, robot.name) + v.client.gui.applyConfiguration(gname, pose) + + +def displayHandle(v, name): + """ + Display handle in gepetto-gui + Retrieve the joint and pose information of the handle in the robot model + and display a frame. + """ + robot = v.robot + joint, pose = robot.getHandlePositionInJoint(name) + hname = "handle__" + name.replace("/", "_") + v.client.gui.addXYZaxis(hname, [0, 1, 0, 1], 0.005, 0.015) + if joint != "universe": + link = robot.getLinkNames(joint)[0] + v.client.gui.addToGroup(hname, robot.name + "/" + link) + else: + v.client.gui.addToGroup(hname, robot.name) + v.client.gui.applyConfiguration(hname, pose)