Add quick tutorial using fetch

quick_tutorial/CMakeLists.txt

@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(quick_tutorial)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES quick_tutorial
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/quick_tutorial.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/quick_tutorial.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_quick_tutorial.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

quick_tutorial/README.md

@@ -0,0 +1,21 @@
+### Quick Tutorial
+This package provides quick tutorials in a step-by-step manner. In [day1](day1.md), you will get to know how to kinematically simulate fetch robot inside Euslisp. In [day2](day2.md), you will be familiar with the workflow for controlling a real robot using Euslisp. In day3 (under construction), you will know how to implement simple sense-and-act type robotic manipulation.
+
+### Installation
+We assume you already installed ROS melodic.
+
+```bash
+mkdir ~/tutorial_ws/src -p
+cd ~/tutorial_ws/src
+wstool init .
+wstool merget -t . https://raw.githubusercontent.com/HiroIshida/jsk_demos/add_quick_tutorial/quick_tuorial_fetch/quick_tutorial.rosinstall
+
+rosdep install --from-paths jsk-ros-pkg/jsk_robot/jsk_fetch_robot/fetcheus -i -r -y
+rosdep install --from-paths jsk-ros-pkg/jsk_demos/jsk_maps -i -r -y
+rosdep install --from-paths jsk-ros-pkg/jsk_demos/quick_tutorial_fetch -i -r -y
+
+cd ..
+source /opt/ros/melodic/setup.bash
+catkin build fetcheus jsk_maps quick_tutorial
+source ~/tutorial_ws/devel/setup.bash
+```

quick_tutorial/day1.md

@@ -0,0 +1,120 @@
+## Day1: playing geometric robot model in Euslisp
+Before startgin this tutorial, please do `roscore` in other terminal. 
+
+### get information of joint angles of a robot
+
+```lisp
+(load "package://fetcheus/fetch-interface.l") 
+(fetch) 
+```
+The `fetch` function creates an instance named `*fetch*` which is a geometrical model of fetch-robot. Now you can view the model by calling the following function:
+```lisp
+(objects *fetch*)
+```
+<div align="center">
+<img src="https://raw.githubusercontent.com/HiroIshida/quick_tutorial/master/images/day1_1.png" alt="none" title="day1_1" width="200">
+</div>
+
+The robot model `*fetch*` contains the information of joints. The fetch robot has 10 joints, so let's look at the state of these joints.
+```lisp
+(send *fetch* :angle-vector)
+;; output: #f(20.0 75.6304 80.2141 -11.4592 98.5487 0.0 95.111 0.0 0.0 0.0)
+```
+As you can see, the state of 10 joints is shown as a float-vector. Probably you need to know which value corresponds to which joints. To this end, the following method is useful.
+```lisp
+(send *fetch* :joint-list :name)
+;; output: ("torso_lift_joint" "shoulder_pan_joint" "shoulder_lift_joint" "upperarm_roll_joint" "elbow_flex_joint" "forearm_roll_joint" "wrist_flex_joint" "wrist_roll_joint" "head_pan_joint" "head_tilt_joint")
+```
+You get the list of 10 strings of the joint name. The order of this list corresponds to the order of the float-vector you got by `:angle-vector` method. By comparing those two, for example, you can know that angle of `torso_lift_joint` is `20.0`.
+
+Now, let's set a custom angle vector to the robot model. 
+```lisp
+(setq *av-zero* (float-vector 0 0 0 0 0 0 0 0 0 0))
+(send *fetch* :angle-vector *av-zero*)
+```
+Please click the previously opened IRT-viewer, then you will see the robot model is updated (IRT-viewer is not updated without click!). 
+<div align="center">
+<img src="https://raw.githubusercontent.com/HiroIshida/quick_tutorial/master/images/day1_2.png" alt="none" title="day1_2" width="200">
+</div>
+
+Maybe, you want to set a specific joint instead of setting all the joints angles at once. For the `shoulder_pan_joint` case, for example, this can be done by:
+```lisp
+(let ((shoulder-pan-joint (send *fetch* :shoulder_pan_joint)))
+    (send shoulder-pan-joint :joint-angle 60))
+```
+(also please check [this note](#access-to-joint-in-jsk-style))
+
+### solving inverse kinematics (IK)
+Usually, in robotics, you want to guide the robot arm's end effector to a commanded pose (position and orientation). Thus, before sending an angle vector, you must know an angle vector with which the end effector will be the commanded pose. This can be done by solving inverse kinematics (IK) (if you are not familiar please google it). First, we create a coordinate (or a pose) `*co-target*` by
+```lisp
+(setq *co-target* (make-coords :pos #f(800 300 800) :rpy #f(0.0 0.1 0.1))) ;; #f(..) is a float-vector
+```
+Then the following code will solver the IK:
+```lisp
+(send *fetch* :angle-vector #f(0 0 0 0 0 0 0 0 0 0)) ;; initial solution
+(send *fetch* :rarm :inverse-kinematics *co-target* ;; fetch robot has only :rarm. PR2 had both :larm and :rarm
+        :rotation-axis t :check-collision t :use-torso nil)
+```
+<div align="center">
+<img src="https://raw.githubusercontent.com/HiroIshida/quick_tutorial/master/images/day1_4.png" alt="none" title="day1_4" width="200">
+</div>
+
+In `:inverse-kinematics`, IK is solved and the obtained angle vector is applied to ` *fetch* `. Note that you must care initial solution for IK. In the Euslisp, the angle-vector set to the robot before solving IK is the initial solution of IK. For example, `#f((0 0 0 0 0 0 0 0 0 0)` is the initial solution. In solving IK you can set some key arguments. `:rotation-axis`, `check-collision` and `use-torso` is particularly important. If `:rotation-axis` is `nil` the IK is solved ignoring orientation (rpy). If `:check-collision` is `nil` the collision between the links of the robot is not considered. Please play with changing these arguments. 
+
+<div align="center">
+<img src="https://raw.githubusercontent.com/HiroIshida/quick_tutorial/master/images/day1_6.png" alt="none" title="day1_6" width="200">
+</div>
+You can see the two coordinates (diplayed by white arrows) are equal to each other.
+
+Note that a solution of IK will be changed if initial solution is changed. (please try different initial solution and solve IK). At the worst case, IK cannot be solved (usually happens). Intuitively speaking, if the target end-effector pose is far from the initial pose, solving IK becomes difficult and likely to be failed. To get over this problem, it is effective to prepare "mid pose" in the middle of the current and targeted end effector pose. Then solve IK for "mid pose" first, and by using obtained angle-vector for "mid-pose" as an initial solution, solve IK for target pose. (TODO: need editing)
+
+### Visualization 
+It is quite helpful if geometric relation between current and target coordinate of end effector. You can get the current coordinate of the end effector by 
+```
+(setq *co-endeffector* (send *fetch* :rarm :end-coords))
+```
+By using this it is possible to make arrow object:
+```
+(require "models/arrow-object.l") ;; you need to load this 
+(setq *co-endeffector-vis* (arrow))
+(send *co-endeffector-vis* :newcoords (send *co-endeffector* :copy-worldcoords))
+```
+In the same manner let's make arrow object of target coordinate
+```
+(setq *co-target-vis* (arrow))
+(send *co-target-vis* :newcoords (make-coords :pos #f(800 300 800) :rpy #f(0.0 0.1 0.1)))
+```
+And visualize all by
+```
+(objects (list *fetch* *co-endeffector-vis* *co-target-vis*))
+```
+<div align="center">
+<img src="https://raw.githubusercontent.com/HiroIshida/quick_tutorial/master/images/vis.png" alt="none" title="vis" width="200">
+</div>
+
+
+### Other important things
+#### float vector 
+
+A pitfall for float vector is that, if you call function inside `#f( )` (e.g. `#f(0 0 (deg2rad 30))`) you will get error:
+```lisp
+error: number expected in read-float-array
+```
+To avoid this issue, you can simply call `(float-vector 0 0 (deg2rad 30))` instead.
+
+#### access to joint in jsk style 
+
+In the above tutorials, to access a joint object I used like `(send *fetch* :shoulder_pan_joint)`. The another equivalent code to do this is 
+```
+(send *fetch* :rarm :shoulder-y) ;; y indcates rotation around `yaw` 
+```
+The mapping from the jsk-style notation to official notation is given in [fetch.yaml](https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/fetcheus/fetch.yaml). Similarly, to access `head_pan_joint`, you can use following two equivalent ways:
+```
+(send *fetch* :head_pan_joint)
+(send *fetch* :head :neck-y);; jsk style
+```
+
+### TODO (need editing)
+1. add `draw-on`. (will be edited after some response in  https://github.com/euslisp/EusLisp/issues/426)
+2. `:methods`
+3. `apropos`

quick_tutorial/day2.l

@@ -0,0 +1,35 @@
+(load "package://fetcheus/fetch-interface.l")
+(require "models/arrow-object.l")
+(fetch-init)
+
+(setq *co-handle* (arrow))
+(send *co-handle* :newcoords (make-coords :pos #f(800 300 1000) :rpy #f(0.0 0.0 1.54)))
+
+(setq *co-endeffector* (arrow))
+(send *co-endeffector* :newcoords (send (send *fetch* :rarm :end-coords) :copy-worldcoords))
+(objects (list *fetch* *co-endeffector* *co-handle*))
+
+(setq *co-ik-target* (arrow))
+(send *co-ik-target* :newcoords (send *co-handle* :copy-worldcoords))
+(send *co-ik-target* :translate #f(-80 0 0) :local)
+
+;; solve ik
+(send *fetch* :rarm :inverse-kinematics *co-ik-target*
+        :rotation-axis t :check-collision t :use-torso t)
+
+;; command to target pose
+(send *ri* :angle-vector (send *fetch* :angle-vector) 1000) ;; command 1
+(send *ri* :wait-interpolation)
+
+;; move forward
+(send *fetch* :rarm :move-end-pos #f(100 0 0) :local)
+(send *ri* :angle-vector (send *fetch* :angle-vector) 1000) ;; command 2
+(send *ri* :wait-interpolation)
+
+;; grasp
+(send *ri* :start-grasp :effort 100)
+
+;; lift
+(send *fetch* :rarm :move-end-pos #f(0 0 100) :world)
+(send *ri* :angle-vector (send *fetch* :angle-vector) 1000)
+(send *ri* :wait-interpolation)