diff --git a/.travis b/.travis index f301bf7a6..555557aaa 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit f301bf7a68c4f208a606a4ba2a4e60c8d30feec2 +Subproject commit 555557aaa06d036784da055b1ca4d04ff11d3e74 diff --git a/PR2_joints.pdf b/PR2_joints.pdf new file mode 100644 index 000000000..87e3ecf88 Binary files /dev/null and b/PR2_joints.pdf differ diff --git a/README.md b/README.md index 0becdcf0c..2336155d0 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,21 @@ -jsk_pr2eus [![Build Status](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus.png?branch=master)](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus) +# jsk_pr2eus +[![Build Status](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus.png?branch=master)](https://travis-ci.org/jsk-ros-pkg/jsk_pr2eus) -========== +[//]: # (!!DO NOT EDIT !!) -| Package | Indigo (Saucy) | Indigo (Trusty) | Jade (Trusty) | Jade (Vivid) | Kinetic (Wily) | Kinetic (Xenial) | -|:-------------------|:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| jsk_pr2eus (armhf) | [![Build Status](http://build.ros.org/job/Ibin_arm_uShf__jsk_pr2eus__ubuntu_saucy_armhf__binary/badge/icon)](http://build.ros.org/job/Ibin_arm_uShf__jsk_pr2eus__ubuntu_saucy_armhf__binary) | [![Build Status](http://build.ros.org/job/Ibin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary/badge/icon)](http://build.ros.org/job/Ibin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary) | [![Build Status](http://build.ros.org/job/Jbin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary/badge/icon)](http://build.ros.org/job/Jbin_arm_uThf__jsk_pr2eus__ubuntu_trusty_armhf__binary) | [![Build Status](http://build.ros.org/job/Jbin_arm_uVhf__jsk_pr2eus__ubuntu_vivid_armhf__binary/badge/icon)](http://build.ros.org/job/Jbin_arm_uVhf__jsk_pr2eus__ubuntu_vivid_armhf__binary) | [![Build Status](http://build.ros.org/job/Kbin_arm_uWhf__jsk_pr2eus__ubuntu_wily_armhf__binary/badge/icon)](http://build.ros.org/job/Kbin_arm_uWhf__jsk_pr2eus__ubuntu_wily_armhf__binary) | [![Build Status](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary/badge/icon)](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary) | -| jsk_pr2eus (i386) | [![Build Status](http://build.ros.org/job/Ibin_uS32__jsk_pr2eus__ubuntu_saucy_i386__binary/badge/icon)](http://build.ros.org/job/Ibin_uS32__jsk_pr2eus__ubuntu_saucy_i386__binary) | [![Build Status](http://build.ros.org/job/Ibin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary/badge/icon)](http://build.ros.org/job/Ibin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary) | [![Build Status](http://build.ros.org/job/Jbin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary/badge/icon)](http://build.ros.org/job/Jbin_uT32__jsk_pr2eus__ubuntu_trusty_i386__binary) | [![Build Status](http://build.ros.org/job/Jbin_uV32__jsk_pr2eus__ubuntu_vivid_i386__binary/badge/icon)](http://build.ros.org/job/Jbin_uV32__jsk_pr2eus__ubuntu_vivid_i386__binary) | [![Build Status](http://build.ros.org/job/Kbin_uW32__jsk_pr2eus__ubuntu_wily_i386__binary/badge/icon)](http://build.ros.org/job/Kbin_uW32__jsk_pr2eus__ubuntu_wily_i386__binary) | [![Build Status](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary/badge/icon)](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary) | -| jsk_pr2eus (amd64) | [![Build Status](http://build.ros.org/job/Ibin_uS64__jsk_pr2eus__ubuntu_saucy_amd64__binary/badge/icon)](http://build.ros.org/job/Ibin_uS64__jsk_pr2eus__ubuntu_saucy_amd64__binary) | [![Build Status](http://build.ros.org/job/Ibin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary/badge/icon)](http://build.ros.org/job/Ibin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/job/Jbin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary/badge/icon)](http://build.ros.org/job/Jbin_uT64__jsk_pr2eus__ubuntu_trusty_amd64__binary) | [![Build Status](http://build.ros.org/job/Jbin_uV64__jsk_pr2eus__ubuntu_vivid_amd64__binary/badge/icon)](http://build.ros.org/job/Jbin_uV64__jsk_pr2eus__ubuntu_vivid_amd64__binary) | [![Build Status](http://build.ros.org/job/Kbin_uW64__jsk_pr2eus__ubuntu_wily_amd64__binary/badge/icon)](http://build.ros.org/job/Kbin_uW64__jsk_pr2eus__ubuntu_wily_amd64__binary) | [![Build Status](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary/badge/icon)](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary) | +[//]: # (THIS SECTION IS AUTOMATICALLY GENERATED BY) + +[//]: # (rosrun jsk_tools generate_deb_status_table.py jsk_pr2eus) + + +| Package | Kinetic (Xenial) | Melodic (Bionic) | +|:-------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|:-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| jsk_pr2eus (arm64) | [![Build Status](http://build.ros.org/job/Kbin_uxv8_uXv8__jsk_pr2eus__ubuntu_xenial_arm64__binary/badge/icon)](http://build.ros.org/job/Kbin_uxv8_uXv8__jsk_pr2eus__ubuntu_xenial_arm64__binary) | [![Build Status](http://build.ros.org/job/Mbin_ubv8_uBv8__jsk_pr2eus__ubuntu_bionic_arm64__binary/badge/icon)](http://build.ros.org/job/Mbin_ubv8_uBv8__jsk_pr2eus__ubuntu_bionic_arm64__binary) | +| jsk_pr2eus (armhf) | [![Build Status](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary/badge/icon)](http://build.ros.org/job/Kbin_uxhf_uXhf__jsk_pr2eus__ubuntu_xenial_armhf__binary) | [![Build Status](http://build.ros.org/job/Mbin_ubhf_uBhf__jsk_pr2eus__ubuntu_bionic_armhf__binary/badge/icon)](http://build.ros.org/job/Mbin_ubhf_uBhf__jsk_pr2eus__ubuntu_bionic_armhf__binary) | +| jsk_pr2eus (i386) | [![Build Status](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary/badge/icon)](http://build.ros.org/job/Kbin_uX32__jsk_pr2eus__ubuntu_xenial_i386__binary) | --- | +| jsk_pr2eus (amd64) | [![Build Status](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary/badge/icon)](http://build.ros.org/job/Kbin_uX64__jsk_pr2eus__ubuntu_xenial_amd64__binary) | [![Build Status](http://build.ros.org/job/Mbin_uB64__jsk_pr2eus__ubuntu_bionic_amd64__binary/badge/icon)](http://build.ros.org/job/Mbin_uB64__jsk_pr2eus__ubuntu_bionic_amd64__binary) | + +[//]: # + + +[PR2_joints.pdf](./PR2_joints.pdf) shows the joint names and joint limits for PR2. \ No newline at end of file diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l index e9150416f..ad2d20291 100644 --- a/pr2eus/robot-interface.l +++ b/pr2eus/robot-interface.l @@ -377,7 +377,7 @@ - tm : (time to goal in [msec]) if designated tm is faster than fastest speed, use fastest speed if not specified, it will use 1/scale of the fastest speed . - if :fastest is specified use fastest speed calculated from max speed + if :fast is specified use fastest speed calculated from max speed - ctype : controller method name - start-time : time to start moving - scale : if tm is not specified, it will use 1/scale of the fastest speed @@ -402,7 +402,7 @@ ((numberp tm) (if (< tm fastest-tm) (setq tm fastest-tm))) - ;;Safe Mode (Speed will be 5 * fastest-time) + ;;Safe Mode (Speed will be 5 * fastest-tm) ((null tm) (setq tm (* 5 fastest-tm))) ;;Error Not Good Input @@ -434,7 +434,7 @@ if tms is atom, then use (list (make-list (length avs) :initial-element tms))) for tms if designated each tmn is faster than fastest speed, use fastest speed if tmn is nil, then it will use 1/scale of the fastest speed . - if :fastest is specified, use fastest speed calculated from max speed + if :fast is specified, use fastest speed calculated from max speed - ctype : controller method name - start-time : time to start moving - scale : if tms is not specified, it will use 1/scale of the fastest speed diff --git a/pr2eus_moveit/CMakeLists.txt b/pr2eus_moveit/CMakeLists.txt index fd9ceffb9..c4684469d 100644 --- a/pr2eus_moveit/CMakeLists.txt +++ b/pr2eus_moveit/CMakeLists.txt @@ -28,7 +28,8 @@ install(DIRECTORY euslisp tutorials if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) find_package(pr2_gazebo QUIET) - if(pr2_gazebo_FOUND) + find_package(pr2_moveit_config QUIET) + if(pr2_gazebo_FOUND AND pr2_moveit_config_FOUND) add_rostest(test/test-pr2eus-moveit.test) else() message(WARNING "pr2_gazebo is not found. Skipping pr2eus_moveit tests") diff --git a/pr2eus_moveit/euslisp/robot-moveit.l b/pr2eus_moveit/euslisp/robot-moveit.l index 39dda30f3..aaf155138 100644 --- a/pr2eus_moveit/euslisp/robot-moveit.l +++ b/pr2eus_moveit/euslisp/robot-moveit.l @@ -555,6 +555,19 @@ (av &rest args &key (ctype controller-type) (start-offset-time 0) (total-time) (move-arm :larm) (reset-total-time 5000.0) (use-send-angle-vector) (scale 1.0) &allow-other-keys) + "Plan collision-free trajectory and send it to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- av : joint angle vector (float-vector) [deg] or sequence of joint angle vector (list av0 av1 ... avn) +- ctype : controller method name +- start-offset-time : time to start moving, the same as start-time in :angle-vector +- total-time : time to execute the whole motion in [msec] + if designated total-time is shorter than the planned time, use the planned time + if not specified, use the planned time + if :fast is specified, use 'scale' times of the planned time +- move-arm : arm symbol (e.g., :larm, :rarm, :arms) you want to move +- reset-total-time : if planned time is too short, planned trajectory is scaled so that this value becomes its execution time +- use-send-angle-vector : whether to use :angle-vector in robot-interface for sending trajectory +- scale : if :fast is specified as total-time, it will use 'scale' times of the planned time +" (let (traj ret orig-total-time sent-controllers other-joints) (setq ctype (or ctype controller-type)) (unless (gethash ctype controller-table) diff --git a/pr2eus_tutorials/README.md b/pr2eus_tutorials/README.md index f21b2e8d4..196c8c70d 100644 --- a/pr2eus_tutorials/README.md +++ b/pr2eus_tutorials/README.md @@ -10,145 +10,175 @@ If you use ROS `indigo` distribution, please replace the word `kinetic` with `in ### Using pre-built package -1. Follow the [instruction of ROS installation](http://wiki.ros.org/kinetic/Installation/Ubuntu) -2. Install the package +#### Install ROS - ```bash - sudo apt install ros-kinetic-pr2eus-tutorials - ``` +Follow the [instruction of ROS installation](http://wiki.ros.org/kinetic/Installation/Ubuntu) -3. Load ROS Environment +#### Install the package - ```bash - source /opt/ros/kinetic/setup.bash` - ``` +```bash +sudo apt install ros-kinetic-pr2eus-tutorials +``` + +#### Load ROS Environment + +```bash +source /opt/ros/kinetic/setup.bash +``` ### Using source package -1. Follow the [instruction of ROS installation](http://wiki.ros.org/kinetic/Installation/Ubuntu) -2. Setup catkin workspace +#### Install ROS + +Follow the [instruction of ROS installation](http://wiki.ros.org/kinetic/Installation/Ubuntu) + +#### Setup catkin workspace + +```bash +source /opt/ros/kinetic/setup.bash +sudo apt install python-catkin-tools python-wstool python-rosdep git +sudo rosdep init +rosdep update +# Create catkin workspace and download source repository +mkdir -p ~/ros/kinetic/src && cd ~/ros/kinetic/src +wstool init +wstool set jsk-ros-pkg/jsk_pr2eus --git https://github.com/jsk-ros-pkg/jsk_pr2eus.git -v master +wstool update +# Install dependencies for building the package +rosdep install --from-paths . -i -r -n -y +# Build the package +cd ~/ros/kinetic +catkin init +catkin build +``` + +#### Load ROS Environment + +```bash +source ~/ros/kinetic/devel/setup.bash +``` + +## Tabletop Object Grasping Demo + +### Tabletop Object Grasping Demo with PR2 + +#### Startup nodes - ```bash - source /opt/ros/kinetic/setup.bash - sudo apt install python-catkin-tools python-wstool python-rosdep git - sudo rosdep init - rosdep update - # Create catkin workspace and download source repository - mkdir -p ~/ros/kinetic/src && cd ~/ros/kinetic/src - wstool init - wstool set jsk-ros-pkg/jsk_pr2eus --git https://github.com/jsk-ros-pkg/jsk_pr2eus.git -v master - wstool update - # Install dependencies for building the package - rosdep install --from-paths . -i -r -n -y - # Build the package - cd ~/ros/kinetic - catkin init - catkin build pr2eus_tutorials - ``` +First we need to start nodes used for this demo. -3. Load ROS Environment +##### Using a real robot - ```bash - source ~/ros/kinetic/devel/setup.bash` - ``` +```bash +# on PR2 real robot +ssh +roslaunch pr2eus_tutorials pr2_tabletop.launch +``` -## Demos +You can locate a desk in front of the robot and put any objects on it. -### PR2 Tabletop Object Detection +##### Using a simulator -1. Startup nodes +You can set physics engine with roslaunch argument. - First we need to start nodes used for this demo. +```bash +# on local machine +# It may take time to download materials for the first time +roslaunch pr2eus_tutorials pr2_tabletop_sim.launch physics:=dart +``` - - Using a real robot +You can see the robot is spawned in a scene with a desk and some objects. - ```bash - # on PR2 real robot - roslaunch pr2eus_tutorials pr2_tabletop.launch - ``` +#### Run demo - You can locate a desk in front of the robot and put any objects on it. +Then we can now start the demo program for picking objects. - - Using a simulator - - you can set physics engine with roslaunch argument. +##### Start Euslisp program - ```bash - # on local machine - roslaunch pr2eus_tutorials pr2_tabletop_sim.launch physics:=dart - # It may take time to download materials for the first time - ``` +```bash +# on local machine +rosrun pr2eus_tutorials pr2-tabletop-object-grasp.l +``` - You can see the robot is spawned in a scene with a desk and some objects. +##### Start Rviz -2. Run demo +After running the demo program above, you can see object bounding boxes in the `RViZ` window. +It means the robot now recognizes each objects as individual objects from camera sensor inputs. - Then we can now start the demo program for picking objects. +```bash +# on local machine +rviz -d $(rospack find pr2eus_tutorials)/config/pr2_tabletop.rviz +``` - ```bash - rosrun pr2eus_tutorials pr2-tabletop-object-grasp.l - ``` +##### Additional setup for Kinetic local machine with a real robot - After running the demo program above, you can see object bounding boxes in the `RViZ` window. - It means the robot now recognizes each objects as individual objects from camera sensor inputs. +If you want to know why we need these node, please see [here](https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/387#issuecomment-470505882). +You also need to switch `Tabletop Object` rviz panel topic from `/bounding_box_interactive_marker/update` to `/bounding_box_interactive_marker/kinetic/update` . - You can click any object that you want the robot to pick up. +```bash +# on local machine +rosrun jsk_robot_utils marker_msg_from_indigo_to_kinetic.py +rosrun topic_tools relay /bounding_box_interactive_marker/kinetic/feedback /bounding_box_interactive_marker/feedback +``` +##### Click object bouding on Rviz - ![pr2_tabletop_sim](https://gist.githubusercontent.com/furushchev/b3f3bb08953407966f80f4b0ac70c7dd/raw/pr2_tabletop_screen.png) +You can click any object that you want the robot to pick up. -3. Step-by-step description of the demo program - In the bottom of the demo program `pr2-tabletop-object-grasp.l`, you can see a main function `demo`. +![pr2_tabletop_sim](https://gist.githubusercontent.com/furushchev/b3f3bb08953407966f80f4b0ac70c7dd/raw/pr2_tabletop_screen.png) - ```lisp - (defun demo () - (setq *grasping-object-p* nil) - (setq *arm* :rarm) - (setq *tfl* (instance ros::transform-listener :init)) - (setq *tfb* (instance ros::transform-broadcaster :init)) - (pr2-init) - (pr2-pregrasp-pose) - (wait-for-grasp-target)) - ``` +### Step-by-step Description of Demo program - The `(pr2-init)` method is just a initialization function for pr2 robot that instantiate two objects required for robot manipulation from euslisp: +In the bottom of the demo program `pr2-tabletop-object-grasp.l`, you can see a main function `demo`. - - `*pr2*`: This object is a kinematic model for a PR2 robot. This object includes any fundamental functions for robot modeling such as inverse kinematics, dynamics, geometric constraints and so on. You can visualize this model by evaluating `(objects (list *pr2*))`. - - `*ri*`: This is an object that send a control signal to the actual robot from euslisp kinematics model and receive the result or actual states of the robot. `ri` is an abbreviation of `robot interface`. +```lisp +(defun demo () + (setq *grasping-object-p* nil) + (setq *arm* :rarm) + (setq *tfl* (instance ros::transform-listener :init)) + (setq *tfb* (instance ros::transform-broadcaster :init)) + (pr2-init) + (pr2-pregrasp-pose) + (wait-for-grasp-target)) +``` - Please note that `(pr2-init)` function is defined in `pr2-interface.l` in the `pr2eus` package. - In this demo program, the function is loaded in the top of the script: - - ```lisp - (require :pr2-interface "package://pr2eus/pr2-interface.l") - ``` +The `(pr2-init)` method is just a initialization function for pr2 robot that instantiate two objects required for robot manipulation from euslisp: - After `(pr2-init)` is executed, the kinematic model looks like below: +- `*pr2*`: This object is a kinematic model for a PR2 robot. This object includes any fundamental functions for robot modeling such as inverse kinematics, dynamics, geometric constraints and so on. You can visualize this model by evaluating `(objects (list *pr2*))`. +- `*ri*`: This is an object that send a control signal to the actual robot from euslisp kinematics model and receive the result or actual states of the robot. `ri` is an abbreviation of `robot interface`. - ![pr2-reset-pose](https://user-images.githubusercontent.com/1901008/39504750-d44efa06-4e08-11e8-8aef-7c0f3ce0802b.png) +Please note that `(pr2-init)` function is defined in `pr2-interface.l` in the `pr2eus` package. +In this demo program, the function is loaded in the top of the script: +```lisp +(require :pr2-interface "package://pr2eus/pr2-interface.l") +``` + +After `(pr2-init)` is executed, the kinematic model looks like below: + +![pr2-reset-pose](https://user-images.githubusercontent.com/1901008/39504750-d44efa06-4e08-11e8-8aef-7c0f3ce0802b.png) - After initialized the robot in euslisp, `(pr2-pregrasp-pose)` method is executed. - ```lisp - (defun pr2-pregrasp-pose () - (send *pr2* :reset-manip-pose) - (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) - (send *ri* :wait-interpolation)) - ``` +After initialized the robot in euslisp, `(pr2-pregrasp-pose)` method is executed. + +```lisp +(defun pr2-pregrasp-pose () + (send *pr2* :reset-manip-pose) + (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) + (send *ri* :wait-interpolation)) +``` - The first line `(send *pr2* :reset-manip-pose)` changes the pose of euslisp kinematic model to the predefined pose called `reset-manip-pose`. - With calling this method, the actual robot does **NOT** move because this method only changes the states of the kinematic model. Instead you can see the current states of the kinematic model by `(objects (list *pr2*))`. - The kinematic model now looks like below: +The first line `(send *pr2* :reset-manip-pose)` changes the pose of euslisp kinematic model to the predefined pose called `reset-manip-pose`. +With calling this method, the actual robot does **NOT** move because this method only changes the states of the kinematic model. Instead you can see the current states of the kinematic model by `(objects (list *pr2*))`. +The kinematic model now looks like below: - ![pr2-reset-manip-pose](https://user-images.githubusercontent.com/1901008/39504749-d42a4ff8-4e08-11e8-8597-6ca54b5a97e7.png) +![pr2-reset-manip-pose](https://user-images.githubusercontent.com/1901008/39504749-d42a4ff8-4e08-11e8-8597-6ca54b5a97e7.png) - The second line then send the current state of kinematic model to the actual robot. - The second argument `5000` allows the robot to take 5000 milliseconds to move to the pose. If the second argument is omitted, the default argument `3000` will be used. +The second line then send the current state of kinematic model to the actual robot. +The second argument `5000` allows the robot to take 5000 milliseconds to move to the pose. If the second argument is omitted, the default argument `3000` will be used. - After the second line, it will take 5 seconds until the robot ends to move, but the method call itself returns immediately. - The last method called in the last line is just for waiting for the robot until he ends to move to the specified pose. +After the second line, it will take 5 seconds until the robot ends to move, but the method call itself returns immediately. +The last method called in the last line is just for waiting for the robot until he ends to move to the specified pose. ## Reach Object Demo