diff --git a/doc/treeview.dox b/doc/treeview.dox index 9a87f00e5138bcad3e6e35a909b7c79a2d57d544..133e80379eda16e6d69086827aec865f2b5bf448 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 0000000000000000000000000000000000000000..d10501abc6e01e609a96ac17e3a147a1177643b7 --- /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 0000000000000000000000000000000000000000..61dfd928497f89389b407ca031a669cd3117361f --- /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 0000000000000000000000000000000000000000..f4e99d8406f788e153f36766bfde95569530ed2a --- /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) +```