From 2d0e73844847ee414710a5f2190d250f6fae996f Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Wed, 11 Apr 2018 18:35:50 +0200
Subject: [PATCH] [Doc] Tutorials DG / IK
---
doc/treeview.dox | 5 +++
doc/tutorials/direct-geometry.md | 28 ++++++++++++
doc/tutorials/intro.md | 4 ++
doc/tutorials/inverse-kinematics.md | 66 +++++++++++++++++++++++++++++
4 files changed, 103 insertions(+)
create mode 100644 doc/tutorials/direct-geometry.md
create mode 100644 doc/tutorials/intro.md
create mode 100644 doc/tutorials/inverse-kinematics.md
diff --git a/doc/treeview.dox b/doc/treeview.dox
index 9a87f00e5..133e80379 100644
--- a/doc/treeview.dox
+++ b/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
//
diff --git a/doc/tutorials/direct-geometry.md b/doc/tutorials/direct-geometry.md
new file mode 100644
index 000000000..d10501abc
--- /dev/null
+++ b/doc/tutorials/direct-geometry.md
@@ -0,0 +1,28 @@
+# 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)
+```
diff --git a/doc/tutorials/intro.md b/doc/tutorials/intro.md
new file mode 100644
index 000000000..61dfd9284
--- /dev/null
+++ b/doc/tutorials/intro.md
@@ -0,0 +1,4 @@
+# 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
diff --git a/doc/tutorials/inverse-kinematics.md b/doc/tutorials/inverse-kinematics.md
new file mode 100644
index 000000000..f4e99d840
--- /dev/null
+++ b/doc/tutorials/inverse-kinematics.md
@@ -0,0 +1,66 @@
+# 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)
+```
--
GitLab