diff --git a/quick_tutorial/CMakeLists.txt b/quick_tutorial/CMakeLists.txt new file mode 100644 index 0000000000..7493341e5d --- /dev/null +++ b/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) diff --git a/quick_tutorial/README.md b/quick_tutorial/README.md new file mode 100644 index 0000000000..82c55f3e6c --- /dev/null +++ b/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 +``` diff --git a/quick_tutorial/day1.md b/quick_tutorial/day1.md new file mode 100644 index 0000000000..238753fd62 --- /dev/null +++ b/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*) +``` +
+none +
+ +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!). +
+none +
+ +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) +``` +
+none +
+ +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. + +
+none +
+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*)) +``` +
+none +
+ + +### 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` diff --git a/quick_tutorial/day2.l b/quick_tutorial/day2.l new file mode 100644 index 0000000000..d964fe5ba8 --- /dev/null +++ b/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) diff --git a/quick_tutorial/day2.md b/quick_tutorial/day2.md new file mode 100644 index 0000000000..d53d133de4 --- /dev/null +++ b/quick_tutorial/day2.md @@ -0,0 +1,92 @@ +## Day2: workflow of real robot (simulated) control + +### Preparation +Before starting this tutorial, in another terminal, please run +```lisp +roslaunch quick_tutorial common.launch +``` + +### Robot control workflow +As a preparation load some libraries and call `fetch-init` function. +```lisp +(load "package://fetcheus/fetch-interface.l") +(require "models/arrow-object.l") +(fetch-init) +``` +Different from `fetch` function that we called in the [previous tutorial](https://raw.githubusercontent.com/HiroIshida/quick_tutorial/master/day2.md), `fetch-init` create robot interface object `*ri*` as well as `*fetch*`. Through *ri*, we can control a real (or simulated) robot and obtain a state of the real robot. Note that joint angles of `*fetch*` are set such that it's the same as the real one. Please call `(objects *fetch*)` and compare the posture of `*fetch*` with the real robot. + +Suppose you want to grasp the handle of a kettle (see gazebo), and *somehow* you know the coordinate of the handle `*co-handle*`(I said *somehow*, but you will get an idea how to do this is next tutorial(TODO)) +. You probably want to guide the end-effector to `*co-handle*`. + +```lisp +(setq *co-handle* (arrow)) +(send *co-handle* :newcoords (make-coords :pos #f(800 300 1000) :rpy #f(0.0 0.0 1.54))) +``` + +Now let's visualize the `*fetch*`, `*co-handle*` and coordinate of the end-effector all together: +```lisp +(setq *co-endeffector* (arrow)) +(send *co-endeffector* :newcoords (send (send *fetch* :rarm :end-coords) :copy-worldcoords)) +(objects (list *fetch* *co-endeffector* *co-handle*)) +``` + +Rather than directory guide the end-effector to `*co-handle*`, let's guide to a coordinate `*co-ik-target*` which is slightly behind the target: +```lisp +(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) +``` +Because we set `:local`, the translation is done w.r.t. `*co-ik-target*` itself. Note that you can also set `:world`. We don't do it here but you can also rotate the coordinate around a specified axis (x, y, z) by a specified angle. For example, rotation around y-axis by 0.1 rad is `(send *co* :rotate 0.1 :y :local)`. + +By calling `(objects (list *fetch* *co-endeffector* *co-handle* *co-ik-target*))` you will see the following figure. +
+none +
+ + +Now we solve the IK, +```lisp +(send *fetch* :rarm :inverse-kinematics *co-ik-target* + :rotation-axis t :check-collision t :use-torso t) +``` +and send it to the real robot! +```lisp +(send *ri* :angle-vector (send *fetch* :angle-vector) 1000) ;; command 1 +(send *ri* :wait-interpolation) +``` +Here, by specifying `1000`, it takes 1000 msec for the real robot to follow the commanded angle-vecttor. Note that you cannot specify smaller value than 1000. + +Then move the end-effector forward by +```lisp +(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) +``` +In the `:move-end-pos` method, IK is solved. Similar to `:translate` method of coordinate you, can also specify `:world` instead of `:local`. Also, rotation of end-effector is possible by `(send *fetch* :rarm :move-end-rot 20 :x :local)`. Note that, different from `:rot` method, unit of rotation is [deg] for `:move-end-rot`. Here, `:wait-interpolation` is important. Without calling this, commanded angle-vector is overwritten. For example, if you send commands 1 and 2 above without `:wait-interpolation` it is almost the same as sending only `command2`. (explain why I said *almost* :TODO). + +Grasp the handle with effort of 100: +```lisp +(send *ri* :start-grasp :effort 100) +``` +Lift it: +```lisp +(send *fetch* :rarm :move-end-pos #f(0 0 100) :world) +(send *ri* :angle-vector (send *fetch* :angle-vector) 1000) +(send *ri* :wait-interpolation) +``` +As you probably noticed, the basic workflow is a repetition of 1) solving IK in the Euslisp side (and check the geometry) and then 2) command it to the real robot by `:angle-vector` method. + +The tutorial above is summarized in `day2.l`. If you run it, you can observe robot motion as the following animation. +
+none +
+ +### Other important things +1 you can obtain real (simulated) robot' angle-vector by `(send *ri* :state :potentio-vector)`. +2 Suppose you want to send two angle-vector's, say `*av1*` and `*av2*`. If you use `:wait-interpolation`, the robot will completely stop when the robot achieves `*av1*`, and then start following `*av2*`. This is too slow. If it's annoying, you can use `unix:usleep` function. For example: +```lisp +(send *ri* :angle-vector `*av1*` 1000) +(unix:usleep 500000) ;; unit is micro-second +(send *ri* :angle-vector `*av2*` 1000) +``` +By this, although it takes 1000 ms to follow `*av1*`, the goal is overwritten by `*av2*` just after `500000`. (TODO: add some illustration) diff --git a/quick_tutorial/day3.md b/quick_tutorial/day3.md new file mode 100644 index 0000000000..4bbbf30efa --- /dev/null +++ b/quick_tutorial/day3.md @@ -0,0 +1 @@ +# day3: recognize an object on the table and grasp it diff --git a/quick_tutorial/images/arrow.png b/quick_tutorial/images/arrow.png new file mode 100644 index 0000000000..6f84dacc81 Binary files /dev/null and b/quick_tutorial/images/arrow.png differ diff --git a/quick_tutorial/images/day1_1.png b/quick_tutorial/images/day1_1.png new file mode 100644 index 0000000000..633495ae0d Binary files /dev/null and b/quick_tutorial/images/day1_1.png differ diff --git a/quick_tutorial/images/day1_2.png b/quick_tutorial/images/day1_2.png new file mode 100644 index 0000000000..900f41c315 Binary files /dev/null and b/quick_tutorial/images/day1_2.png differ diff --git a/quick_tutorial/images/day1_3.png b/quick_tutorial/images/day1_3.png new file mode 100644 index 0000000000..e9afe54907 Binary files /dev/null and b/quick_tutorial/images/day1_3.png differ diff --git a/quick_tutorial/images/day1_4.png b/quick_tutorial/images/day1_4.png new file mode 100644 index 0000000000..af4699bbb9 Binary files /dev/null and b/quick_tutorial/images/day1_4.png differ diff --git a/quick_tutorial/images/day1_5.png b/quick_tutorial/images/day1_5.png new file mode 100644 index 0000000000..1a0a992d79 Binary files /dev/null and b/quick_tutorial/images/day1_5.png differ diff --git a/quick_tutorial/images/day1_6.png b/quick_tutorial/images/day1_6.png new file mode 100644 index 0000000000..f6ba96c6eb Binary files /dev/null and b/quick_tutorial/images/day1_6.png differ diff --git a/quick_tutorial/images/day2_1.png b/quick_tutorial/images/day2_1.png new file mode 100644 index 0000000000..78a7b43f5a Binary files /dev/null and b/quick_tutorial/images/day2_1.png differ diff --git a/quick_tutorial/images/day2_whole.gif b/quick_tutorial/images/day2_whole.gif new file mode 100644 index 0000000000..7cd0ddcbd9 Binary files /dev/null and b/quick_tutorial/images/day2_whole.gif differ diff --git a/quick_tutorial/images/vis.png b/quick_tutorial/images/vis.png new file mode 100644 index 0000000000..90d59ff7fb Binary files /dev/null and b/quick_tutorial/images/vis.png differ diff --git a/quick_tutorial/launch/common.launch b/quick_tutorial/launch/common.launch new file mode 100644 index 0000000000..57742130ad --- /dev/null +++ b/quick_tutorial/launch/common.launch @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/quick_tutorial/launch/detect_kettle.launch b/quick_tutorial/launch/detect_kettle.launch new file mode 100644 index 0000000000..9231b2b451 --- /dev/null +++ b/quick_tutorial/launch/detect_kettle.launch @@ -0,0 +1,84 @@ + + + + + + + + use_multiple_attention: false + dimension_x: 1.8 + dimension_y: 1.5 + dimension_z: 0.8 + initial_pos: + - 1.0 + - 0.0 + - 1.0 + + + + + + + + keep_organized: true + + + + + + + + max_curvature: 0.005 + estimate_normal: true + min_size: 10000 + + + + + + + + + + + diff --git a/quick_tutorial/launch/include/downsample.launch b/quick_tutorial/launch/include/downsample.launch new file mode 100644 index 0000000000..1c667731bb --- /dev/null +++ b/quick_tutorial/launch/include/downsample.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + filter_field_name: z + filter_limit_min: 0.01 + filter_limit_max: 1.5 + filter_limit_negative: False + leaf_size: 0.01 + + + + + + + diff --git a/quick_tutorial/launch/include/fetch.launch.xml b/quick_tutorial/launch/include/fetch.launch.xml new file mode 100644 index 0000000000..1c5d1b6f7c --- /dev/null +++ b/quick_tutorial/launch/include/fetch.launch.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/quick_tutorial/launch/include/prepare_simulated_robot.py b/quick_tutorial/launch/include/prepare_simulated_robot.py new file mode 100755 index 0000000000..c400b4de9d --- /dev/null +++ b/quick_tutorial/launch/include/prepare_simulated_robot.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python + +# Copyright (c) 2015, Fetch Robotics Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# * Neither the name of the Fetch Robotics Inc. nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL FETCH ROBOTICS INC. BE LIABLE FOR ANY +# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +# THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +# Author: Michael Ferguson + +import sys + +import rospy +import actionlib +from control_msgs.msg import (FollowJointTrajectoryAction, + FollowJointTrajectoryGoal, + GripperCommandAction, + GripperCommandGoal) +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + +arm_joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", + "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] +arm_joint_positions = [0.9, 0, 0.20, -1.40, 0, 0, 0] + +head_joint_names = ["head_pan_joint", "head_tilt_joint"] +head_joint_positions = [0.0, 0.9] + +torso_joint_names = ["torso_lift_joint"] +torso_joint_positions = [0.38] + +if __name__ == "__main__": + rospy.init_node("prepare_simulated_robot") + + # Check robot serial number, we never want to run this on a real robot! + if rospy.get_param("robot/serial") != "ABCDEFGHIJKLMNOPQRSTUVWX": + rospy.logerr("This script should not be run on a real robot") + sys.exit(-1) + + rospy.loginfo("Waiting for head_controller...") + head_client = actionlib.SimpleActionClient("head_controller/follow_joint_trajectory", FollowJointTrajectoryAction) + head_client.wait_for_server() + rospy.loginfo("...connected.") + + rospy.loginfo("Waiting for torso_controller...") + torso_client = actionlib.SimpleActionClient("torso_controller/follow_joint_trajectory", FollowJointTrajectoryAction) + torso_client.wait_for_server() + rospy.loginfo("...connected.") + + rospy.loginfo("Waiting for arm_controller...") + arm_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) + arm_client.wait_for_server() + rospy.loginfo("...connected.") + + rospy.loginfo("Waiting for gripper_controller...") + gripper_client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) + gripper_client.wait_for_server() + rospy.loginfo("...connected.") + + + trajectory = JointTrajectory() + trajectory.joint_names = head_joint_names + trajectory.points.append(JointTrajectoryPoint()) + trajectory.points[0].positions = head_joint_positions + trajectory.points[0].velocities = [0.0] * len(head_joint_positions) + trajectory.points[0].accelerations = [0.0] * len(head_joint_positions) + trajectory.points[0].time_from_start = rospy.Duration(5.0) + + head_goal = FollowJointTrajectoryGoal() + head_goal.trajectory = trajectory + head_goal.goal_time_tolerance = rospy.Duration(0.0) + + trajectory = JointTrajectory() + trajectory.joint_names = torso_joint_names + trajectory.points.append(JointTrajectoryPoint()) + trajectory.points[0].positions = torso_joint_positions + trajectory.points[0].velocities = [0.0] * len(torso_joint_positions) + trajectory.points[0].accelerations = [0.0] * len(torso_joint_positions) + trajectory.points[0].time_from_start = rospy.Duration(5.0) + + torso_goal = FollowJointTrajectoryGoal() + torso_goal.trajectory = trajectory + torso_goal.goal_time_tolerance = rospy.Duration(0.0) + + trajectory = JointTrajectory() + trajectory.joint_names = arm_joint_names + trajectory.points.append(JointTrajectoryPoint()) + trajectory.points[0].positions = arm_joint_positions + trajectory.points[0].velocities = [0.0] * len(arm_joint_positions) + trajectory.points[0].accelerations = [0.0] * len(arm_joint_positions) + trajectory.points[0].time_from_start = rospy.Duration(3.0) + + arm_goal = FollowJointTrajectoryGoal() + arm_goal.trajectory = trajectory + arm_goal.goal_time_tolerance = rospy.Duration(0.0) + + gripper_goal = GripperCommandGoal() + gripper_goal.command.max_effort = 10.0 + gripper_goal.command.position = 0.1 + + rospy.loginfo("Setting positions...") + head_client.send_goal(head_goal) + torso_client.send_goal(torso_goal) + arm_client.send_goal(arm_goal) + gripper_client.send_goal(gripper_goal) + gripper_client.wait_for_result(rospy.Duration(5.0)) + arm_client.wait_for_result(rospy.Duration(6.0)) + head_client.wait_for_result(rospy.Duration(6.0)) + torso_client.wait_for_result(rospy.Duration(6.0)) + rospy.loginfo("...done") + diff --git a/quick_tutorial/launch/tmp.launch b/quick_tutorial/launch/tmp.launch new file mode 100644 index 0000000000..369992cf08 --- /dev/null +++ b/quick_tutorial/launch/tmp.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/quick_tutorial/package.xml b/quick_tutorial/package.xml new file mode 100644 index 0000000000..cdfdd52ac0 --- /dev/null +++ b/quick_tutorial/package.xml @@ -0,0 +1,27 @@ + + + quick_tutorial + 0.0.0 + The quick_tutorial package + + hiro + + TODO + catkin + + + fetcheus + eusurdf + fetch_gazebo_demo + + + robot_state_pubclisher + fetch_gazebo + actionlib + fetch_gazebo + fetch_moveit_config + fetch_navigation + moveit_commander + moveit_python + + diff --git a/quick_tutorial/quick_tutorial.rosinstall b/quick_tutorial/quick_tutorial.rosinstall new file mode 100644 index 0000000000..77d3ad8fce --- /dev/null +++ b/quick_tutorial/quick_tutorial.rosinstall @@ -0,0 +1,14 @@ +# This package is included in jsk_demos. +# also, jsk_map in jsk_demos is also required +- git: + local-name: jsk-ros-pkg/jsk_demos + uri: https://github.com/Hiroishida/jsk_demos.git + version: add_quick_tutorial + +# requires fetcheus to use fetch robot from Euslisp +- git: + local-name: jsk-ros-pkg/jsk_robot + uri: https://github.com/jsk-ros-pkg/jsk_robot.git + version: master + + diff --git a/quick_tutorial/urdf/blue_can.urdf b/quick_tutorial/urdf/blue_can.urdf new file mode 100644 index 0000000000..2f3e1f0673 --- /dev/null +++ b/quick_tutorial/urdf/blue_can.urdf @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + + + diff --git a/quick_tutorial/urdf/kettle.gazebo b/quick_tutorial/urdf/kettle.gazebo new file mode 100644 index 0000000000..b3a9efe070 --- /dev/null +++ b/quick_tutorial/urdf/kettle.gazebo @@ -0,0 +1,19 @@ + + + + Gazebo/Red + + + + Gazebo/Black + + + + Gazebo/Black + + + + Gazebo/Black + + + diff --git a/quick_tutorial/urdf/kettle.xacro b/quick_tutorial/urdf/kettle.xacro new file mode 100644 index 0000000000..3b04c52304 --- /dev/null +++ b/quick_tutorial/urdf/kettle.xacro @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/quick_tutorial/world/tabletop.world b/quick_tutorial/world/tabletop.world new file mode 100644 index 0000000000..84cc7903f6 --- /dev/null +++ b/quick_tutorial/world/tabletop.world @@ -0,0 +1,41 @@ + + + + + + + 0 0 -9.81 + + + quick + 50 + 1.4 + + + 0.0 + 0.2 + 100.0 + 0.0 + + + 1000.0 + 0.001 + + + + + model://sun + + + model://ground_plane + + + + + room73b2-karimoku-table-0 + model://room73b2-karimoku-table_static + 0.9 0.0 0.0 0.0 0.0 0.0 + + + +