[Doc] Tutorials DG / IK

doc/treeview.dox

@@ -39,6 +39,11 @@ namespace pinocchio {
        - \subpage md_doc_usage_crtp
   */
 
+  /** \page md_doc_tutorials_intro Tutorials
+       - \subpage md_doc_tutorials_direct-geometry
+       - \subpage md_doc_tutorials_inverse-kinematics
+  */
+
   //
   // Modules organization
   //

doc/tutorials/direct-geometry.md

+# Direct Geometry
+
+## Load the robot
+
+```py
+from pinocchio.robot_wrapper import RobotWrapper
+
+PKG = '/opt/openrobots/share/'
+URDF = PKG + 'ur5_description/urdf/ur5_gripper.urdf'
+
+robot = RobotWrapper(URDF, [PKG])
+```
+
+## Visualize the model
+
+launch `gepetto-gui`, and then
+
+```py
+robot.initDisplay(loadModel=True)
+```
+
+## Put the robot in a particular position
+
+```py
+import numpy as np
+q = np.matrix([-.5, -1, 1.5, -.5, -.5, 0]).T
+robot.display(q)
+```

doc/tutorials/intro.md

+# Tutorials
+
+These tutorials contains quick exemples showing how Pinocchio can be used.
+If you need more detailed exercises, go to https://github.com/stack-of-tasks/pinocchio-tutorials

doc/tutorials/inverse-kinematics.md

+# Inverse Kinematics
+
+## Get the robot
+
+```py
+import pinocchio
+
+PKG = '/opt/openrobots/share/'
+URDF = PKG + 'ur5_description/urdf/ur5_gripper.urdf'
+
+robot = pinocchio.robot_wrapper.RobotWrapper(URDF, [PKG])
+robot.initDisplay(loadModel=True)
+
+#robot.viewer.gui.addFloor('world/floor')
+
+NQ = robot.model.nq
+
+q = pinocchio.utils.rand(NQ)
+robot.display(q)
+```
+
+## Get current and desired position of end effector
+
+```py
+import numpy as np
+
+IDX_TOOL = 22
+Mtool = robot.data.oMf[IDX_TOOL]
+
+Tgoal = [.2, -.4, .7]
+Qgoal = [.4, .02, -.5, .7]
+
+Qgoal = pinocchio.Quaternion(*Qgoal)
+Qgoal.normalize()
+Mgoal = pinocchio.SE3(Qgoal.matrix(), np.matrix(Tgoal).T)
+```
+
+## Show goal
+
+```py
+robot.viewer.gui.addXYZaxis('world/goal', [1, 0, 0, 1], .015, .2)
+Vgoal = pinocchio.utils.XYZQUATToViewerConfiguration(pinocchio.utils.se3ToXYZQUAT(Mgoal))
+robot.viewer.gui.applyConfiguration('world/goal', Vgoal)
+robot.viewer.gui.refresh()
+```
+
+## Reach it
+
+```py
+import time
+from numpy.linalg import norm, pinv
+
+DT = .001
+nu = pinocchio.utils.zero(6) + 1
+
+while norm(nu) > .01:
+    pinocchio.forwardKinematics(robot.model, robot.data, q)
+    pinocchio.framesKinematics(robot.model, robot.data)
+    Mtool = robot.data.oMf[IDX_TOOL]
+    nu = pinocchio.log(Mtool.inverse() * Mgoal).vector
+    J = pinocchio.frameJacobian(robot.model, robot.data, IDX_TOOL, q)
+    vq = pinv(J) * nu
+    robot.increment(q, vq * DT)
+    robot.display(q)
+    time.sleep(DT)
+```