Merge pull request #102 from florent-lamiraux/devel

Add functions to display gripper and handle in gepetto-gui.

src/hpp/gepetto/__init__.py

@@ -18,5 +18,5 @@
 # <http://www.gnu.org/licenses/>.
 
 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

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)